From e90a9aa2bfc40d1b10e7cf52e54a5fe5d5c155b5 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Thu, 20 Apr 2017 16:10:12 +0200 Subject: [PATCH] Migration of the internal LS PVT solver to RTKLIB solver in progress. First working version for GPS L1. Removing SBAS duplicated code. --- conf/gnss-sdr_Hybrid_byte_sim.conf | 4 +- src/algorithms/PVT/adapters/CMakeLists.txt | 2 + src/algorithms/PVT/adapters/hybrid_pvt.cc | 2 +- src/algorithms/PVT/adapters/hybrid_pvt.h | 2 +- src/algorithms/PVT/adapters/rtklib_pvt.cc | 235 ++ src/algorithms/PVT/adapters/rtklib_pvt.h | 95 + .../PVT/gnuradio_blocks/CMakeLists.txt | 2 + .../PVT/gnuradio_blocks/hybrid_pvt_cc.cc | 1 - .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 1104 +++++ .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 172 + src/algorithms/PVT/libs/CMakeLists.txt | 18 +- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 155 - src/algorithms/PVT/libs/hybrid_ls_pvt.h | 95 +- src/algorithms/PVT/libs/rinex_printer.cc | 162 +- src/algorithms/PVT/libs/rinex_printer.h | 3 +- src/algorithms/PVT/libs/rtklib_solver.cc | 291 ++ src/algorithms/PVT/libs/rtklib_solver.h | 104 + src/algorithms/libs/CMakeLists.txt | 1 + src/algorithms/libs/rtklib/CMakeLists.txt | 60 + src/algorithms/libs/rtklib/rtklib.h | 1044 +++++ .../libs/rtklib/rtklib_conversions.cc | 210 + .../libs/rtklib/rtklib_conversions.h | 65 + .../libs/rtklib/rtklib_ephemeris.cc | 797 ++++ src/algorithms/libs/rtklib/rtklib_ephemeris.h | 125 + src/algorithms/libs/rtklib/rtklib_ionex.cc | 516 +++ src/algorithms/libs/rtklib/rtklib_ionex.h | 95 + src/algorithms/libs/rtklib/rtklib_pntpos.cc | 704 ++++ src/algorithms/libs/rtklib/rtklib_pntpos.h | 180 + src/algorithms/libs/rtklib/rtklib_preceph.cc | 788 ++++ src/algorithms/libs/rtklib/rtklib_preceph.h | 136 + src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 3718 +++++++++++++++++ src/algorithms/libs/rtklib/rtklib_rtkcmn.h | 363 ++ src/algorithms/libs/rtklib/rtklib_sbas.cc | 910 ++++ src/algorithms/libs/rtklib/rtklib_sbas.h | 187 + .../adapters/sbas_l1_telemetry_decoder.cc | 3 - .../sbas_l1_telemetry_decoder_cc.cc | 16 +- .../sbas_l1_telemetry_decoder_cc.h | 3 - src/core/receiver/CMakeLists.txt | 1 + src/core/receiver/gnss_block_factory.cc | 7 + src/core/system_parameters/CMakeLists.txt | 9 +- .../system_parameters/gps_cnav_ephemeris.h | 1 + .../sbas_ionospheric_correction.cc | 467 --- .../sbas_ionospheric_correction.h | 203 - .../sbas_satellite_correction.cc | 296 -- .../sbas_satellite_correction.h | 107 - .../system_parameters/sbas_telemetry_data.cc | 978 ----- .../system_parameters/sbas_telemetry_data.h | 492 --- src/core/system_parameters/sbas_time.h | 193 - src/tests/test_main.cc | 6 - src/utils/front-end-cal/main.cc | 5 +- 50 files changed, 12029 insertions(+), 3104 deletions(-) create mode 100644 src/algorithms/PVT/adapters/rtklib_pvt.cc create mode 100644 src/algorithms/PVT/adapters/rtklib_pvt.h create mode 100644 src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc create mode 100644 src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h create mode 100644 src/algorithms/PVT/libs/rtklib_solver.cc create mode 100644 src/algorithms/PVT/libs/rtklib_solver.h create mode 100644 src/algorithms/libs/rtklib/CMakeLists.txt create mode 100644 src/algorithms/libs/rtklib/rtklib.h create mode 100644 src/algorithms/libs/rtklib/rtklib_conversions.cc create mode 100644 src/algorithms/libs/rtklib/rtklib_conversions.h create mode 100644 src/algorithms/libs/rtklib/rtklib_ephemeris.cc create mode 100644 src/algorithms/libs/rtklib/rtklib_ephemeris.h create mode 100644 src/algorithms/libs/rtklib/rtklib_ionex.cc create mode 100644 src/algorithms/libs/rtklib/rtklib_ionex.h create mode 100644 src/algorithms/libs/rtklib/rtklib_pntpos.cc create mode 100644 src/algorithms/libs/rtklib/rtklib_pntpos.h create mode 100644 src/algorithms/libs/rtklib/rtklib_preceph.cc create mode 100644 src/algorithms/libs/rtklib/rtklib_preceph.h create mode 100644 src/algorithms/libs/rtklib/rtklib_rtkcmn.cc create mode 100644 src/algorithms/libs/rtklib/rtklib_rtkcmn.h create mode 100644 src/algorithms/libs/rtklib/rtklib_sbas.cc create mode 100644 src/algorithms/libs/rtklib/rtklib_sbas.h delete mode 100644 src/core/system_parameters/sbas_ionospheric_correction.cc delete mode 100644 src/core/system_parameters/sbas_ionospheric_correction.h delete mode 100644 src/core/system_parameters/sbas_satellite_correction.cc delete mode 100644 src/core/system_parameters/sbas_satellite_correction.h delete mode 100644 src/core/system_parameters/sbas_telemetry_data.cc delete mode 100644 src/core/system_parameters/sbas_telemetry_data.h delete mode 100644 src/core/system_parameters/sbas_time.h diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf index 5c5bacbeb..c5c594e52 100644 --- a/conf/gnss-sdr_Hybrid_byte_sim.conf +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -142,7 +142,7 @@ Resampler.item_type = gr_complex; ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_1C.count=12 +Channels_1C.count=11 ;#count: Number of available Galileo satellite channels. Channels_1B.count=0 ;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver @@ -304,7 +304,7 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +PVT.implementation=RTKLIB_PVT ;#averaging_depth: Number of PVT observations in the moving average algorithm PVT.averaging_depth=10 diff --git a/src/algorithms/PVT/adapters/CMakeLists.txt b/src/algorithms/PVT/adapters/CMakeLists.txt index bea1757e5..3b51f35a4 100644 --- a/src/algorithms/PVT/adapters/CMakeLists.txt +++ b/src/algorithms/PVT/adapters/CMakeLists.txt @@ -18,6 +18,7 @@ set(PVT_ADAPTER_SOURCES hybrid_pvt.cc + rtklib_pvt.cc ) include_directories( @@ -27,6 +28,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} diff --git a/src/algorithms/PVT/adapters/hybrid_pvt.cc b/src/algorithms/PVT/adapters/hybrid_pvt.cc index 98b7a3836..a9d9e7404 100644 --- a/src/algorithms/PVT/adapters/hybrid_pvt.cc +++ b/src/algorithms/PVT/adapters/hybrid_pvt.cc @@ -1,6 +1,6 @@ /*! * \file hybrid_pvt.cc - * \brief Implementation of an adapter of a GALILEO E1 PVT solver block to a + * \brief Implementation of an adapter of a HYBRID PVT solver block to a * PvtInterface * \author Javier Arribas, 2011. jarribas(at)cttc.es * diff --git a/src/algorithms/PVT/adapters/hybrid_pvt.h b/src/algorithms/PVT/adapters/hybrid_pvt.h index c2a5c30d3..ab83fb183 100644 --- a/src/algorithms/PVT/adapters/hybrid_pvt.h +++ b/src/algorithms/PVT/adapters/hybrid_pvt.h @@ -1,6 +1,6 @@ /*! * \file hybrid_pvt.h - * \brief Interface of an adapter of a GALILEO E1 PVT solver block to a + * \brief Interface of an adapter of a HYBRID PVT solver block to a * PvtInterface. * \author Javier Arribas, 2013. jarribas(at)cttc.es * diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc new file mode 100644 index 000000000..6820fd755 --- /dev/null +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -0,0 +1,235 @@ +/*! + * \file rtklib_pvt.cc + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#include "rtklib_pvt.h" +#include +#include +#include +#include +#include +#include "configuration_interface.h" + + +using google::LogMessage; + +RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams) : + role_(role), + in_streams_(in_streams), + out_streams_(out_streams) +{ + // dump parameters + std::string default_dump_filename = "./pvt.dat"; + std::string default_nmea_dump_filename = "./nmea_pvt.nmea"; + std::string default_nmea_dump_devname = "/dev/tty1"; + std::string default_rtcm_dump_devname = "/dev/pts/1"; + DLOG(INFO) << "role " << role; + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); + + // moving average depth parameters + int averaging_depth = configuration->property(role + ".averaging_depth", 10); + bool flag_averaging = configuration->property(role + ".flag_averaging", false); + + // output rate + int output_rate_ms = configuration->property(role + ".output_rate_ms", 500); + + // display rate + int display_rate_ms = configuration->property(role + ".display_rate_ms", 500); + + // NMEA Printer settings + bool flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); + std::string nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); + std::string nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); + + // RTCM Printer settings + bool flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); + std::string rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname); + bool flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false); + unsigned short rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101); + unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); + // RTCM message rates: least common multiple with output_rate_ms + int rtcm_MT1019_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), output_rate_ms); + int rtcm_MT1045_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), output_rate_ms); + int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms); + int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); + int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); + std::map rtcm_msg_rate_ms; + rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; + rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; + for (int k = 1071; k < 1078; k++) // All GPS MSM + { + rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; + } + for (int k = 1091; k < 1098; k++) // All Galileo MSM + { + rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; + } + // getting names from the config file, if available + // default filename for assistance data + const std::string eph_default_xml_filename = "./gps_ephemeris.xml"; + const std::string utc_default_xml_filename = "./gps_utc_model.xml"; + const std::string iono_default_xml_filename = "./gps_iono.xml"; + const std::string ref_time_default_xml_filename = "./gps_ref_time.xml"; + const std::string ref_location_default_xml_filename = "./gps_ref_location.xml"; + eph_xml_filename_ = configuration->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); + //std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); + //std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); + //std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); + //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); + + // 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 E1B + 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 | GPS L5 + Galileo E5a + * 20 | GPS L5 + Galileo E5b + * 21 | GPS L1 C/A + Galileo E1B + GPS L2C + * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + */ + int gps_1C_count = configuration->property("Channels_1C.count", 0); + int gps_2S_count = configuration->property("Channels_2S.count", 0); + int gal_1B_count = configuration->property("Channels_1B.count", 0); + int gal_E5a_count = configuration->property("Channels_5X.count", 0); // GPS L5 or Galileo E5a ? + int gal_E5b_count = configuration->property("Channels_7X.count", 0); + + unsigned int type_of_receiver = 0; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 1; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 2; + + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 4; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 5; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 6; + + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 7; + //if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 9; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 10; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 11; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 12; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 14; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 15; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 17; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 18; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20; + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 21; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22; + + // make PVT object + pvt_ = rtklib_make_pvt_cc(in_streams_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver); + DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; +} + + +bool RtklibPvt::save_assistance_to_XML() +{ + LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_; + std::map eph_map = pvt_->get_GPS_L1_ephemeris_map(); + + if (eph_map.size() > 0) + { + try + { + std::ofstream ofs(eph_xml_filename_.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map); + ofs.close(); + LOG(INFO) << "Saved GPS L1 Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + return false; + } + return true; // return variable (true == succeeded) + } + else + { + LOG(WARNING) << "Failed to save Ephemeris, map is empty"; + return false; + } +} + + +RtklibPvt::~RtklibPvt() +{ + save_assistance_to_XML(); +} + + +void RtklibPvt::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 RtklibPvt::disconnect(gr::top_block_sptr top_block) +{ + if(top_block) { /* top_block is not null */}; + // Nothing to disconnect +} + + +gr::basic_block_sptr RtklibPvt::get_left_block() +{ + return pvt_; +} + + +gr::basic_block_sptr RtklibPvt::get_right_block() +{ + return pvt_; // this is a sink, nothing downstream +} diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h new file mode 100644 index 000000000..963b7c0d7 --- /dev/null +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -0,0 +1,95 @@ +/*! + * \file rtklib_pvt.h + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + + +#ifndef GNSS_SDR_RTKLIB_PVT_H_ +#define GNSS_SDR_RTKLIB_PVT_H_ + +#include +#include "pvt_interface.h" +#include "rtklib_pvt_cc.h" + + +class ConfigurationInterface; + +/*! + * \brief This class implements a PvtInterface for Galileo E1 + */ +class RtklibPvt : public PvtInterface +{ +public: + RtklibPvt(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams); + + virtual ~RtklibPvt(); + + std::string role() + { + return role_; + } + + //! Returns "Hybrid_Pvt" + std::string implementation() + { + return "RTKLIB_PVT"; + } + + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + + void reset() + { + return; + } + + //! All blocks must have an item_size() function implementation. Returns sizeof(gr_complex) + size_t item_size() + { + return sizeof(gr_complex); + } + +private: + rtklib_pvt_cc_sptr pvt_; + bool dump_; + std::string dump_filename_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; + + std::string eph_xml_filename_; + bool save_assistance_to_XML(); +}; + +#endif diff --git a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt index 07a4d18f1..dc02d2a93 100644 --- a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt @@ -18,6 +18,7 @@ set(PVT_GR_BLOCKS_SOURCES hybrid_pvt_cc.cc + rtklib_pvt_cc.cc ) include_directories( @@ -26,6 +27,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 724783a09..9578535a1 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -40,7 +40,6 @@ #include #include #include -#include "concurrent_map.h" using google::LogMessage; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc new file mode 100644 index 000000000..a9abf34b8 --- /dev/null +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -0,0 +1,1104 @@ +/*! + * \file rtklib_pvt_cc.cc + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "rtklib_pvt_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using google::LogMessage; + +rtklib_pvt_cc_sptr +rtklib_make_pvt_cc(unsigned int nchannels, + bool dump, + std::string dump_filename, + int averaging_depth, + bool flag_averaging, + int output_rate_ms, + int display_rate_ms, + bool flag_nmea_tty_port, + std::string nmea_dump_filename, + std::string nmea_dump_devname, + bool flag_rtcm_server, + bool flag_rtcm_tty_port, + unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, + std::map rtcm_msg_rate_ms, + std::string rtcm_dump_devname, + const unsigned int type_of_receiver) +{ + return rtklib_pvt_cc_sptr(new rtklib_pvt_cc(nchannels, + dump, + dump_filename, + averaging_depth, + flag_averaging, + output_rate_ms, + display_rate_ms, + flag_nmea_tty_port, + nmea_dump_filename, + nmea_dump_devname, + flag_rtcm_server, + flag_rtcm_tty_port, + rtcm_tcp_port, + rtcm_station_id, + rtcm_msg_rate_ms, + rtcm_dump_devname, + type_of_receiver)); +} + + +void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) +{ + try { + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS EPHEMERIS ### + std::shared_ptr gps_eph; + gps_eph = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "Ephemeris record has arrived from SAT ID " + << gps_eph->i_satellite_PRN << " (Block " + << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" + << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" + << gps_eph->i_GPS_week; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS IONO ### + std::shared_ptr gps_iono; + gps_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_iono = *gps_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS UTC MODEL ### + std::shared_ptr gps_utc_model; + gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_utc_model = *gps_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo EPHEMERIS ### + std::shared_ptr galileo_eph; + galileo_eph = boost::any_cast>(pmt::any_ref(msg)); + // insert new ephemeris record + DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5 + << ", GALILEO Week Number =" << galileo_eph->WN_5 + << " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo IONO ### + std::shared_ptr galileo_iono; + galileo_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->galileo_iono = *galileo_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo UTC MODEL ### + std::shared_ptr galileo_utc_model; + galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->galileo_utc_model = *galileo_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo Almanac ### + std::shared_ptr galileo_almanac; + galileo_almanac = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->galileo_almanac = *galileo_almanac; + DLOG(INFO) << "New Galileo Almanac has arrived "; + } + + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV message ### + std::shared_ptr gps_cnav_ephemeris; + gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; + LOG(INFO) << "New GPS CNAV ephemeris record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV IONO ### + std::shared_ptr gps_cnav_iono; + gps_cnav_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_cnav_iono = *gps_cnav_iono; + DLOG(INFO) << "New CNAV IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV UTC MODEL ### + std::shared_ptr gps_cnav_utc_model; + gps_cnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_cnav_utc_model = *gps_cnav_utc_model; + DLOG(INFO) << "New CNAV UTC record has arrived "; + } + else + { + LOG(WARNING) << "msg_handler_telemetry unknown object type!"; + } + + } + catch(boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + } +} + + +std::map rtklib_pvt_cc::get_GPS_L1_ephemeris_map() +{ + return d_ls_pvt->gps_ephemeris_map; +} + + +rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename, + int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, + std::string nmea_dump_filename, std::string nmea_dump_devname, + bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname, const unsigned int type_of_receiver) : + gr::block("rtklib_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::io_signature::make(0, 0, sizeof(gr_complex))) + +{ + d_output_rate_ms = output_rate_ms; + d_display_rate_ms = display_rate_ms; + d_dump = dump; + d_nchannels = nchannels; + d_dump_filename = dump_filename; + std::string dump_ls_pvt_filename = dump_filename; + type_of_rx = type_of_receiver; + + // GPS Ephemeris data message port in + this->message_port_register_in(pmt::mp("telemetry")); + this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&rtklib_pvt_cc::msg_handler_telemetry, this, _1)); + + //initialize kml_printer + std::string kml_dump_filename; + kml_dump_filename = d_dump_filename; + d_kml_dump = std::make_shared(); + d_kml_dump->set_headers(kml_dump_filename); + + //initialize geojson_printer + std::string geojson_dump_filename; + geojson_dump_filename = d_dump_filename; + d_geojson_printer = std::make_shared(); + d_geojson_printer->set_headers(geojson_dump_filename); + + //initialize nmea_printer + d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); + + //initialize rtcm_printer + std::string rtcm_dump_filename; + rtcm_dump_filename = d_dump_filename; + d_rtcm_printer = std::make_shared(rtcm_dump_filename, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_dump_devname); + if(rtcm_msg_rate_ms.find(1019) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms[1019]; + } + else + { + d_rtcm_MT1019_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set + } + if(rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; + } + else + { + d_rtcm_MT1045_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set + } + if(rtcm_msg_rate_ms.find(1077) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077 + { + d_rtcm_MT1077_rate_ms = rtcm_msg_rate_ms[1077]; + } + else + { + d_rtcm_MT1077_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + } + if(rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 + { + d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097]; + d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms[1097]; + } + else + { + d_rtcm_MT1097_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + d_rtcm_MSM_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + } + b_rtcm_writing_started = false; + + d_dump_filename.append("_raw.dat"); + dump_ls_pvt_filename.append("_ls_pvt.dat"); + d_averaging_depth = averaging_depth; + d_flag_averaging = flag_averaging; + + d_ls_pvt = std::make_shared((int)nchannels, dump_ls_pvt_filename, d_dump); + d_ls_pvt->set_averaging_depth(d_averaging_depth); + + d_sample_counter = 0; + d_last_sample_nav_output = 0; + d_rx_time = 0.0; + + b_rinex_header_written = false; + b_rinex_header_updated = false; + rp = std::make_shared(); + + d_last_status_print_seg = 0; + + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception opening PVT dump file " << e.what(); + } + } + } + + // Create Sys V message queue + first_fix = true; + sysv_msg_key = 1101; + int msgflg = IPC_CREAT | 0666; + if ((sysv_msqid = msgget(sysv_msg_key, msgflg )) == -1) + { + std::cout << "GNSS-SDR can not create message queues!" << std::endl; + throw new std::exception(); + } +} + + + +rtklib_pvt_cc::~rtklib_pvt_cc() +{ + msgctl(sysv_msqid, IPC_RMID, NULL); + + //save GPS L2CM ephemeris to XML file + std::string file_name="eph_GPS_L2CM.xml"; + + if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_cnav_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GPS L2CM Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GPS L2CM Ephemeris, map is empty"; + } + + //save GPS L1 CA ephemeris to XML file + file_name="eph_GPS_L1CA.xml"; + + if (d_ls_pvt->gps_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GPS L1 CA Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GPS L1 CA Ephemeris, map is empty"; + } + + //save Galileo E1 ephemeris to XML file + file_name="eph_Galileo_E1.xml"; + + if (d_ls_pvt->galileo_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->galileo_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved Galileo E1 Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty"; + } + +} + + + +bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair& a, const std::pair& b) +{ + return (a.second.Pseudorange_m) < (b.second.Pseudorange_m); +} + + +void rtklib_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronization_data) +{ + // Print the current receiver status using std::cout every second + int current_rx_seg = floor((double)channels_synchronization_data[0][0].Tracking_sample_counter/(double)channels_synchronization_data[0][0].fs); + if ( current_rx_seg != d_last_status_print_seg) + { + d_last_status_print_seg = current_rx_seg; + std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush; + //DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) + // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + } +} + + +bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) +{ + /* Fill Sys V message structures */ + int msgsend_size; + ttff_msgbuf msg; + msg.ttff = ttff.ttff; + msgsend_size = sizeof(msg.ttff); + msg.mtype = 1; /* default message ID */ + + /* SEND SOLUTION OVER A MESSAGE QUEUE */ + /* non-blocking Sys V message send */ + msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT); + return true; +} + + +int rtklib_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) +{ + d_sample_counter++; + unsigned int gps_channel = 0; + unsigned int gal_channel = 0; + + gnss_observables_map.clear(); + + Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer + + print_receiver_status(in); + + // ############ 1. READ PSEUDORANGES #### + for (unsigned int i = 0; i < d_nchannels; i++) + { + if (in[i][0].Flag_valid_pseudorange == true) + { + // store valid observables in a map. + gnss_observables_map.insert(std::pair(i, in[i][0])); + d_rx_time = in[i][0].RX_time; + if(d_ls_pvt->gps_ephemeris_map.size() > 0) + { + std::map::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN); + if(tmp_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time + } + } + if(d_ls_pvt->galileo_ephemeris_map.size() > 0) + { + std::map::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN); + if(tmp_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time + } + } + if(d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + std::map::iterator tmp_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN); + if(tmp_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time + } + } + } + } + + std::map::iterator galileo_ephemeris_iter; + std::map::iterator gps_ephemeris_iter; + std::map::iterator gps_cnav_ephemeris_iter; + std::map::iterator gnss_observables_iter; + + /* + * 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 E1B + 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 | GPS L5 + Galileo E5a + * 20 | GPS L5 + Galileo E5b + * 21 | GPS L1 C/A + Galileo E1B + GPS L2C + * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + */ + + // ############ 2 COMPUTE THE PVT ################################ + if (gnss_observables_map.size() > 0) + { + // compute on the fly PVT solution + if ((d_sample_counter % d_output_rate_ms) == 0) + { + bool pvt_result; + pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); + + if (pvt_result == true) + { + // correct the observable to account for the receiver clock offset + + for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) + { + it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s; + } + if(first_fix == true) + { + std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + ttff_msgbuf ttff; + ttff.mtype = 1; + ttff.ttff = d_sample_counter; + send_sys_v_ttff_msg(ttff); + first_fix = false; + } + d_kml_dump->print_position(d_ls_pvt, d_flag_averaging); + d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); + d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); + + // ####################### RINEX FILES ################# + if (!b_rinex_header_written) // & we have utc data in nav message! + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + if(type_of_rx == 1) // GPS L1 C/A only + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 2) // GPS L2C only + { + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 4) // Galileo E1B only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 5) // Galileo E5a only + { + std::string signal("5X"); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 6) // Galileo E5b only + { + std::string signal("7X"); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + { + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("1B"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 10) // GPS L1 C/A + Galileo E5a + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("5X"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 11) // GPS L1 C/A + Galileo E5b + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("7X"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 14) // Galileo E1B + Galileo E5a + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) + { + std::string gal_signal("1B 5X"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 15) // Galileo E1B + Galileo E5b + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) + { + std::string gal_signal("1B 7X"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + } + if(b_rinex_header_written) // The header is already written, we can now log the navigation message data + { + // Limit the RINEX navigation output rate + // Notice that d_sample_counter period is 4ms (for Galileo correlators) + if ((d_sample_counter - d_last_sample_nav_output) >= 6000) + { + if(type_of_rx == 1) // GPS L1 C/A only + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); + } + if(type_of_rx == 2) // GPS L2C only + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); + } + if( (type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) ) // Galileo + { + rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); + } + if((type_of_rx == 9) || (type_of_rx == 10) || (type_of_rx == 11)) // GPS L1 C/A + Galileo + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + } + if((type_of_rx == 14) || (type_of_rx == 15)) // Galileo E1B + Galileo E5a + { + rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); + } + + d_last_sample_nav_output = d_sample_counter; + } + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + // Log observables into the RINEX file + if(type_of_rx == 1) // GPS L1 C/A only + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 2) // GPS L2C only + { + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 4) // Galileo E1B only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 5) // Galileo E5a only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 6) // Galileo E5b only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if( (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 14) // Galileo E1B + Galileo E5a + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 15) // Galileo E1B + Galileo E5b + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + } + + // ####################### RTCM MESSAGES ################# + if(b_rtcm_writing_started) + { + if(type_of_rx == 1) // GPS L1 C/A + { + if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0) + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if((d_sample_counter % d_rtcm_MSM_rate_ms) == 0) + { + std::map::iterator gps_ephemeris_iter; + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + { + if((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4) ) == 0) + { + for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + } + } + if((d_sample_counter % (d_rtcm_MSM_rate_ms / 4) ) == 0) + { + std::map::iterator gal_ephemeris_iter; + gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0) + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if((d_sample_counter % d_rtcm_MSM_rate_ms) == 0) + { + std::map::iterator gps_ephemeris_iter; + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + std::map::iterator gps_cnav_ephemeris_iter; + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if(((d_sample_counter % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0)) + { + for(gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (d_rtcm_MT1045_rate_ms != 0)) + { + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0)) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(gal_channel == 0) + { + if(system.compare("E") == 0) + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + gal_channel = i; + } + } + } + i++; + } + if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) ) + { + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if(((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) ) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + } + + if(!b_rtcm_writing_started) // the first time + { + if(type_of_rx == 1) // GPS L1 C/A + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + + std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + + if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + { + for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + } + + std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + + std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + std::map::iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if(d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(d_rtcm_MT1045_rate_ms != 0) + { + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(gal_channel == 0) + { + if(system.compare("E") == 0) + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + gal_channel = i; + } + } + } + i++; + } + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0) ) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + } + } + } + + // DEBUG MESSAGE: Display position in console output + if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true) + { + std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + + LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; + + /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " + << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP + << " GDOP = " << d_ls_pvt->d_GDOP << std::endl; */ + } + + // MULTIPLEXED FILE RECORDING - Record results to file + if(d_dump == true) + { + try + { + double tmp_double; + for (unsigned int i = 0; i < d_nchannels; i++) + { + tmp_double = in[i][0].Pseudorange_m; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_double = 0; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + d_dump_file.write((char*)&d_rx_time, sizeof(double)); + } + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + } + + consume_each(1); //one by one + return 1; +} diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h new file mode 100644 index 000000000..70429ea3c --- /dev/null +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -0,0 +1,172 @@ +/*! + * \file rtklib_pvt_cc.h + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_RTKLIB_PVT_CC_H +#define GNSS_SDR_RTKLIB_PVT_CC_H + +#include +#include +#include +#include +#include +#include +#include +#include "nmea_printer.h" +#include "kml_printer.h" +#include "geojson_printer.h" +#include "rinex_printer.h" +#include "rtcm_printer.h" +#include "rtklib_solver.h" + + +class rtklib_pvt_cc; + +typedef boost::shared_ptr rtklib_pvt_cc_sptr; + +rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int n_channels, + bool dump, + std::string dump_filename, + int averaging_depth, + bool flag_averaging, + int output_rate_ms, + int display_rate_ms, + bool flag_nmea_tty_port, + std::string nmea_dump_filename, + std::string nmea_dump_devname, + bool flag_rtcm_server, + bool flag_rtcm_tty_port, + unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, + std::map rtcm_msg_rate_ms, + std::string rtcm_dump_devname, + const unsigned int type_of_receiver); + +/*! + * \brief This class implements a block that computes the PVT solution with Galileo E1 signals + */ +class rtklib_pvt_cc : public gr::block +{ +private: + friend rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels, + bool dump, + std::string dump_filename, + int averaging_depth, + bool flag_averaging, + int output_rate_ms, + int display_rate_ms, + bool flag_nmea_tty_port, + std::string nmea_dump_filename, + std::string nmea_dump_devname, + bool flag_rtcm_server, + bool flag_rtcm_tty_port, + unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, + std::map rtcm_msg_rate_ms, + std::string rtcm_dump_devname, + const unsigned int type_of_receiver); + rtklib_pvt_cc(unsigned int nchannels, + bool dump, std::string dump_filename, + int averaging_depth, + bool flag_averaging, + int output_rate_ms, + int display_rate_ms, + bool flag_nmea_tty_port, + std::string nmea_dump_filename, + std::string nmea_dump_devname, + bool flag_rtcm_server, + bool flag_rtcm_tty_port, + unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, + std::map rtcm_msg_rate_ms, + std::string rtcm_dump_devname, + const unsigned int type_of_receiver); + + void msg_handler_telemetry(pmt::pmt_t msg); + + bool d_dump; + bool b_rinex_header_written; + bool b_rinex_header_updated; + bool b_rtcm_writing_started; + int d_rtcm_MT1045_rate_ms; + int d_rtcm_MT1019_rate_ms; + int d_rtcm_MT1077_rate_ms; + int d_rtcm_MT1097_rate_ms; + int d_rtcm_MSM_rate_ms; + + void print_receiver_status(Gnss_Synchro** channels_synchronization_data); + int d_last_status_print_seg; //for status printer + + unsigned int d_nchannels; + std::string d_dump_filename; + std::ofstream d_dump_file; + int d_averaging_depth; + bool d_flag_averaging; + int d_output_rate_ms; + int d_display_rate_ms; + long unsigned int d_sample_counter; + long unsigned int d_last_sample_nav_output; + + std::shared_ptr rp; + std::shared_ptr d_kml_dump; + std::shared_ptr d_nmea_printer; + std::shared_ptr d_geojson_printer; + std::shared_ptr d_rtcm_printer; + double d_rx_time; + + std::shared_ptr d_ls_pvt; + std::map gnss_observables_map; + bool observables_pairCompare_min(const std::pair& a, const std::pair& b); + + unsigned int type_of_rx; + + bool first_fix; + key_t sysv_msg_key; + int sysv_msqid; + typedef struct { + long mtype;//required by sys v message + double ttff; + } ttff_msgbuf; + bool send_sys_v_ttff_msg(ttff_msgbuf ttff); + +public: + /*! + * \brief Get latest set of GPS L1 ephemeris from PVT block + * + * It is used to save the assistance data at the receiver shutdown + */ + std::map get_GPS_L1_ephemeris_map(); + + ~rtklib_pvt_cc (); //!< Default destructor + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing +}; + +#endif diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 26d8eead1..d223454d3 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -27,6 +27,7 @@ set(PVT_LIB_SOURCES nmea_printer.cc rtcm_printer.cc geojson_printer.cc + rtklib_solver.cc ) include_directories( @@ -35,6 +36,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${Boost_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} @@ -44,5 +46,17 @@ file(GLOB PVT_LIB_HEADERS "*.h") list(SORT PVT_LIB_HEADERS) add_library(pvt_lib ${PVT_LIB_SOURCES} ${PVT_LIB_HEADERS}) source_group(Headers FILES ${PVT_LIB_HEADERS}) -add_dependencies(pvt_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE}) -target_link_libraries(pvt_lib ${Boost_LIBRARIES} ${GFlags_LIBS} ${GLOG_LIBRARIES} ${ARMADILLO_LIBRARIES}) +add_dependencies(pvt_lib rtklib_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE}) + + +target_link_libraries( + pvt_lib + rtklib_lib + ${Boost_LIBRARIES} + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${ARMADILLO_LIBRARIES} + ${BLAS} + ${LAPACK} + ) + diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 3d9e1c419..87e40b2d6 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -71,161 +71,6 @@ hybrid_ls_pvt::~hybrid_ls_pvt() d_dump_file.close(); } - -gtime_t hybrid_ls_pvt::epoch2time(const double *ep) -{ - const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335}; - gtime_t time={0}; - int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2]; - - if (year<1970||2099=3?1:0); - sec=(int)floor(ep[5]); - time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec; - time.sec=ep[5]-sec; - return time; -} - -gtime_t hybrid_ls_pvt::gpst2time(int week, double sec) -{ - const static double gpst0[]={1980,1, 6,0,0,0}; /* gps time reference */ - gtime_t t=epoch2time(gpst0); - - if (sec<-1E9||1E912) n=12; - if (1.0-t.sec<0.5/pow(10.0,n)) {t.time++; t.sec=0.0;}; - time2epoch(t,ep); - sprintf(s,"%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f",ep[0],ep[1],ep[2], - ep[3],ep[4],n<=0?2:n+3,n<=0?0:n,ep[5]); -} - -double hybrid_ls_pvt::time2gpst(gtime_t t, int *week) -{ - const static double gpst0[]={1980,1, 6,0,0,0}; /* gps time reference */ - gtime_t t0=epoch2time(gpst0); - time_t sec=t.time-t0.time; - int w=(int)(sec/(86400*7)); - - if (week) *week=w; - return (double)(sec-w*86400*7)+t.sec; -} - -void hybrid_ls_pvt::time2epoch(gtime_t t, double *ep) -{ - const int mday[]={ /* # of days in a month */ - 31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31, - 31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31 - }; - int days,sec,mon,day; - - /* leap year if year%4==0 in 1901-2099 */ - days=(int)(t.time/86400); - sec=(int)(t.time-(time_t)days*86400); - for (day=days%1461,mon=0;mon<48;mon++) { - if (day>=mday[mon]) day-=mday[mon]; else break; - } - ep[0]=1970+days/1461*4+mon/12; ep[1]=mon%12+1; ep[2]=day+1; - ep[3]=sec/3600; ep[4]=sec%3600/60; ep[5]=sec%60+t.sec; -} - -char* hybrid_ls_pvt::time_str(gtime_t t, int n) -{ - static char buff[64]; - time2str(t,buff,n); - return buff; -} - - -/* time difference ------------------------------------------------------------- -* difference between gtime_t structs -* args : gtime_t t1,t2 I gtime_t structs -* return : time difference (t1-t2) (s) -*-----------------------------------------------------------------------------*/ -double hybrid_ls_pvt::timediff(gtime_t t1, gtime_t t2) -{ - return difftime(t1.time,t2.time)+t1.sec-t2.sec; -} - -/* add time -------------------------------------------------------------------- -* add time to gtime_t struct -* args : gtime_t t I gtime_t struct -* double sec I time to add (s) -* return : gtime_t struct (t+sec) -*-----------------------------------------------------------------------------*/ -gtime_t hybrid_ls_pvt::timeadd(gtime_t t, double sec) -{ - double tt; - - t.sec+=sec; tt=floor(t.sec); t.time+=(int)tt; t.sec-=tt; - return t; -} - - -gtime_t hybrid_ls_pvt::utc2gpst(gtime_t t) -{ - const int MAXLEAPS=64; - static double leaps[MAXLEAPS+1][7]={ /* leap seconds (y,m,d,h,m,s,utc-gpst) */ - {2017,1,1,0,0,0,-18}, - {2015,7,1,0,0,0,-17}, - {2012,7,1,0,0,0,-16}, - {2009,1,1,0,0,0,-15}, - {2006,1,1,0,0,0,-14}, - {1999,1,1,0,0,0,-13}, - {1997,7,1,0,0,0,-12}, - {1996,1,1,0,0,0,-11}, - {1994,7,1,0,0,0,-10}, - {1993,7,1,0,0,0, -9}, - {1992,7,1,0,0,0, -8}, - {1991,1,1,0,0,0, -7}, - {1990,1,1,0,0,0, -6}, - {1988,1,1,0,0,0, -5}, - {1985,7,1,0,0,0, -4}, - {1983,7,1,0,0,0, -3}, - {1982,7,1,0,0,0, -2}, - {1981,7,1,0,0,0, -1}, - {0} - }; - int i; - for (i=0;leaps[i][0]>0;i++) { - if (timediff(t,epoch2time(leaps[i]))>=0.0) return timeadd(t,-leaps[i][6]); - } - return t; -} - -gtime_t hybrid_ls_pvt::timeget(void) -{ - static double timeoffset_=0.0; /* time offset (s) */ - double ep[6]={0}; - - struct timeval tv; - struct tm *tt; - - if (!gettimeofday(&tv,NULL)&&(tt=gmtime(&tv.tv_sec))) { - ep[0]=tt->tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday; - ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1E-6; - } - return timeadd(epoch2time(ep),timeoffset_); -} - -int hybrid_ls_pvt::adjgpsweek(int week) -{ - int w; - (void)time2gpst(utc2gpst(timeget()),&w); - if (w<1560) w=1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */ - return week+(w-week+512)/1024*1024; -} - bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging) { std::map::iterator gnss_observables_iter; diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h index 52e9db904..e1084d6e7 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h @@ -41,12 +41,7 @@ #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" #include "gnss_synchro.h" - - -typedef struct { /* time struct */ - time_t time; /* time (s) expressed by standard time_t */ - double sec; /* fraction of second under 1 s */ -} gtime_t; +#include "rtklib_rtkcmn.h" /*! * \brief This class implements a simple PVT Least Squares solution @@ -54,94 +49,6 @@ typedef struct { /* time struct */ class hybrid_ls_pvt : public Ls_Pvt { private: - /* convert calendar day/time to time ------------------------------------------- - * convert calendar day/time to gtime_t struct - * args : double *ep I day/time {year,month,day,hour,min,sec} - * return : gtime_t struct - * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) - *-----------------------------------------------------------------------------*/ - gtime_t epoch2time(const double *ep); - - /* gps time to time ------------------------------------------------------------ - * convert week and tow in gps time to gtime_t struct - * args : int week I week number in gps time - * double sec I time of week in gps time (s) - * return : gtime_t struct - *-----------------------------------------------------------------------------*/ - gtime_t gpst2time(int week, double sec); - - /* get time string ------------------------------------------------------------- - * get time string - * args : gtime_t t I gtime_t struct - * int n I number of decimals - * return : time string - * notes : not reentrant, do not use multiple in a function - *-----------------------------------------------------------------------------*/ - char *time_str(gtime_t t, int n); - - /* time to string -------------------------------------------------------------- - * convert gtime_t struct to string - * args : gtime_t t I gtime_t struct - * char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") - * int n I number of decimals - * return : none - *-----------------------------------------------------------------------------*/ - void time2str(gtime_t t, char *s, int n); - - /* time to calendar day/time --------------------------------------------------- - * convert gtime_t struct to calendar day/time - * args : gtime_t t I gtime_t struct - * double *ep O day/time {year,month,day,hour,min,sec} - * return : none - * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) - *-----------------------------------------------------------------------------*/ - void time2epoch(gtime_t t, double *ep); - - /* adjust gps week number ------------------------------------------------------ - * adjust gps week number using cpu time - * args : int week I not-adjusted gps week number - * return : adjusted gps week number - *-----------------------------------------------------------------------------*/ - int adjgpsweek(int week); - - /* time to gps time ------------------------------------------------------------ - * convert gtime_t struct to week and tow in gps time - * args : gtime_t t I gtime_t struct - * int *week IO week number in gps time (NULL: no output) - * return : time of week in gps time (s) - *-----------------------------------------------------------------------------*/ - double time2gpst(gtime_t t, int *week); - - /* utc to gpstime -------------------------------------------------------------- - * convert utc to gpstime considering leap seconds - * args : gtime_t t I time expressed in utc - * return : time expressed in gpstime - * notes : ignore slight time offset under 100 ns - *-----------------------------------------------------------------------------*/ - gtime_t utc2gpst(gtime_t t); - - /* time difference ------------------------------------------------------------- - * difference between gtime_t structs - * args : gtime_t t1,t2 I gtime_t structs - * return : time difference (t1-t2) (s) - *-----------------------------------------------------------------------------*/ - double timediff(gtime_t t1, gtime_t t2); - - /* add time -------------------------------------------------------------------- - * add time to gtime_t struct - * args : gtime_t t I gtime_t struct - * double sec I time to add (s) - * return : gtime_t struct (t+sec) - *-----------------------------------------------------------------------------*/ - gtime_t timeadd(gtime_t t, double sec); - - /* get current time in utc ----------------------------------------------------- - * get current time in utc - * args : none - * return : current time in utc - *-----------------------------------------------------------------------------*/ - gtime_t timeget(); - public: hybrid_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file); diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 7dcfc704e..0b6fb5030 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -4770,87 +4770,87 @@ void Rinex_Printer::to_date_time(int gps_week, int gps_tow, int &year, int &mont } -void Rinex_Printer::log_rinex_sbs(std::fstream& out, const Sbas_Raw_Msg& sbs_message) -{ - // line 1: PRN / EPOCH / RCVR - std::stringstream line1; - - // SBAS PRN - line1 << sbs_message.get_prn(); - line1 << " "; - - // gps time of reception - int gps_week; - double gps_sec; - if(sbs_message.get_rx_time_obj().get_gps_time(gps_week, gps_sec)) - { - int year; - int month; - int day; - int hour; - int minute; - int second; - - double gps_sec_one_digit_precicion = round(gps_sec *10)/10; // to prevent rounding towards 60.0sec in the stream output - int gps_tow = trunc(gps_sec_one_digit_precicion); - double sub_sec = gps_sec_one_digit_precicion - double(gps_tow); - - to_date_time(gps_week, gps_tow, year, month, day, hour, minute, second); - line1 << asFixWidthString(year, 2, '0') << " " << asFixWidthString(month, 2, '0') << " " << asFixWidthString(day, 2, '0') << " " << asFixWidthString(hour, 2, '0') << " " << asFixWidthString(minute, 2, '0') << " " << rightJustify(asString(double(second)+sub_sec,1),4,' '); - } - else - { - line1 << std::string(19, ' '); - } - line1 << " "; - - // band - line1 << "L1"; - line1 << " "; - - // Length of data message (bytes) - line1 << asFixWidthString(sbs_message.get_msg().size(), 3, ' '); - line1 << " "; - // File-internal receiver index - line1 << " 0"; - line1 << " "; - // Transmission System Identifier - line1 << "SBA"; - line1 << std::string(35, ' '); - lengthCheck(line1.str()); - out << line1.str() << std::endl; - - // DATA RECORD - 1 - std::stringstream line2; - line2 << " "; - // Message frame identifier - if (sbs_message.get_msg_type() < 10) line2 << " "; - line2 << sbs_message.get_msg_type(); - line2 << std::string(4, ' '); - // First 18 bytes of message (hex) - std::vector msg = sbs_message.get_msg(); - for (size_t i = 0; i < 18 && i < msg.size(); ++i) - { - line2 << std::hex << std::setfill('0') << std::setw(2); - line2 << int(msg[i]) << " "; - } - line2 << std::string(19, ' '); - lengthCheck(line2.str()); - out << line2.str() << std::endl; - - // DATA RECORD - 2 - std::stringstream line3; - line3 << std::string(7, ' '); - // Remaining bytes of message (hex) - for (size_t i = 18; i < 36 && i < msg.size(); ++i) - { - line3 << std::hex << std::setfill('0') << std::setw(2); - line3 << int(msg[i]) << " "; - } - line3 << std::string(31, ' '); - lengthCheck(line3.str()); - out << line3.str() << std::endl; -} +//void Rinex_Printer::log_rinex_sbs(std::fstream& out, const Sbas_Raw_Msg& sbs_message) +//{ +// // line 1: PRN / EPOCH / RCVR +// std::stringstream line1; +// +// // SBAS PRN +// line1 << sbs_message.get_prn(); +// line1 << " "; +// +// // gps time of reception +// int gps_week; +// double gps_sec; +// if(sbs_message.get_rx_time_obj().get_gps_time(gps_week, gps_sec)) +// { +// int year; +// int month; +// int day; +// int hour; +// int minute; +// int second; +// +// double gps_sec_one_digit_precicion = round(gps_sec *10)/10; // to prevent rounding towards 60.0sec in the stream output +// int gps_tow = trunc(gps_sec_one_digit_precicion); +// double sub_sec = gps_sec_one_digit_precicion - double(gps_tow); +// +// to_date_time(gps_week, gps_tow, year, month, day, hour, minute, second); +// line1 << asFixWidthString(year, 2, '0') << " " << asFixWidthString(month, 2, '0') << " " << asFixWidthString(day, 2, '0') << " " << asFixWidthString(hour, 2, '0') << " " << asFixWidthString(minute, 2, '0') << " " << rightJustify(asString(double(second)+sub_sec,1),4,' '); +// } +// else +// { +// line1 << std::string(19, ' '); +// } +// line1 << " "; +// +// // band +// line1 << "L1"; +// line1 << " "; +// +// // Length of data message (bytes) +// line1 << asFixWidthString(sbs_message.get_msg().size(), 3, ' '); +// line1 << " "; +// // File-internal receiver index +// line1 << " 0"; +// line1 << " "; +// // Transmission System Identifier +// line1 << "SBA"; +// line1 << std::string(35, ' '); +// lengthCheck(line1.str()); +// out << line1.str() << std::endl; +// +// // DATA RECORD - 1 +// std::stringstream line2; +// line2 << " "; +// // Message frame identifier +// if (sbs_message.get_msg_type() < 10) line2 << " "; +// line2 << sbs_message.get_msg_type(); +// line2 << std::string(4, ' '); +// // First 18 bytes of message (hex) +// std::vector msg = sbs_message.get_msg(); +// for (size_t i = 0; i < 18 && i < msg.size(); ++i) +// { +// line2 << std::hex << std::setfill('0') << std::setw(2); +// line2 << int(msg[i]) << " "; +// } +// line2 << std::string(19, ' '); +// lengthCheck(line2.str()); +// out << line2.str() << std::endl; +// +// // DATA RECORD - 2 +// std::stringstream line3; +// line3 << std::string(7, ' '); +// // Remaining bytes of message (hex) +// for (size_t i = 18; i < 36 && i < msg.size(); ++i) +// { +// line3 << std::hex << std::setfill('0') << std::setw(2); +// line3 << int(msg[i]) << " "; +// } +// line3 << std::string(31, ' '); +// lengthCheck(line3.str()); +// out << line3.str() << std::endl; +//} int Rinex_Printer::signalStrength(const double snr) diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 3ad44d26b..af168ab9b 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -61,7 +61,6 @@ #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" #include "galileo_navigation_message.h" -#include "sbas_telemetry_data.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" #include "gnss_synchro.h" @@ -214,7 +213,7 @@ public: /*! * \brief Writes raw SBAS messages into the RINEX file */ - void log_rinex_sbs(std::fstream & out, const Sbas_Raw_Msg & sbs_message); + //void log_rinex_sbs(std::fstream & out, const Sbas_Raw_Msg & sbs_message); void update_nav_header(std::fstream & out, const Gps_Utc_Model & gps_utc, const Gps_Iono & gps_iono); diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc new file mode 100644 index 000000000..893c4c01e --- /dev/null +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -0,0 +1,291 @@ +/*! + * \file rtklib_solver.cc + * \brief PVT solver based on rtklib library functions adapted to the GNSS-SDR + * data flow and structures + * \authors
    + *
  • 2017, Javier Arribas + *
  • 2017, 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ + +#include +#include "rtklib_conversions.h" +#include "GPS_L1_CA.h" +#include "Galileo_E1.h" +#include "rtklib_solver.h" + +using google::LogMessage; + +rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file) +{ + // init empty ephemeris for all the available GNSS channels + d_nchannels = nchannels; + d_dump_filename = dump_filename; + d_flag_dump_enabled = flag_dump_to_file; + count_valid_position = 0; + d_flag_averaging = false; + + //RTKLIB PVT solver options + /* defaults processing options */ + prcopt_t default_opt={PMODE_SINGLE,0,2,SYS_GPS, /* mode,soltype,nf,navsys */ + 15.0*D2R,{}, /* elmin,snrmask */ + 0,1,1,1, /* sateph,modear,glomodear,bdsmodear */ + 5,0,10,1, /* maxout,minlock,minfix,armaxiter */ + 0,0,0,0, /* estion,esttrop,dynamics,tidecorr */ + 1,0,0,0,0, /* niter,codesmooth,intpref,sbascorr,sbassatsel */ + 0,0, /* rovpos,refpos */ + {100.0,100.0}, /* eratio[] */ + {100.0,0.003,0.003,0.0,1.0}, /* err[] */ + {30.0,0.03,0.3}, /* std[] */ + {1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */ + 5E-12, /* sclkstab */ + {3.0,0.9999,0.25,0.1,0.05}, /* thresar */ + 0.0,0.0,0.05, /* elmaskar,almaskhold,thresslip */ + 30.0,30.0,30.0, /* maxtdif,maxinno,maxgdop */ + {},{},{}, /* baseline,ru,rb */ + {"",""}, /* anttype */ + {},{},{}, /* antdel,pcv,exsats */ + {},{},{},{},{},{},{},{},{},{} + }; + rtklib_opt=default_opt; + + old_pvt_sol={}; + + // ############# ENABLE DATA FILE LOG ################# + if (d_flag_dump_enabled == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::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::ifstream::failure &e) + { + LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); + } + } + } +} + + +rtklib_solver::~rtklib_solver() +{ + d_dump_file.close(); +} + +bool rtklib_solver::get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging) +{ + std::map::iterator gnss_observables_iter; + std::map::iterator galileo_ephemeris_iter; + std::map::iterator gps_ephemeris_iter; + std::map::iterator gps_cnav_ephemeris_iter; + + d_flag_averaging = flag_averaging; + + // ******************************************************************************** + // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ + // ******************************************************************************** + int valid_obs = 0; //valid observations counter + + obsd_t obs_data[MAXOBS]; + eph_t eph_data[MAXOBS]; + + for(gnss_observables_iter = gnss_observables_map.begin(); + gnss_observables_iter != gnss_observables_map.end(); + gnss_observables_iter++) + { + switch(gnss_observables_iter->second.System) + { + case 'E': + { + // 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.end()) + { + //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 + obs_data[valid_obs]=obs_to_rtklib(gnss_observables_iter->second, galileo_ephemeris_iter->second.WN_5); + 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': + { + // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key + std::string sig_(gnss_observables_iter->second.Signal); + if(sig_.compare("1C") == 0) + { + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != gps_ephemeris_map.end()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs]=eph_to_rtklib(gps_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obs_data[valid_obs]=obs_to_rtklib(gnss_observables_iter->second, gps_ephemeris_iter->second.i_GPS_week); + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; + } + } + if(sig_.compare("2S") == 0) + { + gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end()) + { + //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 + obs_data[valid_obs]=obs_to_rtklib(gnss_observables_iter->second, gps_cnav_ephemeris_iter->second.i_GPS_week); + 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****************************************************** + // ********************************************************************** + d_valid_observations = valid_obs; + + b_valid_position = false; + if (valid_obs>0) + { + int result=0; + char rtklib_msg[128]; + nav_t nav_data; + nav_data.eph=eph_data; + nav_data.n=valid_obs; + for (int i=0; i< MAXSAT;i++) + { + nav_data.lam[i][0]=CLIGHT/FREQ1; /* L1/E1 */ + nav_data.lam[i][1]=CLIGHT/FREQ2; /* L2 */ + } + + result=pntpos(obs_data, valid_obs, &nav_data, &rtklib_opt, &old_pvt_sol, NULL, NULL,rtklib_msg); + if(result==0) + { + DLOG(INFO)<<"RTKLIB pntpos error message: "<[seconds] + DLOG(INFO) << "Hybrid Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; + + boost::posix_time::ptime p_time; + gtime_t rtklib_utc_time=gpst2utc(old_pvt_sol.time); + p_time=boost::posix_time::from_time_t(rtklib_utc_time.time); + p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec*1e6)); + d_position_UTC_time = p_time; + cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); + + DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time) + << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d + << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; + + // ######## LOG FILE ######### + if(d_flag_dump_enabled == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + // PVT GPS time + tmp_double = Rx_time; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position East [m] + tmp_double = rx_position_and_time(0); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position North [m] + tmp_double = rx_position_and_time(1); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position Up [m] + tmp_double = rx_position_and_time(2); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // User clock offset [s] + tmp_double = rx_position_and_time(3); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // GEO user position Latitude [deg] + tmp_double = d_latitude_d; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // GEO user position Longitude [deg] + tmp_double = d_longitude_d; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // GEO user position Height [m] + tmp_double = d_height_m; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); + } + } + } + } + return b_valid_position; +} diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h new file mode 100644 index 000000000..110abef89 --- /dev/null +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -0,0 +1,104 @@ +/*! + * \file rtklib_solver.h + * \brief PVT solver based on rtklib library functions adapted to the GNSS-SDR + * data flow and structures + * \authors
    + *
  • 2017, Javier Arribas + *
  • 2017, 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ +#ifndef GNSS_SDR_RTKLIB_SOLVER_H_ +#define GNSS_SDR_RTKLIB_SOLVER_H_ + +#include +#include +#include +#include +#include "rtklib_pntpos.h" +#include "galileo_navigation_message.h" +#include "gps_navigation_message.h" +#include "gps_cnav_navigation_message.h" +#include "gnss_synchro.h" +#include "pvt_solution.h" + +/*! + * \brief This class implements a simple PVT Least Squares solution + */ +class rtklib_solver : public Pvt_Solution +{ +private: + +public: + rtklib_solver(int nchannels,std::string dump_filename, bool flag_dump_to_file); + ~rtklib_solver(); + + bool get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging); + int d_nchannels; //!< Number of available channels for positioning + + std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris + std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris + std::map gps_cnav_ephemeris_map; + + Galileo_Utc_Model galileo_utc_model; + Galileo_Iono galileo_iono; + Galileo_Almanac galileo_almanac; + + Gps_Utc_Model gps_utc_model; + Gps_Iono gps_iono; + + Gps_CNAV_Iono gps_cnav_iono; + Gps_CNAV_Utc_Model gps_cnav_utc_model; + + int count_valid_position; + + bool d_flag_dump_enabled; + + prcopt_t rtklib_opt; + sol_t old_pvt_sol; + + std::string d_dump_filename; + std::ofstream d_dump_file; +}; + +#endif diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index f0a62912f..a0ce21f3d 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -16,6 +16,7 @@ # along with GNSS-SDR. If not, see . # +add_subdirectory(rtklib) set(GNSS_SPLIBS_SOURCES gps_l2c_signal.cc diff --git a/src/algorithms/libs/rtklib/CMakeLists.txt b/src/algorithms/libs/rtklib/CMakeLists.txt new file mode 100644 index 000000000..fd582ea9d --- /dev/null +++ b/src/algorithms/libs/rtklib/CMakeLists.txt @@ -0,0 +1,60 @@ +# Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors) +# +# This file is part of GNSS-SDR. +# +# GNSS-SDR is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# GNSS-SDR is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with GNSS-SDR. If not, see . +# + +add_definitions( -DGNSS_SDR_VERSION="${VERSION}" ) + +set(RTKLIB_LIB_SOURCES + rtklib_rtkcmn.cc + rtklib_ephemeris.cc + rtklib_preceph.cc + rtklib_sbas.cc + rtklib_ionex.cc + rtklib_pntpos.cc + rtklib_conversions.cc +) + +include_directories( + $(CMAKE_CURRENT_SOURCE_DIR) + ${CMAKE_SOURCE_DIR}/src/core/system_parameters + ${CMAKE_SOURCE_DIR}/src/core/interfaces + ${CMAKE_SOURCE_DIR}/src/core/receiver + ${Boost_INCLUDE_DIRS} + ${ARMADILLO_INCLUDE_DIRS} + ${GFlags_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) +file(GLOB RTKLIB_LIB_HEADERS "*.h") +list(SORT RTKLIB_LIB_HEADERS) +add_library(rtklib_lib ${RTKLIB_LIB_SOURCES} ${RTKLIB_LIB_HEADERS}) +source_group(Headers FILES ${RTKLIB_LIB_HEADERS}) +add_dependencies(rtklib_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE}) + +#set_property(SOURCE rtklib_rtkcmn.cc APPEND_STRING PROPERTY COMPILE_FLAGS " -Wno-missing-field-initializers ") + +target_link_libraries( + rtklib_lib + ${Boost_LIBRARIES} + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${ARMADILLO_LIBRARIES} + ${BLAS} + ${LAPACK} + ) + +#MESSAGE( STATUS "*****************BLAS: " ${BLAS} ) + diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h new file mode 100644 index 000000000..e8bb7bd76 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -0,0 +1,1044 @@ +/*! + * \file rtklib.h + * \brief main header file for the rtklib library + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef RTKLIB_H_ +#define RTKLIB_H_ + +#include +#include +#include +#include +#include +#include +#include + +#define thread_t pthread_t +#define lock_t pthread_mutex_t +#define initlock(f) pthread_mutex_init(f,NULL) +#define lock(f) pthread_mutex_lock(f) +#define unlock(f) pthread_mutex_unlock(f) +#define FILEPATHSEP '/' + +#define PI 3.1415926535897932 /* pi */ +#define D2R (PI/180.0) /* deg to rad */ +#define R2D (180.0/PI) /* rad to deg */ +#define CLIGHT 299792458.0 /* speed of light (m/s) */ +#define SC2RAD 3.1415926535898 /* semi-circle to radian (IS-GPS) */ +#define AU 149597870691.0 /* 1 AU (m) */ +#define AS2R (D2R/3600.0) /* arc sec to radian */ + +#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */ + +#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */ +#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */ + +#define HION 350000.0 /* ionosphere height (m) */ + +#define P2_5 0.03125 /* 2^-5 */ +#define P2_6 0.015625 /* 2^-6 */ +#define P2_11 4.882812500000000E-04 /* 2^-11 */ +#define P2_15 3.051757812500000E-05 /* 2^-15 */ +#define P2_17 7.629394531250000E-06 /* 2^-17 */ +#define P2_19 1.907348632812500E-06 /* 2^-19 */ +#define P2_20 9.536743164062500E-07 /* 2^-20 */ +#define P2_21 4.768371582031250E-07 /* 2^-21 */ +#define P2_23 1.192092895507810E-07 /* 2^-23 */ +#define P2_24 5.960464477539063E-08 /* 2^-24 */ +#define P2_27 7.450580596923828E-09 /* 2^-27 */ +#define P2_29 1.862645149230957E-09 /* 2^-29 */ +#define P2_30 9.313225746154785E-10 /* 2^-30 */ +#define P2_31 4.656612873077393E-10 /* 2^-31 */ +#define P2_32 2.328306436538696E-10 /* 2^-32 */ +#define P2_33 1.164153218269348E-10 /* 2^-33 */ +#define P2_35 2.910383045673370E-11 /* 2^-35 */ +#define P2_38 3.637978807091710E-12 /* 2^-38 */ +#define P2_39 1.818989403545856E-12 /* 2^-39 */ +#define P2_40 9.094947017729280E-13 /* 2^-40 */ +#define P2_43 1.136868377216160E-13 /* 2^-43 */ +#define P2_48 3.552713678800501E-15 /* 2^-48 */ +#define P2_50 8.881784197001252E-16 /* 2^-50 */ +#define P2_55 2.775557561562891E-17 /* 2^-55 */ + +#define POLYCRC32 0xEDB88320u /* CRC32 polynomial */ +#define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */ + +#define PMODE_SINGLE 0 /* positioning mode: single */ +#define PMODE_DGPS 1 /* positioning mode: DGPS/DGNSS */ +#define PMODE_KINEMA 2 /* positioning mode: kinematic */ +#define PMODE_STATIC 3 /* positioning mode: static */ +#define PMODE_MOVEB 4 /* positioning mode: moving-base */ +#define PMODE_FIXED 5 /* positioning mode: fixed */ +#define PMODE_PPP_KINEMA 6 /* positioning mode: PPP-kinemaric */ +#define PMODE_PPP_STATIC 7 /* positioning mode: PPP-static */ +#define PMODE_PPP_FIXED 8 /* positioning mode: PPP-fixed */ + +#define SOLF_LLH 0 /* solution format: lat/lon/height */ +#define SOLF_XYZ 1 /* solution format: x/y/z-ecef */ +#define SOLF_ENU 2 /* solution format: e/n/u-baseline */ +#define SOLF_NMEA 3 /* solution format: NMEA-183 */ +#define SOLF_STAT 4 /* solution format: solution status */ +#define SOLF_GSIF 5 /* solution format: GSI F1/F2 */ + +#define SOLQ_NONE 0 /* solution status: no solution */ +#define SOLQ_FIX 1 /* solution status: fix */ +#define SOLQ_FLOAT 2 /* solution status: float */ +#define SOLQ_SBAS 3 /* solution status: SBAS */ +#define SOLQ_DGPS 4 /* solution status: DGPS/DGNSS */ +#define SOLQ_SINGLE 5 /* solution status: single */ +#define SOLQ_PPP 6 /* solution status: PPP */ +#define SOLQ_DR 7 /* solution status: dead reconing */ +#define MAXSOLQ 7 /* max number of solution status */ + +#define TIMES_GPST 0 /* time system: gps time */ +#define TIMES_UTC 1 /* time system: utc */ +#define TIMES_JST 2 /* time system: jst */ + +#define MAXFREQ 7 /* max NFREQ */ + +#define FREQ1 1.57542E9 /* L1/E1 frequency (Hz) */ +#define FREQ2 1.22760E9 /* L2 frequency (Hz) */ +#define FREQ5 1.17645E9 /* L5/E5a frequency (Hz) */ +#define FREQ6 1.27875E9 /* E6/LEX frequency (Hz) */ +#define FREQ7 1.20714E9 /* E5b frequency (Hz) */ +#define FREQ8 1.191795E9 /* E5a+b frequency (Hz) */ +#define FREQ9 2.492028E9 /* S frequency (Hz) */ +#define FREQ1_GLO 1.60200E9 /* GLONASS G1 base frequency (Hz) */ +#define DFRQ1_GLO 0.56250E6 /* GLONASS G1 bias frequency (Hz/n) */ +#define FREQ2_GLO 1.24600E9 /* GLONASS G2 base frequency (Hz) */ +#define DFRQ2_GLO 0.43750E6 /* GLONASS G2 bias frequency (Hz/n) */ +#define FREQ3_GLO 1.202025E9 /* GLONASS G3 frequency (Hz) */ +#define FREQ1_CMP 1.561098E9 /* BeiDou B1 frequency (Hz) */ +#define FREQ2_CMP 1.20714E9 /* BeiDou B2 frequency (Hz) */ +#define FREQ3_CMP 1.26852E9 /* BeiDou B3 frequency (Hz) */ + +#define MAXLEAPS 64 /* max number of leap seconds table */ +#define DTTOL 0.005 /* tolerance of time difference (s) */ + +#define NFREQ 3 +#define NEXOBS 0 /* number of extended obs codes */ +#define MAXANT 64 /* max length of station name/antenna type */ + +#define MINPRNGPS 1 /* min satellite PRN number of GPS */ +#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ +#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */ +#define NSYSGPS 1 + +#define SYS_NONE 0x00 /* navigation system: none */ +#define SYS_GPS 0x01 /* navigation system: GPS */ +#define SYS_SBS 0x02 /* navigation system: SBAS */ +#define SYS_GLO 0x04 /* navigation system: GLONASS */ +#define SYS_GAL 0x08 /* navigation system: Galileo */ +#define SYS_QZS 0x10 /* navigation system: QZSS */ +#define SYS_CMP 0x20 /* navigation system: BeiDou */ +#define SYS_IRN 0x40 /* navigation system: IRNS */ +#define SYS_LEO 0x80 /* navigation system: LEO */ +#define SYS_ALL 0xFF /* navigation system: all */ + +#define CODE_NONE 0 /* obs code: none or unknown */ +#define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ +#define CODE_L1P 2 /* obs code: L1P,G1P (GPS,GLO) */ +#define CODE_L1W 3 /* obs code: L1 Z-track (GPS) */ +#define CODE_L1Y 4 /* obs code: L1Y (GPS) */ +#define CODE_L1M 5 /* obs code: L1M (GPS) */ +#define CODE_L1N 6 /* obs code: L1codeless (GPS) */ +#define CODE_L1S 7 /* obs code: L1C(D) (GPS,QZS) */ +#define CODE_L1L 8 /* obs code: L1C(P) (GPS,QZS) */ +#define CODE_L1E 9 /* (not used) */ +#define CODE_L1A 10 /* obs code: E1A (GAL) */ +#define CODE_L1B 11 /* obs code: E1B (GAL) */ +#define CODE_L1X 12 /* obs code: E1B+C,L1C(D+P) (GAL,QZS) */ +#define CODE_L1Z 13 /* obs code: E1A+B+C,L1SAIF (GAL,QZS) */ +#define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */ +#define CODE_L2D 15 /* obs code: L2 L1C/A-(P2-P1) (GPS) */ +#define CODE_L2S 16 /* obs code: L2C(M) (GPS,QZS) */ +#define CODE_L2L 17 /* obs code: L2C(L) (GPS,QZS) */ +#define CODE_L2X 18 /* obs code: L2C(M+L),B1I+Q (GPS,QZS,CMP) */ +#define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */ +#define CODE_L2W 20 /* obs code: L2 Z-track (GPS) */ +#define CODE_L2Y 21 /* obs code: L2Y (GPS) */ +#define CODE_L2M 22 /* obs code: L2M (GPS) */ +#define CODE_L2N 23 /* obs code: L2codeless (GPS) */ +#define CODE_L5I 24 /* obs code: L5/E5aI (GPS,GAL,QZS,SBS) */ +#define CODE_L5Q 25 /* obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ +#define CODE_L5X 26 /* obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) */ +#define CODE_L7I 27 /* obs code: E5bI,B2I (GAL,CMP) */ +#define CODE_L7Q 28 /* obs code: E5bQ,B2Q (GAL,CMP) */ +#define CODE_L7X 29 /* obs code: E5bI+Q,B2I+Q (GAL,CMP) */ +#define CODE_L6A 30 /* obs code: E6A (GAL) */ +#define CODE_L6B 31 /* obs code: E6B (GAL) */ +#define CODE_L6C 32 /* obs code: E6C (GAL) */ +#define CODE_L6X 33 /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,CMP) */ +#define CODE_L6Z 34 /* obs code: E6A+B+C (GAL) */ +#define CODE_L6S 35 /* obs code: LEXS (QZS) */ +#define CODE_L6L 36 /* obs code: LEXL (QZS) */ +#define CODE_L8I 37 /* obs code: E5(a+b)I (GAL) */ +#define CODE_L8Q 38 /* obs code: E5(a+b)Q (GAL) */ +#define CODE_L8X 39 /* obs code: E5(a+b)I+Q (GAL) */ +#define CODE_L2I 40 /* obs code: B1I (BDS) */ +#define CODE_L2Q 41 /* obs code: B1Q (BDS) */ +#define CODE_L6I 42 /* obs code: B3I (BDS) */ +#define CODE_L6Q 43 /* obs code: B3Q (BDS) */ +#define CODE_L3I 44 /* obs code: G3I (GLO) */ +#define CODE_L3Q 45 /* obs code: G3Q (GLO) */ +#define CODE_L3X 46 /* obs code: G3I+Q (GLO) */ +#define CODE_L1I 47 /* obs code: B1I (BDS) */ +#define CODE_L1Q 48 /* obs code: B1Q (BDS) */ +#define CODE_L5A 49 /* obs code: L5A SPS (IRN) */ +#define CODE_L5B 50 /* obs code: L5B RS(D) (IRN) */ +#define CODE_L5C 51 /* obs code: L5C RS(P) (IRN) */ +#define CODE_L9A 52 /* obs code: SA SPS (IRN) */ +#define CODE_L9B 53 /* obs code: SB RS(D) (IRN) */ +#define CODE_L9C 54 /* obs code: SC RS(P) (IRN) */ +#define CODE_L9X 55 /* obs code: SB+C (IRN) */ +#define MAXCODE 55 /* max number of obs code */ + +#ifdef ENAGLO +#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ +#define MAXPRNGLO 27 /* max satellite slot number of GLONASS */ +#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */ +#define NSYSGLO 1 +#else +#define MINPRNGLO 0 +#define MAXPRNGLO 0 +#define NSATGLO 0 +#define NSYSGLO 0 +#endif +#ifdef ENAGAL +#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ +#define MAXPRNGAL 30 /* max satellite PRN number of Galileo */ +#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */ +#define NSYSGAL 1 +#else +#define MINPRNGAL 0 +#define MAXPRNGAL 0 +#define NSATGAL 0 +#define NSYSGAL 0 +#endif +#ifdef ENAQZS +#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ +#define MAXPRNQZS 199 /* max satellite PRN number of QZSS */ +#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */ +#define MAXPRNQZS_S 189 /* max satellite PRN number of QZSS SAIF */ +#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */ +#define NSYSQZS 1 +#else +#define MINPRNQZS 0 +#define MAXPRNQZS 0 +#define MINPRNQZS_S 0 +#define MAXPRNQZS_S 0 +#define NSATQZS 0 +#define NSYSQZS 0 +#endif +#ifdef ENACMP +#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ +#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */ +#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */ +#define NSYSCMP 1 +#else +#define MINPRNCMP 0 +#define MAXPRNCMP 0 +#define NSATCMP 0 +#define NSYSCMP 0 +#endif +#ifdef ENAIRN +#define MINPRNIRN 1 /* min satellite sat number of IRNSS */ +#define MAXPRNIRN 7 /* max satellite sat number of IRNSS */ +#define NSATIRN (MAXPRNIRN-MINPRNIRN+1) /* number of IRNSS satellites */ +#define NSYSIRN 1 +#else +#define MINPRNIRN 0 +#define MAXPRNIRN 0 +#define NSATIRN 0 +#define NSYSIRN 0 +#endif +#ifdef ENALEO +#define MINPRNLEO 1 /* min satellite sat number of LEO */ +#define MAXPRNLEO 10 /* max satellite sat number of LEO */ +#define NSATLEO (MAXPRNLEO-MINPRNLEO+1) /* number of LEO satellites */ +#define NSYSLEO 1 +#else +#define MINPRNLEO 0 +#define MAXPRNLEO 0 +#define NSATLEO 0 +#define NSYSLEO 0 +#endif + +#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP+NSYSIRN+NSYSLEO) /* number of systems */ + +#define MINPRNSBS 120 /* min satellite PRN number of SBAS */ +#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */ +#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */ + +#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATIRN+NSATSBS+NSATLEO) + +#define MAXNIGP 201 /* max number of IGP in SBAS band */ + + +#define MAXCODE 55 /* max number of obs code */ + +#define MAXSTA 255 + +#ifndef MAXOBS +#define MAXOBS 64 /* max number of obs in an epoch */ +#endif + +#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */ +#define MAXOBSTYPE 64 /* max number of obs type in RINEX */ +#define DTTOL 0.005 /* tolerance of time difference (s) */ +#define MAXDTOE 7200.0 /* max time difference to GPS Toe (s) */ +#define MAXDTOE_QZS 7200.0 /* max time difference to QZSS Toe (s) */ +#define MAXDTOE_GAL 10800.0 /* max time difference to Galileo Toe (s) */ +#define MAXDTOE_CMP 21600.0 /* max time difference to BeiDou Toe (s) */ +#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */ +#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */ +#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */ +#define MAXGDOP 300.0 /* max GDOP */ + +#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ +#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ +#define MAXSBSURA 8 /* max URA of SBAS satellite */ +#define MAXBAND 10 /* max SBAS band of IGP */ +#define MAXNIGP 201 /* max number of IGP in SBAS band */ +#define MAXNGEO 4 /* max number of GEO satellites */ + +#define MAXSOLMSG 8191 /* max length of solution message */ +#define MAXERRMSG 4096 /* max length of error/warning message */ + +#define IONOOPT_OFF 0 /* ionosphere option: correction off */ +#define IONOOPT_BRDC 1 /* ionosphere option: broadcast model */ +#define IONOOPT_SBAS 2 /* ionosphere option: SBAS model */ +#define IONOOPT_IFLC 3 /* ionosphere option: L1/L2 or L1/L5 iono-free LC */ +#define IONOOPT_EST 4 /* ionosphere option: estimation */ +#define IONOOPT_TEC 5 /* ionosphere option: IONEX TEC model */ +#define IONOOPT_QZS 6 /* ionosphere option: QZSS broadcast model */ +#define IONOOPT_LEX 7 /* ionosphere option: QZSS LEX ionospehre */ +#define IONOOPT_STEC 8 /* ionosphere option: SLANT TEC model */ + +#define TROPOPT_OFF 0 /* troposphere option: correction off */ +#define TROPOPT_SAAS 1 /* troposphere option: Saastamoinen model */ +#define TROPOPT_SBAS 2 /* troposphere option: SBAS model */ +#define TROPOPT_EST 3 /* troposphere option: ZTD estimation */ +#define TROPOPT_ESTG 4 /* troposphere option: ZTD+grad estimation */ +#define TROPOPT_ZTD 5 /* troposphere option: ZTD correction */ + +#define EPHOPT_BRDC 0 /* ephemeris option: broadcast ephemeris */ +#define EPHOPT_PREC 1 /* ephemeris option: precise ephemeris */ +#define EPHOPT_SBAS 2 /* ephemeris option: broadcast + SBAS */ +#define EPHOPT_SSRAPC 3 /* ephemeris option: broadcast + SSR_APC */ +#define EPHOPT_SSRCOM 4 /* ephemeris option: broadcast + SSR_COM */ +#define EPHOPT_LEX 5 /* ephemeris option: QZSS LEX ephemeris */ + +#define EFACT_GPS 1.0 /* error factor: GPS */ +#define EFACT_GLO 1.5 /* error factor: GLONASS */ +#define EFACT_GAL 1.0 /* error factor: Galileo */ +#define EFACT_QZS 1.0 /* error factor: QZSS */ +#define EFACT_CMP 1.0 /* error factor: BeiDou */ +#define EFACT_IRN 1.5 /* error factor: IRNSS */ +#define EFACT_SBS 3.0 /* error factor: SBAS */ + + +#define MAXEXFILE 1024 /* max number of expanded files */ +#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ +#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ +#define MAXSBSURA 8 /* max URA of SBAS satellite */ +#define MAXBAND 10 /* max SBAS band of IGP */ +#define MAXNIGP 201 /* max number of IGP in SBAS band */ + +typedef void fatalfunc_t(const char *); /* fatal callback function type */ + +typedef struct { /* time struct */ + time_t time; /* time (s) expressed by standard time_t */ + double sec; /* fraction of second under 1 s */ +} gtime_t; + +typedef struct { /* observation data record */ + gtime_t time; /* receiver sampling time (GPST) */ + unsigned char sat,rcv; /* satellite/receiver number */ + unsigned char SNR [NFREQ+NEXOBS]; /* signal strength (0.25 dBHz) */ + unsigned char LLI [NFREQ+NEXOBS]; /* loss of lock indicator */ + unsigned char code[NFREQ+NEXOBS]; /* code indicator (CODE_???) */ + double L[NFREQ+NEXOBS]; /* observation data carrier-phase (cycle) */ + double P[NFREQ+NEXOBS]; /* observation data pseudorange (m) */ + float D[NFREQ+NEXOBS]; /* observation data doppler frequency (Hz) */ +} obsd_t; + +typedef struct { /* observation data */ + int n,nmax; /* number of obervation data/allocated */ + obsd_t *data; /* observation data records */ +} obs_t; + +typedef struct { /* earth rotation parameter data type */ + double mjd; /* mjd (days) */ + double xp,yp; /* pole offset (rad) */ + double xpr,ypr; /* pole offset rate (rad/day) */ + double ut1_utc; /* ut1-utc (s) */ + double lod; /* length of day (s/day) */ +} erpd_t; + +typedef struct { /* earth rotation parameter type */ + int n,nmax; /* number and max number of data */ + erpd_t *data; /* earth rotation parameter data */ +} erp_t; + +typedef struct { /* antenna parameter type */ + int sat; /* satellite number (0:receiver) */ + char type[MAXANT]; /* antenna type */ + char code[MAXANT]; /* serial number or satellite code */ + gtime_t ts,te; /* valid time start and end */ + double off[NFREQ][ 3]; /* phase center offset e/n/u or x/y/z (m) */ + double var[NFREQ][19]; /* phase center variation (m) */ + /* el=90,85,...,0 or nadir=0,1,2,3,... (deg) */ +} pcv_t; + +typedef struct { /* antenna parameters type */ + int n,nmax; /* number of data/allocated */ + pcv_t *pcv; /* antenna parameters data */ +} pcvs_t; + +typedef struct { /* almanac type */ + int sat; /* satellite number */ + int svh; /* sv health (0:ok) */ + int svconf; /* as and sv config */ + int week; /* GPS/QZS: gps week, GAL: galileo week */ + gtime_t toa; /* Toa */ + /* SV orbit parameters */ + double A,e,i0,OMG0,omg,M0,OMGd; + double toas; /* Toa (s) in week */ + double f0,f1; /* SV clock parameters (af0,af1) */ +} alm_t; + +typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ + int sat; /* satellite number */ + int iode,iodc; /* IODE,IODC */ + int sva; /* SV accuracy (URA index) */ + int svh; /* SV health (0:ok) */ + int week; /* GPS/QZS: gps week, GAL: galileo week */ + int code; /* GPS/QZS: code on L2, GAL/CMP: data sources */ + int flag; /* GPS/QZS: L2 P data flag, CMP: nav type */ + gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */ + /* SV orbit parameters */ + double A,e,i0,OMG0,omg,M0,deln,OMGd,idot; + double crc,crs,cuc,cus,cic,cis; + double toes; /* Toe (s) in week */ + double fit; /* fit interval (h) */ + double f0,f1,f2; /* SV clock parameters (af0,af1,af2) */ + double tgd[4]; /* group delay parameters */ + /* GPS/QZS:tgd[0]=TGD */ + /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ + /* CMP :tgd[0]=BGD1,tgd[1]=BGD2 */ + double Adot,ndot; /* Adot,ndot for CNAV */ +} eph_t; + +typedef struct { /* GLONASS broadcast ephemeris type */ + int sat; /* satellite number */ + int iode; /* IODE (0-6 bit of tb field) */ + int frq; /* satellite frequency number */ + int svh,sva,age; /* satellite health, accuracy, age of operation */ + gtime_t toe; /* epoch of epherides (gpst) */ + gtime_t tof; /* message frame time (gpst) */ + double pos[3]; /* satellite position (ecef) (m) */ + double vel[3]; /* satellite velocity (ecef) (m/s) */ + double acc[3]; /* satellite acceleration (ecef) (m/s^2) */ + double taun,gamn; /* SV clock bias (s)/relative freq bias */ + double dtaun; /* delay between L1 and L2 (s) */ +} geph_t; + +typedef struct { /* precise ephemeris type */ + gtime_t time; /* time (GPST) */ + int index; /* ephemeris index for multiple files */ + double pos[MAXSAT][4]; /* satellite position/clock (ecef) (m|s) */ + float std[MAXSAT][4]; /* satellite position/clock std (m|s) */ + double vel[MAXSAT][4]; /* satellite velocity/clk-rate (m/s|s/s) */ + float vst[MAXSAT][4]; /* satellite velocity/clk-rate std (m/s|s/s) */ + float cov[MAXSAT][3]; /* satellite position covariance (m^2) */ + float vco[MAXSAT][3]; /* satellite velocity covariance (m^2) */ +} peph_t; + +typedef struct { /* precise clock type */ + gtime_t time; /* time (GPST) */ + int index; /* clock index for multiple files */ + double clk[MAXSAT][1]; /* satellite clock (s) */ + float std[MAXSAT][1]; /* satellite clock std (s) */ +} pclk_t; + +typedef struct { /* SBAS ephemeris type */ + int sat; /* satellite number */ + gtime_t t0; /* reference epoch time (GPST) */ + gtime_t tof; /* time of message frame (GPST) */ + int sva; /* SV accuracy (URA index) */ + int svh; /* SV health (0:ok) */ + double pos[3]; /* satellite position (m) (ecef) */ + double vel[3]; /* satellite velocity (m/s) (ecef) */ + double acc[3]; /* satellite acceleration (m/s^2) (ecef) */ + double af0,af1; /* satellite clock-offset/drift (s,s/s) */ +} seph_t; + +typedef struct { /* norad two line element data type */ + char name [32]; /* common name */ + char alias[32]; /* alias name */ + char satno[16]; /* satellilte catalog number */ + char satclass; /* classification */ + char desig[16]; /* international designator */ + gtime_t epoch; /* element set epoch (UTC) */ + double ndot; /* 1st derivative of mean motion */ + double nddot; /* 2st derivative of mean motion */ + double bstar; /* B* drag term */ + int etype; /* element set type */ + int eleno; /* element number */ + double inc; /* orbit inclination (deg) */ + double OMG; /* right ascension of ascending node (deg) */ + double ecc; /* eccentricity */ + double omg; /* argument of perigee (deg) */ + double M; /* mean anomaly (deg) */ + double n; /* mean motion (rev/day) */ + int rev; /* revolution number at epoch */ +} tled_t; + +typedef struct { /* norad two line element type */ + int n,nmax; /* number/max number of two line element data */ + tled_t *data; /* norad two line element data */ +} tle_t; + +typedef struct { /* TEC grid type */ + gtime_t time; /* epoch time (GPST) */ + int ndata[3]; /* TEC grid data size {nlat,nlon,nhgt} */ + double rb; /* earth radius (km) */ + double lats[3]; /* latitude start/interval (deg) */ + double lons[3]; /* longitude start/interval (deg) */ + double hgts[3]; /* heights start/interval (km) */ + double *data; /* TEC grid data (tecu) */ + float *rms; /* RMS values (tecu) */ +} tec_t; + +typedef struct { /* satellite fcb data type */ + gtime_t ts,te; /* start/end time (GPST) */ + double bias[MAXSAT][3]; /* fcb value (cyc) */ + double std [MAXSAT][3]; /* fcb std-dev (cyc) */ +} fcbd_t; + +typedef struct { /* SBAS message type */ + int week,tow; /* receiption time */ + int prn; /* SBAS satellite PRN number */ + unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ +} sbsmsg_t; + +typedef struct { /* SBAS messages type */ + int n,nmax; /* number of SBAS messages/allocated */ + sbsmsg_t *msgs; /* SBAS messages */ +} sbs_t; + +typedef struct { /* SBAS fast correction type */ + gtime_t t0; /* time of applicability (TOF) */ + double prc; /* pseudorange correction (PRC) (m) */ + double rrc; /* range-rate correction (RRC) (m/s) */ + double dt; /* range-rate correction delta-time (s) */ + int iodf; /* IODF (issue of date fast corr) */ + short udre; /* UDRE+1 */ + short ai; /* degradation factor indicator */ +} sbsfcorr_t; + +typedef struct { /* SBAS long term satellite error correction type */ + gtime_t t0; /* correction time */ + int iode; /* IODE (issue of date ephemeris) */ + double dpos[3]; /* delta position (m) (ecef) */ + double dvel[3]; /* delta velocity (m/s) (ecef) */ + double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ +} sbslcorr_t; + +typedef struct { /* SBAS satellite correction type */ + int sat; /* satellite number */ + sbsfcorr_t fcorr; /* fast correction */ + sbslcorr_t lcorr; /* long term correction */ +} sbssatp_t; + +typedef struct { /* SBAS satellite corrections type */ + int iodp; /* IODP (issue of date mask) */ + int nsat; /* number of satellites */ + int tlat; /* system latency (s) */ + sbssatp_t sat[MAXSAT]; /* satellite correction */ +} sbssat_t; + +typedef struct { /* SBAS ionospheric correction type */ + gtime_t t0; /* correction time */ + short lat,lon; /* latitude/longitude (deg) */ + short give; /* GIVI+1 */ + float delay; /* vertical delay estimate (m) */ +} sbsigp_t; + +typedef struct { /* IGP band type */ + short x; /* longitude/latitude (deg) */ + const short *y; /* latitudes/longitudes (deg) */ + unsigned char bits; /* IGP mask start bit */ + unsigned char bite; /* IGP mask end bit */ +} sbsigpband_t; + +typedef struct { /* SBAS ionospheric corrections type */ + int iodi; /* IODI (issue of date ionos corr) */ + int nigp; /* number of igps */ + sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ +} sbsion_t; + +typedef struct { /* DGPS/GNSS correction type */ + gtime_t t0; /* correction time */ + double prc; /* pseudorange correction (PRC) (m) */ + double rrc; /* range rate correction (RRC) (m/s) */ + int iod; /* issue of data (IOD) */ + double udre; /* UDRE */ +} dgps_t; + +typedef struct { /* SSR correction type */ + gtime_t t0[6]; /* epoch time (GPST) {eph,clk,hrclk,ura,bias,pbias} */ + double udi[6]; /* SSR update interval (s) */ + int iod[6]; /* iod ssr {eph,clk,hrclk,ura,bias,pbias} */ + int iode; /* issue of data */ + int iodcrc; /* issue of data crc for beidou/sbas */ + int ura; /* URA indicator */ + int refd; /* sat ref datum (0:ITRF,1:regional) */ + double deph [3]; /* delta orbit {radial,along,cross} (m) */ + double ddeph[3]; /* dot delta orbit {radial,along,cross} (m/s) */ + double dclk [3]; /* delta clock {c0,c1,c2} (m,m/s,m/s^2) */ + double hrclk; /* high-rate clock corection (m) */ + float cbias[MAXCODE]; /* code biases (m) */ + double pbias[MAXCODE]; /* phase biases (m) */ + float stdpb[MAXCODE]; /* std-dev of phase biases (m) */ + double yaw_ang,yaw_rate; /* yaw angle and yaw rate (deg,deg/s) */ + unsigned char update; /* update flag (0:no update,1:update) */ +} ssr_t; + +typedef struct { /* QZSS LEX message type */ + int prn; /* satellite PRN number */ + int type; /* message type */ + int alert; /* alert flag */ + unsigned char stat; /* signal tracking status */ + unsigned char snr; /* signal C/N0 (0.25 dBHz) */ + unsigned int ttt; /* tracking time (ms) */ + unsigned char msg[212]; /* LEX message data part 1695 bits */ +} lexmsg_t; + +typedef struct { /* QZSS LEX messages type */ + int n,nmax; /* number of LEX messages and allocated */ + lexmsg_t *msgs; /* LEX messages */ +} lex_t; + +typedef struct { /* QZSS LEX ephemeris type */ + gtime_t toe; /* epoch time (GPST) */ + gtime_t tof; /* message frame time (GPST) */ + int sat; /* satellite number */ + unsigned char health; /* signal health (L1,L2,L1C,L5,LEX) */ + unsigned char ura; /* URA index */ + double pos[3]; /* satellite position (m) */ + double vel[3]; /* satellite velocity (m/s) */ + double acc[3]; /* satellite acceleration (m/s2) */ + double jerk[3]; /* satellite jerk (m/s3) */ + double af0,af1; /* satellite clock bias and drift (s,s/s) */ + double tgd; /* TGD */ + double isc[8]; /* ISC */ +} lexeph_t; + +typedef struct { /* QZSS LEX ionosphere correction type */ + gtime_t t0; /* epoch time (GPST) */ + double tspan; /* valid time span (s) */ + double pos0[2]; /* reference position {lat,lon} (rad) */ + double coef[3][2]; /* coefficients lat x lon (3 x 2) */ +} lexion_t; + +typedef struct { /* stec data type */ + gtime_t time; /* time (GPST) */ + unsigned char sat; /* satellite number */ + double ion; /* slant ionos delay (m) */ + float std; /* std-dev (m) */ + float azel[2]; /* azimuth/elevation (rad) */ + unsigned char flag; /* fix flag */ +} stec_t; + +typedef struct { /* trop data type */ + gtime_t time; /* time (GPST) */ + double trp[3]; /* zenith tropos delay/gradient (m) */ + float std[3]; /* std-dev (m) */ +} trop_t; + +typedef struct { /* ppp corrections type */ + int nsta; /* number of stations */ + char stas[MAXSTA][8]; /* station names */ + double rr[MAXSTA][3]; /* station ecef positions (m) */ + int ns[MAXSTA],nsmax[MAXSTA]; /* number of stec data */ + int nt[MAXSTA],ntmax[MAXSTA]; /* number of trop data */ + stec_t *stec[MAXSTA]; /* stec data */ + trop_t *trop[MAXSTA]; /* trop data */ +} pppcorr_t; + +typedef struct { /* navigation data type */ + int n,nmax; /* number of broadcast ephemeris */ + int ng,ngmax; /* number of glonass ephemeris */ + int ns,nsmax; /* number of sbas ephemeris */ + int ne,nemax; /* number of precise ephemeris */ + int nc,ncmax; /* number of precise clock */ + int na,namax; /* number of almanac data */ + int nt,ntmax; /* number of tec grid data */ + int nf,nfmax; /* number of satellite fcb data */ + eph_t *eph; /* GPS/QZS/GAL ephemeris */ + geph_t *geph; /* GLONASS ephemeris */ + seph_t *seph; /* SBAS ephemeris */ + peph_t *peph; /* precise ephemeris */ + pclk_t *pclk; /* precise clock */ + alm_t *alm; /* almanac data */ + tec_t *tec; /* tec grid data */ + fcbd_t *fcb; /* satellite fcb data */ + erp_t erp; /* earth rotation parameters */ + double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */ + double utc_glo[4]; /* GLONASS UTC GPS time parameters */ + double utc_gal[4]; /* Galileo UTC GPS time parameters */ + double utc_qzs[4]; /* QZS UTC GPS time parameters */ + double utc_cmp[4]; /* BeiDou UTC parameters */ + double utc_irn[4]; /* IRNSS UTC parameters */ + double utc_sbs[4]; /* SBAS UTC parameters */ + double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */ + double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + int leaps; /* leap seconds (s) */ + double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */ + double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ + double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ + double wlbias[MAXSAT]; /* wide-lane bias (cycle) */ + double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */ + char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */ + pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ + sbssat_t sbssat; /* SBAS satellite corrections */ + sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */ + dgps_t dgps[MAXSAT]; /* DGPS corrections */ + ssr_t ssr[MAXSAT]; /* SSR corrections */ + lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */ + lexion_t lexion; /* LEX ionosphere correction */ + pppcorr_t pppcorr; /* ppp corrections */ +} nav_t; + +typedef struct { /* station parameter type */ + char name [MAXANT]; /* marker name */ + char marker [MAXANT]; /* marker number */ + char antdes [MAXANT]; /* antenna descriptor */ + char antsno [MAXANT]; /* antenna serial number */ + char rectype[MAXANT]; /* receiver type descriptor */ + char recver [MAXANT]; /* receiver firmware version */ + char recsno [MAXANT]; /* receiver serial number */ + int antsetup; /* antenna setup id */ + int itrf; /* ITRF realization year */ + int deltype; /* antenna delta type (0:enu,1:xyz) */ + double pos[3]; /* station position (ecef) (m) */ + double del[3]; /* antenna position delta (e/n/u or x/y/z) (m) */ + double hgt; /* antenna height (m) */ +} sta_t; + +typedef struct { /* solution type */ + gtime_t time; /* time (GPST) */ + double rr[6]; /* position/velocity (m|m/s) */ + /* {x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ + float qr[6]; /* position variance/covariance (m^2) */ + /* {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} or */ + /* {c_ee,c_nn,c_uu,c_en,c_nu,c_ue} */ + double dtr[6]; /* receiver clock bias to time systems (s) */ + unsigned char type; /* type (0:xyz-ecef,1:enu-baseline) */ + unsigned char stat; /* solution status (SOLQ_???) */ + unsigned char ns; /* number of valid satellites */ + float age; /* age of differential (s) */ + float ratio; /* AR ratio factor for valiation */ + float thres; /* AR ratio threshold for valiation */ +} sol_t; + +typedef struct { /* solution buffer type */ + int n,nmax; /* number of solution/max number of buffer */ + int cyclic; /* cyclic buffer flag */ + int start,end; /* start/end index */ + gtime_t time; /* current solution time */ + sol_t *data; /* solution data */ + double rb[3]; /* reference position {x,y,z} (ecef) (m) */ + unsigned char buff[MAXSOLMSG+1]; /* message buffer */ + int nb; /* number of byte in message buffer */ +} solbuf_t; + +typedef struct { /* solution status type */ + gtime_t time; /* time (GPST) */ + unsigned char sat; /* satellite number */ + unsigned char frq; /* frequency (1:L1,2:L2,...) */ + float az,el; /* azimuth/elevation angle (rad) */ + float resp; /* pseudorange residual (m) */ + float resc; /* carrier-phase residual (m) */ + unsigned char flag; /* flags: (vsat<<5)+(slip<<3)+fix */ + unsigned char snr; /* signal strength (0.25 dBHz) */ + unsigned short lock; /* lock counter */ + unsigned short outc; /* outage counter */ + unsigned short slipc; /* slip counter */ + unsigned short rejc; /* reject counter */ +} solstat_t; + +typedef struct { /* solution status buffer type */ + int n,nmax; /* number of solution/max number of buffer */ + solstat_t *data; /* solution status data */ +} solstatbuf_t; + +typedef struct { /* RTCM control struct type */ + int staid; /* station id */ + int stah; /* station health */ + int seqno; /* sequence number for rtcm 2 or iods msm */ + int outtype; /* output message type */ + gtime_t time; /* message time */ + gtime_t time_s; /* message start time */ + obs_t obs; /* observation data (uncorrected) */ + nav_t nav; /* satellite ephemerides */ + sta_t sta; /* station parameters */ + dgps_t *dgps; /* output of dgps corrections */ + ssr_t ssr[MAXSAT]; /* output of ssr corrections */ + char msg[128]; /* special message */ + char msgtype[256]; /* last message type */ + char msmtype[6][128]; /* msm signal types */ + int obsflag; /* obs data complete flag (1:ok,0:not complete) */ + int ephsat; /* update satellite of ephemeris */ + double cp[MAXSAT][NFREQ+NEXOBS]; /* carrier-phase measurement */ + unsigned short lock[MAXSAT][NFREQ+NEXOBS]; /* lock time */ + unsigned short loss[MAXSAT][NFREQ+NEXOBS]; /* loss of lock count */ + gtime_t lltime[MAXSAT][NFREQ+NEXOBS]; /* last lock time */ + int nbyte; /* number of bytes in message buffer */ + int nbit; /* number of bits in word buffer */ + int len; /* message length (bytes) */ + unsigned char buff[1200]; /* message buffer */ + unsigned int word; /* word buffer for rtcm 2 */ + unsigned int nmsg2[100]; /* message count of RTCM 2 (1-99:1-99,0:other) */ + unsigned int nmsg3[400]; /* message count of RTCM 3 (1-299:1001-1299,300-399:2000-2099,0:ohter) */ + char opt[256]; /* RTCM dependent options */ +} rtcm_t; + +typedef struct { /* download url type */ + char type[32]; /* data type */ + char path[1024]; /* url path */ + char dir [1024]; /* local directory */ + double tint; /* time interval (s) */ +} url_t; + +typedef struct { /* option type */ + const char *name; /* option name */ + int format; /* option format (0:int,1:double,2:string,3:enum) */ + void *var; /* pointer to option variable */ + const char *comment; /* option comment/enum labels/unit */ +} opt_t; + +typedef struct { /* extended receiver error model */ + int ena[4]; /* model enabled */ + double cerr[4][NFREQ*2]; /* code errors (m) */ + double perr[4][NFREQ*2]; /* carrier-phase errors (m) */ + double gpsglob[NFREQ]; /* gps-glonass h/w bias (m) */ + double gloicb [NFREQ]; /* glonass interchannel bias (m/fn) */ +} exterr_t; + +typedef struct { /* SNR mask type */ + int ena[2]; /* enable flag {rover,base} */ + double mask[NFREQ][9]; /* mask (dBHz) at 5,10,...85 deg */ +} snrmask_t; + +typedef struct { /* processing options type */ + int mode; /* positioning mode (PMODE_???) */ + int soltype; /* solution type (0:forward,1:backward,2:combined) */ + int nf; /* number of frequencies (1:L1,2:L1+L2,3:L1+L2+L5) */ + int navsys; /* navigation system */ + double elmin; /* elevation mask angle (rad) */ + snrmask_t snrmask; /* SNR mask */ + int sateph; /* satellite ephemeris/clock (EPHOPT_???) */ + int modear; /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ + int glomodear; /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */ + int bdsmodear; /* BeiDou AR mode (0:off,1:on) */ + int maxout; /* obs outage count to reset bias */ + int minlock; /* min lock count to fix ambiguity */ + int minfix; /* min fix count to hold ambiguity */ + int armaxiter; /* max iteration to resolve ambiguity */ + int ionoopt; /* ionosphere option (IONOOPT_???) */ + int tropopt; /* troposphere option (TROPOPT_???) */ + int dynamics; /* dynamics model (0:none,1:velociy,2:accel) */ + int tidecorr; /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */ + int niter; /* number of filter iteration */ + int codesmooth; /* code smoothing window size (0:none) */ + int intpref; /* interpolate reference obs (for post mission) */ + int sbascorr; /* SBAS correction options */ + int sbassatsel; /* SBAS satellite selection (0:all) */ + int rovpos; /* rover position for fixed mode */ + int refpos; /* base position for relative mode */ + /* (0:pos in prcopt, 1:average of single pos, */ + /* 2:read from file, 3:rinex header, 4:rtcm pos) */ + double eratio[NFREQ]; /* code/phase error ratio */ + double err[5]; /* measurement error factor */ + /* [0]:reserved */ + /* [1-3]:error factor a/b/c of phase (m) */ + /* [4]:doppler frequency (hz) */ + double std[3]; /* initial-state std [0]bias,[1]iono [2]trop */ + double prn[6]; /* process-noise std [0]bias,[1]iono [2]trop [3]acch [4]accv [5] pos */ + double sclkstab; /* satellite clock stability (sec/sec) */ + double thresar[8]; /* AR validation threshold */ + double elmaskar; /* elevation mask of AR for rising satellite (deg) */ + double elmaskhold; /* elevation mask to hold ambiguity (deg) */ + double thresslip; /* slip threshold of geometry-free phase (m) */ + double maxtdiff; /* max difference of time (sec) */ + double maxinno; /* reject threshold of innovation (m) */ + double maxgdop; /* 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) */ + int maxaveep; /* max averaging epoches */ + int initrst; /* initialize by restart */ + int outsingle; /* output single by dgps/float/fix/ppp outage */ + char rnxopt[2][256]; /* rinex options {rover,base} */ + int posopt[6]; /* positioning options */ + int syncsol; /* solution sync mode (0:off,1:on) */ + double odisp[2][6*11]; /* ocean tide loading parameters {rov,base} */ + exterr_t exterr; /* extended receiver error model */ + int freqopt; /* disable L2-AR */ + char pppopt[256]; /* ppp option */ +} prcopt_t; + +typedef struct { /* solution options type */ + int posf; /* solution format (SOLF_???) */ + int times; /* time system (TIMES_???) */ + int timef; /* time format (0:sssss.s,1:yyyy/mm/dd hh:mm:ss.s) */ + int timeu; /* time digits under decimal point */ + int degf; /* latitude/longitude format (0:ddd.ddd,1:ddd mm ss) */ + int outhead; /* output header (0:no,1:yes) */ + int outopt; /* output processing options (0:no,1:yes) */ + int datum; /* datum (0:WGS84,1:Tokyo) */ + int height; /* height (0:ellipsoidal,1:geodetic) */ + int geoid; /* geoid model (0:EGM96,1:JGD2000) */ + int solstatic; /* solution of static mode (0:all,1:single) */ + int sstat; /* solution statistics level (0:off,1:states,2:residuals) */ + int trace; /* debug trace level (0:off,1-5:debug) */ + double nmeaintv[2]; /* nmea output interval (s) (<0:no,0:all) */ + /* nmeaintv[0]:gprmc,gpgga,nmeaintv[1]:gpgsv */ + char sep[64]; /* field separator */ + char prog[64]; /* program name */ + double maxsolstd; /* max std-dev for solution output (m) (0:all) */ +} solopt_t; + + + +typedef struct { /* satellite status type */ + unsigned char sys; /* navigation system */ + unsigned char vs; /* valid satellite flag single */ + double azel[2]; /* azimuth/elevation angles {az,el} (rad) */ + double resp[NFREQ]; /* residuals of pseudorange (m) */ + double resc[NFREQ]; /* residuals of carrier-phase (m) */ + unsigned char vsat[NFREQ]; /* valid satellite flag */ + unsigned char snr [NFREQ]; /* signal strength (0.25 dBHz) */ + unsigned char fix [NFREQ]; /* ambiguity fix flag (1:fix,2:float,3:hold) */ + unsigned char slip[NFREQ]; /* cycle-slip flag */ + unsigned char half[NFREQ]; /* half-cycle valid flag */ + int lock [NFREQ]; /* lock counter of phase */ + unsigned int outc [NFREQ]; /* obs outage counter of phase */ + unsigned int slipc[NFREQ]; /* cycle-slip counter */ + unsigned int rejc [NFREQ]; /* reject counter */ + double gf; /* geometry-free phase L1-L2 (m) */ + double gf2; /* geometry-free phase L1-L5 (m) */ + double mw; /* MW-LC (m) */ + double phw; /* phase windup (cycle) */ + gtime_t pt[2][NFREQ]; /* previous carrier-phase time */ + double ph[2][NFREQ]; /* previous carrier-phase observable (cycle) */ +} ssat_t; + +typedef struct { /* ambiguity control type */ + gtime_t epoch[4]; /* last epoch */ + int n[4]; /* number of epochs */ + double LC [4]; /* linear combination average */ + double LCv[4]; /* linear combination variance */ + int fixcnt; /* fix count */ + char flags[MAXSAT]; /* fix flags */ +} ambc_t; + +typedef struct { /* RTK control/result type */ + sol_t sol; /* RTK solution */ + double rb[6]; /* base position/velocity (ecef) (m|m/s) */ + int nx,na; /* number of float states/fixed states */ + double tt; /* time difference between current and previous (s) */ + double *x, *P; /* float states and their covariance */ + double *xa,*Pa; /* fixed states and their covariance */ + int nfix; /* number of continuous fixes of ambiguity */ + ambc_t ambc[MAXSAT]; /* ambibuity control */ + ssat_t ssat[MAXSAT]; /* satellite status */ + int neb; /* bytes in error message buffer */ + char errbuf[MAXERRMSG]; /* error message buffer */ + prcopt_t opt; /* processing options */ +} rtk_t; + +typedef struct half_cyc_tag { /* half-cycle correction list type */ + unsigned char sat; /* satellite number */ + unsigned char freq; /* frequency number (0:L1,1:L2,2:L5) */ + unsigned char valid; /* half-cycle valid flag */ + char corr; /* half-cycle corrected (x 0.5 cyc) */ + gtime_t ts,te; /* time start, time end */ + struct half_cyc_tag *next; /* pointer to next correction */ +} half_cyc_t; + + +const double chisqr[100]={ /* chi-sqr(n) (alpha=0.001) */ + 10.8,13.8,16.3,18.5,20.5,22.5,24.3,26.1,27.9,29.6, + 31.3,32.9,34.5,36.1,37.7,39.3,40.8,42.3,43.8,45.3, + 46.8,48.3,49.7,51.2,52.6,54.1,55.5,56.9,58.3,59.7, + 61.1,62.5,63.9,65.2,66.6,68.0,69.3,70.7,72.1,73.4, + 74.7,76.0,77.3,78.6,80.0,81.3,82.6,84.0,85.4,86.7, + 88.0,89.3,90.6,91.9,93.3,94.7,96.0,97.4,98.7,100 , + 101 ,102 ,103 ,104 ,105 ,107 ,108 ,109 ,110 ,112 , + 113 ,114 ,115 ,116 ,118 ,119 ,120 ,122 ,123 ,125 , + 126 ,127 ,128 ,129 ,131 ,132 ,133 ,134 ,135 ,137 , + 138 ,139 ,140 ,142 ,143 ,144 ,145 ,147 ,148 ,149 +}; +const double lam_carr[MAXFREQ]={ /* carrier wave length (m) */ + CLIGHT/FREQ1,CLIGHT/FREQ2,CLIGHT/FREQ5,CLIGHT/FREQ6,CLIGHT/FREQ7, + CLIGHT/FREQ8,CLIGHT/FREQ9 +}; + +#endif diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc new file mode 100644 index 000000000..12d3108f2 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -0,0 +1,210 @@ +/*! + * \file rtklib_conversions.cc + * \brief GNSS-SDR to RTKLIB data structures conversion functions + * \authors
    + *
  • 2017, Javier Arribas + *
  • 2017, 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ +#include "rtklib_conversions.h" + + +obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week) +{ + obsd_t rtklib_obs; + rtklib_obs.D[0]=gnss_synchro.Carrier_Doppler_hz; + rtklib_obs.P[0]=gnss_synchro.Pseudorange_m; + rtklib_obs.L[0]=gnss_synchro.Carrier_phase_rads;//todo: check units + //rtklib_obs.SNR=gnss_synchro.CN0_dB_hz; + rtklib_obs.sat=gnss_synchro.PRN; + rtklib_obs.time=gpst2time(adjgpsweek(week),gnss_synchro.RX_time); + //printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec); + return rtklib_obs; +} +eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph) +{ + eph_t rtklib_sat; + rtklib_sat.sat = gal_eph.i_satellite_PRN; + rtklib_sat.A=gal_eph.A_1*gal_eph.A_1; + rtklib_sat.M0=gal_eph.M0_1; + rtklib_sat.deln=gal_eph.delta_n_3; + rtklib_sat.OMG0=gal_eph.OMEGA_0_2; + rtklib_sat.OMGd=gal_eph.OMEGA_dot_3; + rtklib_sat.omg=gal_eph.omega_2; + rtklib_sat.i0=gal_eph.i_0_2; + rtklib_sat.idot=gal_eph.iDot_2; + rtklib_sat.e=gal_eph.e_1; + rtklib_sat.Adot=0; //only in CNAV; + rtklib_sat.ndot=0; //only in CNAV; + + rtklib_sat.week=adjgpsweek(gal_eph.WN_5); /* week of tow */ + rtklib_sat.cic=gal_eph.C_ic_4; + rtklib_sat.cis=gal_eph.C_is_4; + rtklib_sat.cuc=gal_eph.C_uc_3; + rtklib_sat.cus=gal_eph.C_us_3; + rtklib_sat.crc=gal_eph.C_rc_3; + rtklib_sat.crs=gal_eph.C_rs_3; + rtklib_sat.f0=gal_eph.af0_4; + rtklib_sat.f1=gal_eph.af1_4; + rtklib_sat.f2=gal_eph.af2_4; + rtklib_sat.tgd[0]=0; + rtklib_sat.tgd[1]=0; + rtklib_sat.tgd[2]=0; + rtklib_sat.tgd[3]=0; + rtklib_sat.toes=gal_eph.t0e_1; + rtklib_sat.toc=gpst2time(rtklib_sat.week,gal_eph.t0c_4); + rtklib_sat.ttr=gpst2time(rtklib_sat.week,gal_eph.TOW_5); + + /* adjustment for week handover */ + double tow, toc; + tow=time2gpst(rtklib_sat.ttr,&rtklib_sat.week); + toc=time2gpst(rtklib_sat.toc,NULL); + if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} + rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); + rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); + rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + + return rtklib_sat; +} + +eph_t eph_to_rtklib(Gps_Ephemeris gps_eph) +{ + eph_t rtklib_sat; + rtklib_sat.sat = gps_eph.i_satellite_PRN; + rtklib_sat.A=gps_eph.d_sqrt_A*gps_eph.d_sqrt_A; + rtklib_sat.M0=gps_eph.d_M_0; + rtklib_sat.deln=gps_eph.d_Delta_n; + rtklib_sat.OMG0=gps_eph.d_OMEGA0; + rtklib_sat.OMGd=gps_eph.d_OMEGA_DOT; + rtklib_sat.omg=gps_eph.d_OMEGA; + rtklib_sat.i0=gps_eph.d_i_0; + rtklib_sat.idot=gps_eph.d_IDOT; + rtklib_sat.e=gps_eph.d_e_eccentricity; + rtklib_sat.Adot=0; //only in CNAV; + rtklib_sat.ndot=0; //only in CNAV; + + rtklib_sat.week=adjgpsweek(gps_eph.i_GPS_week); /* week of tow */ + rtklib_sat.cic=gps_eph.d_Cic; + rtklib_sat.cis=gps_eph.d_Cis; + rtklib_sat.cuc=gps_eph.d_Cuc; + rtklib_sat.cus=gps_eph.d_Cus; + rtklib_sat.crc=gps_eph.d_Crc; + rtklib_sat.crs=gps_eph.d_Crs; + rtklib_sat.f0=gps_eph.d_A_f0; + rtklib_sat.f1=gps_eph.d_A_f1; + rtklib_sat.f2=gps_eph.d_A_f2; + rtklib_sat.tgd[0]=gps_eph.d_TGD; + rtklib_sat.tgd[1]=0; + rtklib_sat.tgd[2]=0; + rtklib_sat.tgd[3]=0; + rtklib_sat.toes=gps_eph.d_Toe; + rtklib_sat.toc=gpst2time(rtklib_sat.week,gps_eph.d_Toc); + rtklib_sat.ttr=gpst2time(rtklib_sat.week,gps_eph.d_TOW); + + /* adjustment for week handover */ + double tow, toc; + tow=time2gpst(rtklib_sat.ttr,&rtklib_sat.week); + toc=time2gpst(rtklib_sat.toc,NULL); + if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} + rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); + rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); + rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + + //printf("EPHEMERIS TIME [%i]: %s,%f\n\r",rtklib_sat.sat,time_str(rtklib_sat.toe,3),rtklib_sat.toe.sec); + + return rtklib_sat; +} +eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph) +{ + eph_t rtklib_sat; + rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; + const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 + rtklib_sat.A=A_REF + gps_cnav_eph.d_DELTA_A; + rtklib_sat.M0=gps_cnav_eph.d_M_0; + rtklib_sat.deln=gps_cnav_eph.d_Delta_n; + rtklib_sat.OMG0=gps_cnav_eph.d_OMEGA0; + // Compute the angle between the ascending node and the Greenwich meridian + const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164 + double d_OMEGA_DOT = OMEGA_DOT_REF*GPS_L2_PI + gps_cnav_eph.d_DELTA_OMEGA_DOT; + rtklib_sat.OMGd=d_OMEGA_DOT; + rtklib_sat.omg=gps_cnav_eph.d_OMEGA; + rtklib_sat.i0=gps_cnav_eph.d_i_0; + rtklib_sat.idot=gps_cnav_eph.d_IDOT; + rtklib_sat.e=gps_cnav_eph.d_e_eccentricity; + rtklib_sat.Adot=gps_cnav_eph.d_A_DOT; //only in CNAV; + rtklib_sat.ndot=gps_cnav_eph.d_DELTA_DOT_N; //only in CNAV; + + rtklib_sat.week=adjgpsweek(gps_cnav_eph.i_GPS_week); /* week of tow */ + rtklib_sat.cic=gps_cnav_eph.d_Cic; + rtklib_sat.cis=gps_cnav_eph.d_Cis; + rtklib_sat.cuc=gps_cnav_eph.d_Cuc; + rtklib_sat.cus=gps_cnav_eph.d_Cus; + rtklib_sat.crc=gps_cnav_eph.d_Crc; + rtklib_sat.crs=gps_cnav_eph.d_Crs; + rtklib_sat.f0=gps_cnav_eph.d_A_f0; + rtklib_sat.f1=gps_cnav_eph.d_A_f1; + rtklib_sat.f2=gps_cnav_eph.d_A_f2; + rtklib_sat.tgd[0]=gps_cnav_eph.d_TGD; + rtklib_sat.tgd[1]=0; + rtklib_sat.tgd[2]=0; + rtklib_sat.tgd[3]=0; + rtklib_sat.toes=gps_cnav_eph.d_Toe1; + rtklib_sat.toc=gpst2time(rtklib_sat.week,gps_cnav_eph.d_Toc); + rtklib_sat.ttr=gpst2time(rtklib_sat.week,gps_cnav_eph.d_TOW); + + /* adjustment for week handover */ + double tow, toc; + tow=time2gpst(rtklib_sat.ttr,&rtklib_sat.week); + toc=time2gpst(rtklib_sat.toc,NULL); + if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} + rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); + rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); + rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + + return rtklib_sat; + +} diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h new file mode 100644 index 000000000..4a9fd2374 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -0,0 +1,65 @@ +/*! + * \file rtklib_conversions.h + * \brief GNSS-SDR to RTKLIB data structures conversion functions + * \authors
    + *
  • 2017, Javier Arribas + *
  • 2017, 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ +#ifndef SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ +#define SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ + +#include "rtklib_rtkcmn.h" +#include "gnss_synchro.h" +#include "galileo_ephemeris.h" +#include "gps_ephemeris.h" +#include "gps_cnav_ephemeris.h" + +eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph); +eph_t eph_to_rtklib(Gps_Ephemeris gps_eph); +eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph); + +obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week); + +#endif /* SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc new file mode 100644 index 000000000..02e671fb2 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -0,0 +1,797 @@ +/*! + * \file rtklib_ephemeris.cc + * \brief satellite ephemeris and clock functions + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +* history : 2010/07/28 1.1 moved from rtkcmn.c +* added api: +* eph2clk(),geph2clk(),seph2clk(),satantoff() +* satposs() +* changed api: +* eph2pos(),geph2pos(),satpos() +* deleted api: +* satposv(),satposiode() +* 2010/08/26 1.2 add ephemeris option EPHOPT_LEX +* 2010/09/09 1.3 fix problem when precise clock outage +* 2011/01/12 1.4 add api alm2pos() +* change api satpos(),satposs() +* enable valid unhealthy satellites and output status +* fix bug on exception by glonass ephem computation +* 2013/01/10 1.5 support beidou (compass) +* use newton's method to solve kepler eq. +* update ssr correction algorithm +* 2013/03/20 1.6 fix problem on ssr clock relativitic correction +* 2013/09/01 1.7 support negative pseudorange +* fix bug on variance in case of ura ssr = 63 +* 2013/11/11 1.8 change constant MAXAGESSR 70.0 -> 90.0 +* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15 1E-13 +* set MAX_ITER_KEPLER for alm2pos() +*-----------------------------------------------------------------------------*/ + +#include "rtklib_ephemeris.h" + +static const char rcsid[]="$Id:$"; + +/* constants and macros ------------------------------------------------------*/ + +#define SQR(x) ((x)*(x)) + +#define RE_GLO 6378136.0 /* radius of earth (m) ref [2] */ +#define MU_GPS 3.9860050E14 /* gravitational constant ref [1] */ +#define MU_GLO 3.9860044E14 /* gravitational constant ref [2] */ +#define MU_GAL 3.986004418E14 /* earth gravitational constant ref [7] */ +#define MU_CMP 3.986004418E14 /* earth gravitational constant ref [9] */ +#define J2_GLO 1.0826257E-3 /* 2nd zonal harmonic of geopot ref [2] */ + +#define OMGE_GLO 7.292115E-5 /* earth angular velocity (rad/s) ref [2] */ +#define OMGE_GAL 7.2921151467E-5 /* earth angular velocity (rad/s) ref [7] */ +#define OMGE_CMP 7.292115E-5 /* earth angular velocity (rad/s) ref [9] */ + +#define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */ +#define COS_5 0.9961946980917456 /* cos(-5.0 deg) */ + +#define ERREPH_GLO 5.0 /* error of glonass ephemeris (m) */ +#define TSTEP 60.0 /* integration step glonass ephemeris (s) */ +#define RTOL_KEPLER 1E-13 /* relative tolerance for Kepler equation */ + +#define DEFURASSR 0.15 /* default accurary of ssr corr (m) */ +#define MAXECORSSR 10.0 /* max orbit correction of ssr (m) */ +#define MAXCCORSSR (1E-6*CLIGHT) /* max clock correction of ssr (m) */ +#define MAXAGESSR 90.0 /* max age of ssr orbit and clock (s) */ +#define MAXAGESSR_HRCLK 10.0 /* max age of ssr high-rate clock (s) */ +#define STD_BRDCCLK 30.0 /* error of broadcast clock (m) */ + +#define MAX_ITER_KEPLER 30 /* max number of iteration of Kelpler */ + +/* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/ +double var_uraeph(int ura) +{ + const double ura_value[]={ + 2.4,3.4,4.85,6.85,9.65,13.65,24.0,48.0,96.0,192.0,384.0,768.0,1536.0, + 3072.0,6144.0 + }; + return ura<0||15=63) return SQR(5.4665); + std=(pow(3.0,(ura>>3)&7)*(1.0+(ura&7)/4.0)-1.0)*1E-3; + return SQR(std); +} +/* almanac to satellite position and clock bias -------------------------------- +* compute satellite position and clock bias with almanac (gps, galileo, qzss) +* args : gtime_t time I time (gpst) +* alm_t *alm I almanac +* double *rs O satellite position (ecef) {x,y,z} (m) +* double *dts O satellite clock bias (s) +* return : none +* notes : see ref [1],[7],[8] +*-----------------------------------------------------------------------------*/ +void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts) +{ + double tk,M,E,Ek,sinE,cosE,u,r,i,O,x,y,sinO,cosO,cosi,mu; + int n; + + trace(4,"alm2pos : time=%s sat=%2d\n",time_str(time,3),alm->sat); + + tk=timediff(time,alm->toa); + + if (alm->A<=0.0) { + rs[0]=rs[1]=rs[2]=*dts=0.0; + return; + } + mu=satsys(alm->sat,NULL)==SYS_GAL?MU_GAL:MU_GPS; + + M=alm->M0+sqrt(mu/(alm->A*alm->A*alm->A))*tk; + for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-alm->e*cos(E)); + } + if (n>=MAX_ITER_KEPLER) { + trace(2,"alm2pos: kepler iteration overflow sat=%2d\n",alm->sat); + return; + } + sinE=sin(E); cosE=cos(E); + u=atan2(sqrt(1.0-alm->e*alm->e)*sinE,cosE-alm->e)+alm->omg; + r=alm->A*(1.0-alm->e*cosE); + i=alm->i0; + O=alm->OMG0+(alm->OMGd-OMGE)*tk-OMGE*alm->toas; + x=r*cos(u); y=r*sin(u); sinO=sin(O); cosO=cos(O); cosi=cos(i); + rs[0]=x*cosO-y*cosi*sinO; + rs[1]=x*sinO+y*cosi*cosO; + rs[2]=y*sin(i); + *dts=alm->f0+alm->f1*tk; +} +/* broadcast ephemeris to satellite clock bias --------------------------------- +* compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss) +* args : gtime_t time I time by satellite clock (gpst) +* eph_t *eph I broadcast ephemeris +* return : satellite clock bias (s) without relativeity correction +* notes : see ref [1],[7],[8] +* satellite clock does not include relativity correction and tdg +*-----------------------------------------------------------------------------*/ +double eph2clk(gtime_t time, const eph_t *eph) +{ + double t; + int i; + + trace(4,"eph2clk : time=%s sat=%2d\n",time_str(time,3),eph->sat); + + t=timediff(time,eph->toc); + + for (i=0;i<2;i++) { + t-=eph->f0+eph->f1*t+eph->f2*t*t; + } + return eph->f0+eph->f1*t+eph->f2*t*t; +} +/* broadcast ephemeris to satellite position and clock bias -------------------- +* compute satellite position and clock bias with broadcast ephemeris (gps, +* galileo, qzss) +* args : gtime_t time I time (gpst) +* eph_t *eph I broadcast ephemeris +* double *rs O satellite position (ecef) {x,y,z} (m) +* double *dts O satellite clock bias (s) +* double *var O satellite position and clock variance (m^2) +* return : none +* notes : see ref [1],[7],[8] +* satellite clock includes relativity correction without code bias +* (tgd or bgd) +*-----------------------------------------------------------------------------*/ +void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, + double *var) +{ + double tk,M,E,Ek,sinE,cosE,u,r,i,O,sin2u,cos2u,x,y,sinO,cosO,cosi,mu,omge; + double xg,yg,zg,sino,coso; + int n,sys,prn; + + trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat); + + if (eph->A<=0.0) { + rs[0]=rs[1]=rs[2]=*dts=*var=0.0; + return; + } + tk=timediff(time,eph->toe); + + switch ((sys=satsys(eph->sat,&prn))) { + case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break; + case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break; + default: mu=MU_GPS; omge=OMGE; break; + } + M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk; + + for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-eph->e*cos(E)); + } + if (n>=MAX_ITER_KEPLER) { + trace(2,"eph2pos: kepler iteration overflow sat=%2d\n",eph->sat); + return; + } + sinE=sin(E); cosE=cos(E); + + trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek); + + u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg; + r=eph->A*(1.0-eph->e*cosE); + i=eph->i0+eph->idot*tk; + sin2u=sin(2.0*u); cos2u=cos(2.0*u); + u+=eph->cus*sin2u+eph->cuc*cos2u; + r+=eph->crs*sin2u+eph->crc*cos2u; + i+=eph->cis*sin2u+eph->cic*cos2u; + x=r*cos(u); y=r*sin(u); cosi=cos(i); + + /* beidou geo satellite (ref [9]) */ + if (sys==SYS_CMP&&prn<=5) { + O=eph->OMG0+eph->OMGd*tk-omge*eph->toes; + sinO=sin(O); cosO=cos(O); + xg=x*cosO-y*cosi*sinO; + yg=x*sinO+y*cosi*cosO; + zg=y*sin(i); + sino=sin(omge*tk); coso=cos(omge*tk); + rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5; + rs[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5; + rs[2]=-yg*SIN_5+zg*COS_5; + } + else { + O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes; + sinO=sin(O); cosO=cos(O); + rs[0]=x*cosO-y*cosi*sinO; + rs[1]=x*sinO+y*cosi*cosO; + rs[2]=y*sin(i); + } + tk=timediff(time,eph->toc); + *dts=eph->f0+eph->f1*tk+eph->f2*tk*tk; + + /* relativity correction */ + *dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT); + + /* position and clock error variance */ + *var=var_uraeph(eph->sva); +} +/* glonass orbit differential equations --------------------------------------*/ +void deq(const double *x, double *xdot, const double *acc) +{ + double a,b,c,r2=dot(x,x,3),r3=r2*sqrt(r2),omg2=SQR(OMGE_GLO); + + if (r2<=0.0) { + xdot[0]=xdot[1]=xdot[2]=xdot[3]=xdot[4]=xdot[5]=0.0; + return; + } + /* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */ + a=1.5*J2_GLO*MU_GLO*SQR(RE_GLO)/r2/r3; /* 3/2*J2*mu*Ae^2/r^5 */ + b=5.0*x[2]*x[2]/r2; /* 5*z^2/r^2 */ + c=-MU_GLO/r3-a*(1.0-b); /* -mu/r^3-a(1-b) */ + xdot[0]=x[3]; xdot[1]=x[4]; xdot[2]=x[5]; + xdot[3]=(c+omg2)*x[0]+2.0*OMGE_GLO*x[4]+acc[0]; + xdot[4]=(c+omg2)*x[1]-2.0*OMGE_GLO*x[3]+acc[1]; + xdot[5]=(c-2.0*a)*x[2]+acc[2]; +} +/* glonass position and velocity by numerical integration --------------------*/ +void glorbit(double t, double *x, const double *acc) +{ + double k1[6],k2[6],k3[6],k4[6],w[6]; + int i; + + deq(x,k1,acc); for (i=0;i<6;i++) w[i]=x[i]+k1[i]*t/2.0; + deq(w,k2,acc); for (i=0;i<6;i++) w[i]=x[i]+k2[i]*t/2.0; + deq(w,k3,acc); for (i=0;i<6;i++) w[i]=x[i]+k3[i]*t; + deq(w,k4,acc); + for (i=0;i<6;i++) x[i]+=(k1[i]+2.0*k2[i]+2.0*k3[i]+k4[i])*t/6.0; +} +/* glonass ephemeris to satellite clock bias ----------------------------------- +* compute satellite clock bias with glonass ephemeris +* args : gtime_t time I time by satellite clock (gpst) +* geph_t *geph I glonass ephemeris +* return : satellite clock bias (s) +* notes : see ref [2] +*-----------------------------------------------------------------------------*/ +double geph2clk(gtime_t time, const geph_t *geph) +{ + double t; + int i; + + trace(4,"geph2clk: time=%s sat=%2d\n",time_str(time,3),geph->sat); + + t=timediff(time,geph->toe); + + for (i=0;i<2;i++) { + t-=-geph->taun+geph->gamn*t; + } + return -geph->taun+geph->gamn*t; +} +/* glonass ephemeris to satellite position and clock bias ---------------------- +* compute satellite position and clock bias with glonass ephemeris +* args : gtime_t time I time (gpst) +* geph_t *geph I glonass ephemeris +* double *rs O satellite position {x,y,z} (ecef) (m) +* double *dts O satellite clock bias (s) +* double *var O satellite position and clock variance (m^2) +* return : none +* notes : see ref [2] +*-----------------------------------------------------------------------------*/ +void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts, + double *var) +{ + double t,tt,x[6]; + int i; + + trace(4,"geph2pos: time=%s sat=%2d\n",time_str(time,3),geph->sat); + + t=timediff(time,geph->toe); + + *dts=-geph->taun+geph->gamn*t; + + for (i=0;i<3;i++) { + x[i ]=geph->pos[i]; + x[i+3]=geph->vel[i]; + } + for (tt=t<0.0?-TSTEP:TSTEP;fabs(t)>1E-9;t-=tt) { + if (fabs(t)acc); + } + for (i=0;i<3;i++) rs[i]=x[i]; + + *var=SQR(ERREPH_GLO); +} +/* sbas ephemeris to satellite clock bias -------------------------------------- +* compute satellite clock bias with sbas ephemeris +* args : gtime_t time I time by satellite clock (gpst) +* seph_t *seph I sbas ephemeris +* return : satellite clock bias (s) +* notes : see ref [3] +*-----------------------------------------------------------------------------*/ +double seph2clk(gtime_t time, const seph_t *seph) +{ + double t; + int i; + + trace(4,"seph2clk: time=%s sat=%2d\n",time_str(time,3),seph->sat); + + t=timediff(time,seph->t0); + + for (i=0;i<2;i++) { + t-=seph->af0+seph->af1*t; + } + return seph->af0+seph->af1*t; +} +/* sbas ephemeris to satellite position and clock bias ------------------------- +* compute satellite position and clock bias with sbas ephemeris +* args : gtime_t time I time (gpst) +* seph_t *seph I sbas ephemeris +* double *rs O satellite position {x,y,z} (ecef) (m) +* double *dts O satellite clock bias (s) +* double *var O satellite position and clock variance (m^2) +* return : none +* notes : see ref [3] +*-----------------------------------------------------------------------------*/ +void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts, + double *var) +{ + double t; + int i; + + trace(4,"seph2pos: time=%s sat=%2d\n",time_str(time,3),seph->sat); + + t=timediff(time,seph->t0); + + for (i=0;i<3;i++) { + rs[i]=seph->pos[i]+seph->vel[i]*t+seph->acc[i]*t*t/2.0; + } + *dts=seph->af0+seph->af1*t; + + *var=var_uraeph(seph->sva); +} +/* select ephememeris --------------------------------------------------------*/ +eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) +{ + double t,tmax,tmin; + int i,j=-1; + + trace(4,"seleph : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode); + + switch (satsys(sat,NULL)) { + case SYS_QZS: tmax=MAXDTOE_QZS+1.0; break; + case SYS_GAL: tmax=MAXDTOE_GAL+1.0; break; + case SYS_CMP: tmax=MAXDTOE_CMP+1.0; break; + default: tmax=MAXDTOE+1.0; break; + } + tmin=tmax+1.0; + + for (i=0;in;i++) { + if (nav->eph[i].sat!=sat) continue; + if (iode>=0&&nav->eph[i].iode!=iode) continue; + if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue; + if (iode>=0) return nav->eph+i; + if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ + } + if (iode>=0||j<0) { + trace(3,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0), + sat,iode); + return NULL; + } + return nav->eph+j; +} +/* select glonass ephememeris ------------------------------------------------*/ +geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav) +{ + double t,tmax=MAXDTOE_GLO,tmin=tmax+1.0; + int i,j=-1; + + trace(4,"selgeph : time=%s sat=%2d iode=%2d\n",time_str(time,3),sat,iode); + + for (i=0;ing;i++) { + if (nav->geph[i].sat!=sat) continue; + if (iode>=0&&nav->geph[i].iode!=iode) continue; + if ((t=fabs(timediff(nav->geph[i].toe,time)))>tmax) continue; + if (iode>=0) return nav->geph+i; + if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ + } + if (iode>=0||j<0) { + trace(3,"no glonass ephemeris : %s sat=%2d iode=%2d\n",time_str(time,0), + sat,iode); + return NULL; + } + return nav->geph+j; +} +/* select sbas ephememeris ---------------------------------------------------*/ +seph_t *selseph(gtime_t time, int sat, const nav_t *nav) +{ + double t,tmax=MAXDTOE_SBS,tmin=tmax+1.0; + int i,j=-1; + + trace(4,"selseph : time=%s sat=%2d\n",time_str(time,3),sat); + + for (i=0;ins;i++) { + if (nav->seph[i].sat!=sat) continue; + if ((t=fabs(timediff(nav->seph[i].t0,time)))>tmax) continue; + if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ + } + if (j<0) { + trace(3,"no sbas ephemeris : %s sat=%2d\n",time_str(time,0),sat); + return NULL; + } + return nav->seph+j; +} +/* satellite clock with broadcast ephemeris ----------------------------------*/ +int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *dts) +{ + eph_t *eph; + geph_t *geph; + seph_t *seph; + int sys; + + trace(4,"ephclk : time=%s sat=%2d\n",time_str(time,3),sat); + + sys=satsys(sat,NULL); + + if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { + if (!(eph=seleph(teph,sat,-1,nav))) return 0; + *dts=eph2clk(time,eph); + } + else if (sys==SYS_GLO) { + if (!(geph=selgeph(teph,sat,-1,nav))) return 0; + *dts=geph2clk(time,geph); + } + else if (sys==SYS_SBS) { + if (!(seph=selseph(teph,sat,nav))) return 0; + *dts=seph2clk(time,seph); + } + else return 0; + + return 1; +} +/* satellite position and clock by broadcast ephemeris -----------------------*/ +int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int iode, double *rs, double *dts, double *var, int *svh) +{ + eph_t *eph; + geph_t *geph; + seph_t *seph; + double rst[3],dtst[1],tt=1E-3; + int i,sys; + + trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode); + + sys=satsys(sat,NULL); + + *svh=-1; + + if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { + if (!(eph=seleph(teph,sat,iode,nav))) return 0; + + eph2pos(time,eph,rs,dts,var); + time=timeadd(time,tt); + eph2pos(time,eph,rst,dtst,var); + *svh=eph->svh; + } + else if (sys==SYS_GLO) { + if (!(geph=selgeph(teph,sat,iode,nav))) return 0; + geph2pos(time,geph,rs,dts,var); + time=timeadd(time,tt); + geph2pos(time,geph,rst,dtst,var); + *svh=geph->svh; + } + else if (sys==SYS_SBS) { + if (!(seph=selseph(teph,sat,nav))) return 0; + + seph2pos(time,seph,rs,dts,var); + time=timeadd(time,tt); + seph2pos(time,seph,rst,dtst,var); + *svh=seph->svh; + } + else return 0; + + /* satellite velocity and clock drift by differential approx */ + for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt; + dts[1]=(dtst[0]-dts[0])/tt; + + return 1; +} +/* satellite position and clock with sbas correction -------------------------*/ +int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *rs, double *dts, double *var, int *svh) +{ + const sbssatp_t *sbs; + int i; + + trace(4,"satpos_sbas: time=%s sat=%2d\n",time_str(time,3),sat); + + /* search sbas satellite correciton */ + for (i=0;isbssat.nsat;i++) { + sbs=nav->sbssat.sat+i; + if (sbs->sat==sat) break; + } + if (i>=nav->sbssat.nsat) { + trace(2,"no sbas correction for orbit: %s sat=%2d\n",time_str(time,0),sat); + ephpos(time,teph,sat,nav,-1,rs,dts,var,svh); + *svh=-1; + return 0; + } + /* satellite postion and clock by broadcast ephemeris */ + if (!ephpos(time,teph,sat,nav,sbs->lcorr.iode,rs,dts,var,svh)) return 0; + + /* sbas satellite correction (long term and fast) */ + if (sbssatcorr(time,sat,nav,rs,dts,var)) return 1; + *svh=-1; + return 0; +} +/* satellite position and clock with ssr correction --------------------------*/ +int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int opt, double *rs, double *dts, double *var, int *svh) +{ + const ssr_t *ssr; + eph_t *eph; + double t1,t2,t3,er[3],ea[3],ec[3],rc[3],deph[3],dclk,dant[3]={0},tk; + int i,sys; + + trace(4,"satpos_ssr: time=%s sat=%2d\n",time_str(time,3),sat); + + ssr=nav->ssr+sat-1; + + if (!ssr->t0[0].time) { + trace(2,"no ssr orbit correction: %s sat=%2d\n",time_str(time,0),sat); + return 0; + } + if (!ssr->t0[1].time) { + trace(2,"no ssr clock correction: %s sat=%2d\n",time_str(time,0),sat); + return 0; + } + /* inconsistency between orbit and clock correction */ + if (ssr->iod[0]!=ssr->iod[1]) { + trace(2,"inconsist ssr correction: %s sat=%2d iod=%d %d\n", + time_str(time,0),sat,ssr->iod[0],ssr->iod[1]); + *svh=-1; + return 0; + } + t1=timediff(time,ssr->t0[0]); + t2=timediff(time,ssr->t0[1]); + t3=timediff(time,ssr->t0[2]); + + /* ssr orbit and clock correction (ref [4]) */ + if (fabs(t1)>MAXAGESSR||fabs(t2)>MAXAGESSR) { + trace(2,"age of ssr error: %s sat=%2d t=%.0f %.0f\n",time_str(time,0), + sat,t1,t2); + *svh=-1; + return 0; + } + if (ssr->udi[0]>=1.0) t1-=ssr->udi[0]/2.0; + if (ssr->udi[1]>=1.0) t2-=ssr->udi[0]/2.0; + + for (i=0;i<3;i++) deph[i]=ssr->deph[i]+ssr->ddeph[i]*t1; + dclk=ssr->dclk[0]+ssr->dclk[1]*t2+ssr->dclk[2]*t2*t2; + + /* ssr highrate clock correction (ref [4]) */ + if (ssr->iod[0]==ssr->iod[2]&&ssr->t0[2].time&&fabs(t3)hrclk; + } + if (norm(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) { + trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n", + time_str(time,0),norm(deph,3),dclk); + *svh=-1; + return 0; + } + /* satellite postion and clock by broadcast ephemeris */ + if (!ephpos(time,teph,sat,nav,ssr->iode,rs,dts,var,svh)) return 0; + + /* satellite clock for gps, galileo and qzss */ + sys=satsys(sat,NULL); + if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { + if (!(eph=seleph(teph,sat,ssr->iode,nav))) return 0; + + /* satellite clock by clock parameters */ + tk=timediff(time,eph->toc); + dts[0]=eph->f0+eph->f1*tk+eph->f2*tk*tk; + dts[1]=eph->f1+2.0*eph->f2*tk; + + /* relativity correction */ + dts[0]-=2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT; + } + /* radial-along-cross directions in ecef */ + if (!normv3(rs+3,ea)) return 0; + cross3(rs,rs+3,rc); + if (!normv3(rc,ec)) { + *svh=-1; + return 0; + } + cross3(ea,ec,er); + + /* satellite antenna offset correction */ + if (opt) { + satantoff(time,rs,sat,nav,dant); + } + for (i=0;i<3;i++) { + rs[i]+=-(er[i]*deph[0]+ea[i]*deph[1]+ec[i]*deph[2])+dant[i]; + } + /* t_corr = t_sv - (dts(brdc) + dclk(ssr) / CLIGHT) (ref [10] eq.3.12-7) */ + dts[0]+=dclk/CLIGHT; + + /* variance by ssr ura */ + *var=var_urassr(ssr->ura); + + trace(5,"satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n", + time_str(time,2),sat,deph[0],deph[1],deph[2],er[0],er[1],er[2],dclk,*var); + + return 1; +} +/* satellite position and clock ------------------------------------------------ +* compute satellite position, velocity and clock +* args : gtime_t time I time (gpst) +* gtime_t teph I time to select ephemeris (gpst) +* int sat I satellite number +* nav_t *nav I navigation data +* int ephopt I ephemeris option (EPHOPT_???) +* double *rs O sat position and velocity (ecef) +* {x,y,z,vx,vy,vz} (m|m/s) +* double *dts O sat clock {bias,drift} (s|s/s) +* double *var O sat position and clock error variance (m^2) +* int *svh O sat health flag (-1:correction not available) +* return : status (1:ok,0:error) +* notes : satellite position is referenced to antenna phase center +* satellite clock does not include code bias correction (tgd or bgd) +*-----------------------------------------------------------------------------*/ +int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, + const nav_t *nav, double *rs, double *dts, double *var, + int *svh) +{ + trace(4,"satpos : time=%s sat=%2d ephopt=%d\n",time_str(time,3),sat,ephopt); + + *svh=0; + + switch (ephopt) { + case EPHOPT_BRDC : return ephpos (time,teph,sat,nav,-1,rs,dts,var,svh); + case EPHOPT_SBAS : return satpos_sbas(time,teph,sat,nav, rs,dts,var,svh); + case EPHOPT_SSRAPC: return satpos_ssr (time,teph,sat,nav, 0,rs,dts,var,svh); + case EPHOPT_SSRCOM: return satpos_ssr (time,teph,sat,nav, 1,rs,dts,var,svh); + case EPHOPT_PREC : + if (!peph2pos(time,sat,nav,1,rs,dts,var)) break; else return 1; + //TODO: enable lex + //case EPHOPT_LEX : + // if (!lexeph2pos(time,sat,nav,rs,dts,var)) break; else return 1; + } + *svh=-1; + return 0; +} +/* satellite positions and clocks ---------------------------------------------- +* compute satellite positions, velocities and clocks +* args : gtime_t teph I time to select ephemeris (gpst) +* obsd_t *obs I observation data +* int n I number of observation data +* nav_t *nav I navigation data +* int ephopt I ephemeris option (EPHOPT_???) +* double *rs O satellite positions and velocities (ecef) +* double *dts O satellite clocks +* double *var O sat position and clock error variances (m^2) +* int *svh O sat health flag (-1:correction not available) +* return : none +* notes : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m) +* rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s) +* dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s) +* var[i] = obs[i] sat position and clock error variance (m^2) +* svh[i] = obs[i] sat health flag +* if no navigation data, set 0 to rs[], dts[], var[] and svh[] +* satellite position and clock are values at signal transmission time +* satellite position is referenced to antenna phase center +* satellite clock does not include code bias correction (tgd or bgd) +* any pseudorange and broadcast ephemeris are always needed to get +* signal transmission time +*-----------------------------------------------------------------------------*/ +void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, + int ephopt, double *rs, double *dts, double *var, int *svh) +{ + gtime_t time[MAXOBS]={}; + double dt,pr; + int i,j; + + trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt); + + for (i=0;i=NFREQ) { + trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat); + continue; + } + /* transmission time by satellite clock */ + time[i]=timeadd(obs[i].time,-pr/CLIGHT); + + /* satellite clock bias by broadcast ephemeris */ + if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) { + trace(3,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat); + continue; + } + time[i]=timeadd(time[i],-dt); + + /* satellite position and clock at transmission time */ + if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i, + svh+i)) { + trace(3,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat); + continue; + } + /* if no precise clock available, use broadcast clock instead */ + if (dts[i*2]==0.0) { + if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue; + dts[1+i*2]=0.0; + *var=SQR(STD_BRDCCLK); + } + } + for (i=0;i + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + * + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +* history : 2010/07/28 1.1 moved from rtkcmn.c +* added api: +* eph2clk(),geph2clk(),seph2clk(),satantoff() +* satposs() +* changed api: +* eph2pos(),geph2pos(),satpos() +* deleted api: +* satposv(),satposiode() +* 2010/08/26 1.2 add ephemeris option EPHOPT_LEX +* 2010/09/09 1.3 fix problem when precise clock outage +* 2011/01/12 1.4 add api alm2pos() +* change api satpos(),satposs() +* enable valid unhealthy satellites and output status +* fix bug on exception by glonass ephem computation +* 2013/01/10 1.5 support beidou (compass) +* use newton's method to solve kepler eq. +* update ssr correction algorithm +* 2013/03/20 1.6 fix problem on ssr clock relativitic correction +* 2013/09/01 1.7 support negative pseudorange +* fix bug on variance in case of ura ssr = 63 +* 2013/11/11 1.8 change constant MAXAGESSR 70.0 -> 90.0 +* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15 1E-13 +* set MAX_ITER_KEPLER for alm2pos() +*-----------------------------------------------------------------------------*/ + + +#ifndef RTKLIB_EPHEMERIS_H_ +#define RTKLIB_EPHEMERIS_H_ + +#include "rtklib.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_sbas.h" +#include "rtklib_preceph.h" + +double var_uraeph(int ura); +double var_urassr(int ura); +void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts); +double eph2clk(gtime_t time, const eph_t *eph); +void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, + double *var); +void deq(const double *x, double *xdot, const double *acc); +void glorbit(double t, double *x, const double *acc); +double geph2clk(gtime_t time, const geph_t *geph); + +void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts, + double *var); +double seph2clk(gtime_t time, const seph_t *seph); +void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts, + double *var); +eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav); +geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav); +seph_t *selseph(gtime_t time, int sat, const nav_t *nav); +int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *dts); +//satellite position and clock by broadcast ephemeris +int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int iode, double *rs, double *dts, double *var, int *svh); +int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *rs, double *dts, double *var, int *svh); +int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int opt, double *rs, double *dts, double *var, int *svh); + +int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, + const nav_t *nav, double *rs, double *dts, double *var, + int *svh); +void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, + int ephopt, double *rs, double *dts, double *var, int *svh); + + + + +#endif /* RTKLIB_EPHEMERIS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc new file mode 100644 index 000000000..0fd99cb44 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -0,0 +1,516 @@ +/*! + * \file rtklib_ionex.h + * \brief ionex functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* +* references: +* [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange +* Format Version 1, February 25, 1998 +* [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere +* Maps based on GPS Carrier Phase Data Routinely producted by CODE +* Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 +* +* version : $Revision:$ $Date:$ +* history : 2011/03/29 1.0 new +* 2013/03/05 1.1 change api readtec() +* fix problem in case of lat>85deg or lat<-85deg +* 2014/02/22 1.2 fix problem on compiled as C++ +*-----------------------------------------------------------------------------*/ +#include "rtklib_ionex.h" + +/* get index -----------------------------------------------------------------*/ +int getindex(double value, const double *range) +{ + if (range[2]==0.0) return 0; + if (range[1]>0.0&&(valuent>=nav->ntmax) { + nav->ntmax+=256; + if (!(nav_tec=(tec_t *)realloc(nav->tec,sizeof(tec_t)*nav->ntmax))) { + trace(1,"readionex malloc error ntmax=%d\n",nav->ntmax); + free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0; + return NULL; + } + nav->tec=nav_tec; + } + p=nav->tec+nav->nt; + p->time=time0; + p->rb=rb; + for (i=0;i<3;i++) { + p->ndata[i]=ndata[i]; + p->lats[i]=lats[i]; + p->lons[i]=lons[i]; + p->hgts[i]=hgts[i]; + } + n=ndata[0]*ndata[1]*ndata[2]; + + if (!(p->data=(double *)malloc(sizeof(double)*n))|| + !(p->rms =(float *)malloc(sizeof(float )*n))) { + return NULL; + } + for (i=0;idata[i]=0.0; + p->rms [i]=0.0f; + } + nav->nt++; + return p; +} +/* read ionex dcb aux data ----------------------------------------------------*/ +void readionexdcb(FILE *fp, double *dcb, double *rms) +{ + int i,sat; + char buff[1024],id[32],*label; + + trace(3,"readionexdcb:\n"); + + for (i=0;int-1;i>=0;i--) { + if (fabs(timediff(time,nav->tec[i].time))>=1.0) continue; + p=nav->tec+i; + break; + } + } + else if (p) p->time=time; + } + else if (strstr(label,"LAT/LON1/LON2/DLON/H")==label&&p) { + lat =str2num(buff, 2,6); + lon[0]=str2num(buff, 8,6); + lon[1]=str2num(buff,14,6); + lon[2]=str2num(buff,20,6); + hgt =str2num(buff,26,6); + + i=getindex(lat,p->lats); + k=getindex(hgt,p->hgts); + n=nitem(lon); + + for (m=0;mlons); + if ((index=dataindex(i,j,k,p->ndata))<0) continue; + + if ((x=str2num(buff,m%16*5,5))==9999.0) continue; + + if (type==1) p->data[index]=x*pow(10.0,nexp); + else p->rms[index]=(float)(x*pow(10.0,nexp)); + } + } + } + return 1; +} +/* combine tec grid data -----------------------------------------------------*/ +void combtec(nav_t *nav) +{ + tec_t tmp; + int i,j,n=0; + + trace(3,"combtec : nav->nt=%d\n",nav->nt); + + for (i=0;int-1;i++) { + for (j=i+1;jnt;j++) { + if (timediff(nav->tec[j].time,nav->tec[i].time)<0.0) { + tmp=nav->tec[i]; + nav->tec[i]=nav->tec[j]; + nav->tec[j]=tmp; + } + } + } + for (i=0;int;i++) { + if (i>0&&timediff(nav->tec[i].time,nav->tec[n-1].time)==0.0) { + free(nav->tec[n-1].data); + free(nav->tec[n-1].rms ); + nav->tec[n-1]=nav->tec[i]; + continue; + } + nav->tec[n++]=nav->tec[i]; + } + nav->nt=n; + + trace(4,"combtec : nav->nt=%d\n",nav->nt); +} +/* read ionex tec grid file ---------------------------------------------------- +* read ionex ionospheric tec grid file +* args : char *file I ionex tec grid file +* (wind-card * is expanded) +* nav_t *nav IO navigation data +* nav->nt, nav->ntmax and nav->tec are modified +* int opt I read option (1: no clear of tec data,0:clear) +* return : none +* notes : see ref [1] +*-----------------------------------------------------------------------------*/ +void readtec(const char *file, nav_t *nav, int opt) +{ + FILE *fp; + double lats[3]={0},lons[3]={0},hgts[3]={0},rb=0.0,nexp=-1.0; + double dcb[MAXSAT]={0},rms[MAXSAT]={0}; + int i,n; + char *efiles[MAXEXFILE]; + + trace(3,"readtec : file=%s\n",file); + + /* clear of tec grid data option */ + if (!opt) { + free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0; + } + for (i=0;i=0;i--) free(efiles[i]); + return; + } + } + /* expand wild card in file path */ + n=expath(file,efiles,MAXEXFILE); + + for (i=0;int>0) combtec(nav); + + /* P1-P2 dcb */ + for (i=0;icbias[i][0]=CLIGHT*dcb[i]*1E-9; /* ns->m */ + } +} +/* interpolate tec grid data -------------------------------------------------*/ +int interptec(const tec_t *tec, int k, const double *posp, double *value, + double *rms) +{ + double dlat,dlon,a,b,d[4]={0},r[4]={0}; + int i,j,n,index; + + trace(3,"interptec: k=%d posp=%.2f %.2f\n",k,posp[0]*R2D,posp[1]*R2D); + *value=*rms=0.0; + + if (tec->lats[2]==0.0||tec->lons[2]==0.0) return 0; + + dlat=posp[0]*R2D-tec->lats[0]; + dlon=posp[1]*R2D-tec->lons[0]; + if (tec->lons[2]>0.0) dlon-=floor( dlon/360)*360.0; /* 0<=dlon<360 */ + else dlon+=floor(-dlon/360)*360.0; /* -360lats[2]; + b=dlon/tec->lons[2]; + i=(int)floor(a); a-=i; + j=(int)floor(b); b-=j; + + /* get gridded tec data */ + for (n=0;n<4;n++) { + if ((index=dataindex(i+(n%2),j+(n<2?0:1),k,tec->ndata))<0) continue; + d[n]=tec->data[index]; + r[n]=tec->rms [index]; + } + if (d[0]>0.0&&d[1]>0.0&&d[2]>0.0&&d[3]>0.0) { + + /* bilinear interpolation (inside of grid) */ + *value=(1.0-a)*(1.0-b)*d[0]+a*(1.0-b)*d[1]+(1.0-a)*b*d[2]+a*b*d[3]; + *rms =(1.0-a)*(1.0-b)*r[0]+a*(1.0-b)*r[1]+(1.0-a)*b*r[2]+a*b*r[3]; + } + /* nearest-neighbour extrapolation (outside of grid) */ + else if (a<=0.5&&b<=0.5&&d[0]>0.0) {*value=d[0]; *rms=r[0];} + else if (a> 0.5&&b<=0.5&&d[1]>0.0) {*value=d[1]; *rms=r[1];} + else if (a<=0.5&&b> 0.5&&d[2]>0.0) {*value=d[2]; *rms=r[2];} + else if (a> 0.5&&b> 0.5&&d[3]>0.0) {*value=d[3]; *rms=r[3];} + else { + i=0; + for (n=0;n<4;n++) if (d[n]>0.0) {i++; *value+=d[n]; *rms+=r[n];} + if(i==0) return 0; + *value/=i; *rms/=i; + } + return 1; +} +/* ionosphere delay by tec grid data -----------------------------------------*/ +int iondelay(gtime_t time, const tec_t *tec, const double *pos, + const double *azel, int opt, double *delay, double *var) +{ + const double fact=40.30E16/FREQ1/FREQ1; /* tecu->L1 iono (m) */ + double fs,posp[3]={0},vtec,rms,hion,rp; + int i; + + trace(3,"iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n",time_str(time,0), + pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D); + + *delay=*var=0.0; + + for (i=0;indata[2];i++) { /* for a layer */ + + hion=tec->hgts[0]+tec->hgts[2]*i; + + /* ionospheric pierce point position */ + fs=ionppp(pos,azel,tec->rb,hion,posp); + + if (opt&2) { + /* modified single layer mapping function (M-SLM) ref [2] */ + rp=tec->rb/(tec->rb+hion)*sin(0.9782*(PI/2.0-azel[1])); + fs=1.0/sqrt(1.0-rp*rp); + } + if (opt&1) { + /* earth rotation correction (sun-fixed coordinate) */ + posp[1]+=2.0*PI*timediff(time,tec->time)/86400.0; + } + /* interpolate tec grid data */ + if (!interptec(tec,i,posp,&vtec,&rms)) return 0; + + *delay+=fact*fs*vtec; + *var+=fact*fact*fs*fs*rms*rms; + } + trace(4,"iondelay: delay=%7.2f std=%6.2f\n",*delay,sqrt(*var)); + + return 1; +} +/* ionosphere model by tec grid data ------------------------------------------- +* compute ionospheric delay by tec grid data +* args : gtime_t time I time (gpst) +* nav_t *nav I navigation data +* double *pos I receiver position {lat,lon,h} (rad,m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* int opt I model option +* bit0: 0:earth-fixed,1:sun-fixed +* bit1: 0:single-layer,1:modified single-layer +* double *delay O ionospheric delay (L1) (m) +* double *var O ionospheric dealy (L1) variance (m^2) +* return : status (1:ok,0:error) +* notes : before calling the function, read tec grid data by calling readtec() +* return ok with delay=0 and var=VAR_NOTEC if elnt;i++) { + if (timediff(nav->tec[i].time,time)>0.0) break; + } + if (i==0||i>=nav->nt) { + trace(2,"%s: tec grid out of period\n",time_str(time,0)); + return 0; + } + if ((tt=timediff(nav->tec[i].time,nav->tec[i-1].time))==0.0) { + trace(2,"tec grid time interval error\n"); + return 0; + } + /* ionospheric delay by tec grid data */ + stat[0]=iondelay(time,nav->tec+i-1,pos,azel,opt,dels ,vars ); + stat[1]=iondelay(time,nav->tec+i ,pos,azel,opt,dels+1,vars+1); + + if (!stat[0]&&!stat[1]) { + trace(2,"%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n", + time_str(time,0),pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D); + return 0; + } + if (stat[0]&&stat[1]) { /* linear interpolation by time */ + a=timediff(time,nav->tec[i-1].time)/tt; + *delay=dels[0]*(1.0-a)+dels[1]*a; + *var =vars[0]*(1.0-a)+vars[1]*a; + } + else if (stat[0]) { /* nearest-neighbour extrapolation by time */ + *delay=dels[0]; + *var =vars[0]; + } + else { + *delay=dels[1]; + *var =vars[1]; + } + trace(3,"iontec : delay=%5.2f std=%5.2f\n",*delay,sqrt(*var)); + return 1; +} diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.h b/src/algorithms/libs/rtklib/rtklib_ionex.h new file mode 100644 index 000000000..155b92ff9 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ionex.h @@ -0,0 +1,95 @@ +/*! + * \file rtklib_ionex.cc + * \brief ionex functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* +* references: +* [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange +* Format Version 1, February 25, 1998 +* [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere +* Maps based on GPS Carrier Phase Data Routinely producted by CODE +* Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 +* +* version : $Revision:$ $Date:$ +* history : 2011/03/29 1.0 new +* 2013/03/05 1.1 change api readtec() +* fix problem in case of lat>85deg or lat<-85deg +* 2014/02/22 1.2 fix problem on compiled as C++ +*-----------------------------------------------------------------------------*/ +#ifndef RTKLIB_IONEX_H_ +#define RTKLIB_IONEX_H_ + +#include "rtklib_rtkcmn.h" + +#define SQR(x) ((x)*(x)) +#define VAR_NOTEC SQR(30.0) /* variance of no tec */ +#define MIN_EL 0.0 /* min elevation angle (rad) */ +#define MIN_HGT -1000.0 /* min user height (m) */ + +int getindex(double value, const double *range); + +int nitem(const double *range); +int dataindex(int i, int j, int k, const int *ndata); +tec_t *addtec(const double *lats, const double *lons, const double *hgts, + double rb, nav_t *nav); +void readionexdcb(FILE *fp, double *dcb, double *rms); +double readionexh(FILE *fp, double *lats, double *lons, double *hgts, + double *rb, double *nexp, double *dcb, double *rms); +int readionexb(FILE *fp, const double *lats, const double *lons, + const double *hgts, double rb, double nexp, nav_t *nav); +void combtec(nav_t *nav); +void readtec(const char *file, nav_t *nav, int opt); +int interptec(const tec_t *tec, int k, const double *posp, double *value, + double *rms); + +int iondelay(gtime_t time, const tec_t *tec, const double *pos, + const double *azel, int opt, double *delay, double *var); +int iontec(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, int opt, double *delay, double *var); + +#endif /* SRC_ALGORITHMS_PVT_LIBS_RTKLIB_IONEX_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc new file mode 100644 index 000000000..1e45c07bc --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -0,0 +1,704 @@ +/*! + * \file rtklib_pntpos.cc + * \brief standard code-based positioning + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +* history : 2010/07/28 1.0 moved from rtkcmn.c +* changed api: +* pntpos() +* deleted api: +* pntvel() +* 2011/01/12 1.1 add option to include unhealthy satellite +* reject duplicated observation data +* changed api: ionocorr() +* 2011/11/08 1.2 enable snr mask for single-mode (rtklib_2.4.1_p3) +* 2012/12/25 1.3 add variable snr mask +* 2014/05/26 1.4 support galileo and beidou +* 2015/03/19 1.5 fix bug on ionosphere correction for GLO and BDS +*-----------------------------------------------------------------------------*/ +#include "rtklib_pntpos.h" + + +/* pseudorange measurement error variance ------------------------------------*/ +double varerr(const prcopt_t *opt, double el, int sys) +{ + double fact,varr; + fact=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); + varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el)); + if (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */ + return SQR(fact)*varr; +} +/* get tgd parameter (m) -----------------------------------------------------*/ +double gettgd(int sat, const nav_t *nav) +{ + int i; + for (i=0;in;i++) { + if (nav->eph[i].sat!=sat) continue; + return CLIGHT*nav->eph[i].tgd[0]; + } + return 0.0; +} +/* psendorange with code bias correction -------------------------------------*/ +double prange(const obsd_t *obs, const nav_t *nav, const double *azel, + int iter, const prcopt_t *opt, double *var) +{ + const double *lam=nav->lam[obs->sat-1]; + double PC,P1,P2,P1_P2,P1_C1,P2_C2,gamma; + int i=0,j=1,sys; + + *var=0.0; + + if (!(sys=satsys(obs->sat,NULL))) + { + trace(4,"prange: satsys NULL\n"); + return 0.0; + } + + + /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ + if (NFREQ>=3&&(sys&(SYS_GAL|SYS_SBS))) j=2; + + if (NFREQ<2||lam[i]==0.0||lam[j]==0.0) + { + trace(4,"prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); + printf("i: %d j:%d, lam[i]: %f lam[j] %f\n",i,j,lam[i],lam[j]); + return 0.0; + } + + /* test snr mask */ + if (iter>0) { + if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) { + trace(4,"snr mask: %s sat=%2d el=%.1f snr=%.1f\n", + time_str(obs->time,0),obs->sat,azel[1]*R2D,obs->SNR[i]*0.25); + return 0.0; + } + if (opt->ionoopt==IONOOPT_IFLC) { + if (testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) + { + trace(4,"prange: testsnr error\n"); + return 0.0; + } + } + } + gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */ + P1=obs->P[i]; + P2=obs->P[j]; + P1_P2=nav->cbias[obs->sat-1][0]; + P1_C1=nav->cbias[obs->sat-1][1]; + P2_C2=nav->cbias[obs->sat-1][2]; + + /* if no P1-P2 DCB, use TGD instead */ + if (P1_P2==0.0&&(sys&(SYS_GPS|SYS_GAL|SYS_QZS))) { + P1_P2=(1.0-gamma)*gettgd(obs->sat,nav); + } + if (opt->ionoopt==IONOOPT_IFLC) { /* dual-frequency */ + + if (P1==0.0||P2==0.0) return 0.0; + if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ + if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */ + + /* iono-free combination */ + PC=(gamma*P1-P2)/(gamma-1.0); + } + else { /* single-frequency */ + + if (P1==0.0) return 0.0; + if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ + PC=P1-P1_P2/(1.0-gamma); + } + if (opt->sateph==EPHOPT_SBAS) PC-=P1_C1; /* sbas clock based C1 */ + + *var=SQR(ERR_CBIAS); + + return PC; +} +/* ionospheric correction ------------------------------------------------------ +* compute ionospheric correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* int sat I satellite number +* double *pos I receiver position {lat,lon,h} (rad|m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* int ionoopt I ionospheric correction option (IONOOPT_???) +* double *ion O ionospheric delay (L1) (m) +* double *var O ionospheric delay (L1) variance (m^2) +* return : status(1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, + const double *azel, int ionoopt, double *ion, double *var) +{ + trace(4,"ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n", + time_str(time,3),ionoopt,sat,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, + azel[1]*R2D); + + /* broadcast model */ + if (ionoopt==IONOOPT_BRDC) { + *ion=ionmodel(time,nav->ion_gps,pos,azel); + *var=SQR(*ion*ERR_BRDCI); + return 1; + } + /* sbas ionosphere model */ + if (ionoopt==IONOOPT_SBAS) { + return sbsioncorr(time,nav,pos,azel,ion,var); + } + /* ionex tec model */ + if (ionoopt==IONOOPT_TEC) { + return iontec(time,nav,pos,azel,1,ion,var); + } + /* qzss broadcast model */ + if (ionoopt==IONOOPT_QZS&&norm(nav->ion_qzs,8)>0.0) { + *ion=ionmodel(time,nav->ion_qzs,pos,azel); + *var=SQR(*ion*ERR_BRDCI); + return 1; + } + /* lex ionosphere model */ + //if (ionoopt==IONOOPT_LEX) { + // return lexioncorr(time,nav,pos,azel,ion,var); + //} + *ion=0.0; + *var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0; + return 1; +} +/* tropospheric correction ----------------------------------------------------- +* compute tropospheric correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* double *pos I receiver position {lat,lon,h} (rad|m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* int tropopt I tropospheric correction option (TROPOPT_???) +* double *trp O tropospheric delay (m) +* double *var O tropospheric delay variance (m^2) +* return : status(1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int tropcorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, int tropopt, double *trp, double *var) +{ + trace(4,"tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n", + time_str(time,3),tropopt,pos[0]*R2D,pos[1]*R2D,azel[0]*R2D, + azel[1]*R2D); + + /* saastamoinen model */ + if (tropopt==TROPOPT_SAAS||tropopt==TROPOPT_EST||tropopt==TROPOPT_ESTG) { + *trp=tropmodel(time,pos,azel,REL_HUMI); + *var=SQR(ERR_SAAS/(sin(azel[1])+0.1)); + return 1; + } + /* sbas troposphere model */ + if (tropopt==TROPOPT_SBAS) { + *trp=sbstropcorr(time,pos,azel,var); + return 1; + } + /* no correction */ + *trp=0.0; + *var=tropopt==TROPOPT_OFF?SQR(ERR_TROP):0.0; + return 1; +} +/* pseudorange residuals -----------------------------------------------------*/ +int rescode(int iter, const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, const prcopt_t *opt, + double *v, double *H, double *var, double *azel, int *vsat, + double *resp, int *ns) +{ + double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1; + int i,j,nv=0,sys,mask[4]={0}; + + trace(3,"resprng : n=%d\n",n); + + for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3]; + + ecef2pos(rr,pos); + + for (i=*ns=0;ielmin) + { + trace(4,"geodist / satazel error\n"); + continue; + } + + /* psudorange with code bias correction */ + if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) + { + trace(4,"prange error\n"); + continue; + } + + /* excluded satellite? */ + if (satexclude(obs[i].sat,svh[i],opt)) + { + trace(4,"satexclude error\n"); + continue; + } + + /* ionospheric corrections */ + if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2, + iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) + { + trace(4,"ionocorr error\n"); + continue; + } + + /* GPS-L1 -> L1/B1 */ + if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) { + dion*=SQR(lam_L1/lam_carr[0]); + } + /* tropospheric corrections */ + if (!tropcorr(obs[i].time,nav,pos,azel+i*2, + iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) { + trace(4,"tropocorr error\n"); + continue; + } + /* pseudorange residual */ + v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp); + + /* design matrix */ + for (j=0;jnx&&vv>chisqr[nv-nx-1]) { + sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]); + return 0; + } + /* large gdop check */ + for (i=ns=0;ielmin,dop); + if (dop[0]<=0.0||dop[0]>opt->maxgdop) { + sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]); + return 0; + } + return 1; +} +/* estimate receiver position ------------------------------------------------*/ +int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, + const double *vare, const int *svh, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, + double *resp, char *msg) +{ + double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig; + int i,j,k,info,stat,nv,ns; + + trace(3,"estpos : n=%d\n",n); + + v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); + + for (i=0;i<3;i++) x[i]=sol->rr[i]; + + for (i=0;itype=0; + sol->time=timeadd(obs[0].time,-x[3]/CLIGHT); + sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */ + sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */ + sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */ + sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */ + for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0; + for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX]; + sol->qr[3]=(float)Q[1]; /* cov xy */ + sol->qr[4]=(float)Q[2+NX]; /* cov yz */ + sol->qr[5]=(float)Q[2]; /* cov zx */ + sol->ns=(unsigned char)ns; + sol->age=sol->ratio=0.0; + + /* validate solution */ + if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) { + sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE; + } + free(v); free(H); free(var); + + return stat; + } + } + if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i); + + free(v); free(H); free(var); + + return 0; +} +/* raim fde (failure detection and exclution) -------------------------------*/ +int raim_fde(const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + double *azel, int *vsat, double *resp, char *msg) +{ + obsd_t *obs_e; + sol_t sol_e={}; + char tstr[32],name[16],msg_e[128]; + double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0; + int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0; + + trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n); + + if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0; + rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n); + svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n); + + for (i=0;irms) continue; + + /* save result */ + for (j=k=0;jlam[obs[i].sat-1][0]; + + if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) { + continue; + } + /* line-of-sight vector in ecef */ + cosel=cos(azel[1+i*2]); + a[0]=sin(azel[i*2])*cosel; + a[1]=cos(azel[i*2])*cosel; + a[2]=sin(azel[1+i*2]); + matmul("TN",3,1,3,1.0,E,a,0.0,e); + + /* satellite velocity relative to receiver in ecef */ + for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j]; + + /* range rate with earth rotation correction */ + rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]- + rs[3+i*6]*rr[1]-rs[ i*6]*x[1]); + + /* doppler residual */ + v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]); + + /* design matrix */ + for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0; + + nv++; + } + return nv; +} +/* estimate receiver velocity ------------------------------------------------*/ +void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + const double *azel, const int *vsat) +{ + double x[4]={0},dx[4],Q[16],*v,*H; + int i,j,nv; + + trace(3,"estvel : n=%d\n",n); + + v=mat(n,1); H=mat(4,n); + + for (i=0;irr,x,azel,vsat,v,H))<4) { + break; + } + /* least square estimation */ + if (lsq(H,v,4,nv,dx,Q)) break; + + for (j=0;j<4;j++) x[j]+=dx[j]; + + if (norm(dx,4)<1E-6) { + for (i=0;i<3;i++) sol->rr[i+3]=x[i]; + break; + } + } + free(v); free(H); +} +/* single-point positioning ---------------------------------------------------- +* compute receiver position, velocity, clock bias by single-point positioning +* with pseudorange and doppler observables +* args : obsd_t *obs I observation data +* int n I number of observation data +* nav_t *nav I navigation data +* prcopt_t *opt I processing options +* sol_t *sol IO solution +* double *azel IO azimuth/elevation angle (rad) (NULL: no output) +* ssat_t *ssat IO satellite status (NULL: no output) +* char *msg O error message for error exit +* return : status(1:ok,0:error) +* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and +* receiver bias are negligible (only involving glonass-gps time offset +* and receiver bias) +*-----------------------------------------------------------------------------*/ +int pntpos(const obsd_t *obs, int n, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, + char *msg) +{ +// int k=0; +// for (k=0;kn;k++) +// { +// printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n", +// k, +// nav->eph[k].sat, +// nav->eph[k].A, +// nav->eph[k].Adot, +// nav->eph[k].M0, +// nav->eph[k].OMG0, +// nav->eph[k].OMGd, +// nav->eph[k].cic, +// nav->eph[k].cis, +// nav->eph[k].code, +// nav->eph[k].crc, +// nav->eph[k].crs, +// nav->eph[k].cuc, +// nav->eph[k].cus, +// nav->eph[k].deln, +// nav->eph[k].e, +// nav->eph[k].f0, +// nav->eph[k].f1, +// nav->eph[k].f2, +// nav->eph[k].fit, +// nav->eph[k].flag, +// nav->eph[k].i0, +// nav->eph[k].idot, +// nav->eph[k].iodc, +// nav->eph[k].iode, +// nav->eph[k].ndot, +// nav->eph[k].omg, +// nav->eph[k].sat, +// nav->eph[k].sva, +// nav->eph[k].svh, +// nav->eph[k].tgd[0], +// nav->eph[k].toc.sec, +// nav->eph[k].toe.sec, +// nav->eph[k].toes, +// nav->eph[k].ttr.sec, +// nav->eph[k].week); +// } + + prcopt_t opt_=*opt; + double *rs,*dts,*var,*azel_,*resp; + int i,stat,vsat[MAXOBS]={0},svh[MAXOBS]; + + sol->stat=SOLQ_NONE; + + if (n<=0) {strcpy(msg,"no observation data"); return 0;} + + trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); + + sol->time=obs[0].time; msg[0]='\0'; + + rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n); + + if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */ +#if 0 + opt_.sateph =EPHOPT_BRDC; +#endif + opt_.ionoopt=IONOOPT_BRDC; + opt_.tropopt=TROPOPT_SAAS; + } + /* satellite positons, velocities and clocks */ + satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh); + + /* estimate receiver position with pseudorange */ + stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); + + /* raim fde */ + if (!stat&&n>=6&&opt->posopt[4]) { + stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); + } + /* estimate receiver velocity with doppler */ + if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); + + if (azel) { + for (i=0;i + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + * + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +* history : 2010/07/28 1.0 moved from rtkcmn.c +* changed api: +* pntpos() +* deleted api: +* pntvel() +* 2011/01/12 1.1 add option to include unhealthy satellite +* reject duplicated observation data +* changed api: ionocorr() +* 2011/11/08 1.2 enable snr mask for single-mode (rtklib_2.4.1_p3) +* 2012/12/25 1.3 add variable snr mask +* 2014/05/26 1.4 support galileo and beidou +* 2015/03/19 1.5 fix bug on ionosphere correction for GLO and BDS +*-----------------------------------------------------------------------------*/ + +#ifndef RTKLIB_PNTPOS_H_ +#define RTKLIB_PNTPOS_H_ + +#include "rtklib.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_ephemeris.h" +#include "rtklib_ionex.h" + +/* constants -----------------------------------------------------------------*/ + +#define SQR(x) ((x)*(x)) + +#define NX (4+3) /* # of estimated parameters */ + +#define MAXITR 10 /* max number of iteration for point pos */ +#define ERR_ION 5.0 /* ionospheric delay std (m) */ +#define ERR_TROP 3.0 /* tropspheric delay std (m) */ +#define ERR_SAAS 0.3 /* saastamoinen model error std (m) */ +#define ERR_BRDCI 0.5 /* broadcast iono model error factor */ +#define ERR_CBIAS 0.3 /* code bias error std (m) */ +#define REL_HUMI 0.7 /* relative humidity for saastamoinen model */ + +/* pseudorange measurement error variance ------------------------------------*/ +double varerr(const prcopt_t *opt, double el, int sys); + +/* get tgd parameter (m) -----------------------------------------------------*/ +double gettgd(int sat, const nav_t *nav); + +/* psendorange with code bias correction -------------------------------------*/ +double prange(const obsd_t *obs, const nav_t *nav, const double *azel, + int iter, const prcopt_t *opt, double *var); + +/* ionospheric correction ------------------------------------------------------ +* compute ionospheric correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* int sat I satellite number +* double *pos I receiver position {lat,lon,h} (rad|m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* int ionoopt I ionospheric correction option (IONOOPT_???) +* double *ion O ionospheric delay (L1) (m) +* double *var O ionospheric delay (L1) variance (m^2) +* return : status(1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, + const double *azel, int ionoopt, double *ion, double *var); +/* tropospheric correction ----------------------------------------------------- +* compute tropospheric correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* double *pos I receiver position {lat,lon,h} (rad|m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* int tropopt I tropospheric correction option (TROPOPT_???) +* double *trp O tropospheric delay (m) +* double *var O tropospheric delay variance (m^2) +* return : status(1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int tropcorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, int tropopt, double *trp, double *var); + +/* pseudorange residuals -----------------------------------------------------*/ +int rescode(int iter, const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, const prcopt_t *opt, + double *v, double *H, double *var, double *azel, int *vsat, + double *resp, int *ns); + +/* validate solution ---------------------------------------------------------*/ +int valsol(const double *azel, const int *vsat, int n, + const prcopt_t *opt, const double *v, int nv, int nx, + char *msg); + +/* estimate receiver position ------------------------------------------------*/ +int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, + const double *vare, const int *svh, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, + double *resp, char *msg); + +/* raim fde (failure detection and exclution) -------------------------------*/ +int raim_fde(const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + double *azel, int *vsat, double *resp, char *msg); + +/* doppler residuals ---------------------------------------------------------*/ +int resdop(const obsd_t *obs, int n, const double *rs, const double *dts, + const nav_t *nav, const double *rr, const double *x, + const double *azel, const int *vsat, double *v, double *H); + +/* estimate receiver velocity ------------------------------------------------*/ +void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + const double *azel, const int *vsat); + +/*! +* \brief single-point positioning +* compute receiver position, velocity, clock bias by single-point positioning +* with pseudorange and doppler observables +* args : obsd_t *obs I observation data +* int n I number of observation data +* nav_t *nav I navigation data +* prcopt_t *opt I processing options +* sol_t *sol IO solution +* double *azel IO azimuth/elevation angle (rad) (NULL: no output) +* ssat_t *ssat IO satellite status (NULL: no output) +* char *msg O error message for error exit +* return : status(1:ok,0:error) +* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and +* receiver bias are negligible (only involving glonass-gps time offset +* and receiver bias) +*/ +int pntpos(const obsd_t *obs, int n, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, + char *msg); + +#endif /* RTKLIB_PNTPOS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.cc b/src/algorithms/libs/rtklib/rtklib_preceph.cc new file mode 100644 index 000000000..080920b2a --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_preceph.cc @@ -0,0 +1,788 @@ +/*! + * \file rtklib_preceph.cc + * \brief precise ephemeris and clock functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +* +* references : +* [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c), +* 12 February, 2007 +* [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information, +* 27 August, 1998 +* [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996 +* [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed, +* Space Technology Library, 2004 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2009/01/18 1.0 new +* 2009/01/31 1.1 fix bug on numerical error to read sp3a ephemeris +* 2009/05/15 1.2 support glonass,galileo,qzs +* 2009/12/11 1.3 support wild-card expansion of file path +* 2010/07/21 1.4 added api: +* eci2ecef(),sunmoonpos(),peph2pos(),satantoff(), +* readdcb() +* changed api: +* readsp3() +* deleted api: +* eph2posp() +* 2010/09/09 1.5 fix problem when precise clock outage +* 2011/01/23 1.6 support qzss satellite code +* 2011/09/12 1.7 fix problem on precise clock outage +* move sunmmonpos() to rtkcmn.c +* 2011/12/01 1.8 modify api readsp3() +* precede later ephemeris if ephemeris is NULL +* move eci2ecef() to rtkcmn.c +* 2013/05/08 1.9 fix bug on computing std-dev of precise clocks +* 2013/11/20 1.10 modify option for api readsp3() +* 2014/04/03 1.11 accept extenstion including sp3,eph,SP3,EPH +* 2014/05/23 1.12 add function to read sp3 velocity records +* change api: satantoff() +* 2014/08/31 1.13 add member cov and vco in peph_t sturct +* 2014/10/13 1.14 fix bug on clock error variance in peph2pos() +* 2015/05/10 1.15 add api readfcb() +* modify api readdcb() +*-----------------------------------------------------------------------------*/ +#include "rtklib_preceph.h" + + +/* satellite code to satellite system ----------------------------------------*/ +int code2sys(char code) +{ + if (code=='G'||code==' ') return SYS_GPS; + if (code=='R') return SYS_GLO; + if (code=='E') return SYS_GAL; /* extension to sp3-c */ + if (code=='J') return SYS_QZS; /* extension to sp3-c */ + if (code=='C') return SYS_CMP; /* extension to sp3-c */ + if (code=='L') return SYS_LEO; /* extension to sp3-c */ + return SYS_NONE; +} +/* read sp3 header -----------------------------------------------------------*/ +int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats, + double *bfact, char *tsys) +{ + int i,j,k=0,ns=0,sys,prn; + char buff[1024]; + + trace(3,"readsp3h:\n"); + + for (i=0;i<22;i++) { + if (!fgets(buff,sizeof(buff),fp)) break; + + if (i==0) { + *type=buff[2]; + if (str2time(buff,3,28,time)) return 0; + } + else if (2<=i&&i<=6) { + if (i==2) { + ns=(int)str2num(buff,4,2); + } + for (j=0;j<17&&kne>=nav->nemax) { + nav->nemax+=256; + if (!(nav_peph=(peph_t *)realloc(nav->peph,sizeof(peph_t)*nav->nemax))) { + trace(1,"readsp3b malloc error n=%d\n",nav->nemax); + free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0; + return 0; + } + nav->peph=nav_peph; + } + nav->peph[nav->ne++]=*peph; + return 1; +} +/* read sp3 body -------------------------------------------------------------*/ +void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, + char *tsys, int index, int opt, nav_t *nav) +{ + peph_t peph; + gtime_t time; + double val,std,base; + int i,j,sat,sys,prn,n=ns*(type=='P'?1:2),pred_o,pred_c,v; + char buff[1024]; + + trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt); + + while (fgets(buff,sizeof(buff),fp)) { + + if (!strncmp(buff,"EOF",3)) break; + + if (buff[0]!='*'||str2time(buff,3,28,&time)) { + trace(2,"sp3 invalid epoch %31.31s\n",buff); + continue; + } + if (!strcmp(tsys,"UTC")) time=utc2gpst(time); /* utc->gpst */ + peph.time =time; + peph.index=index; + + for (i=0;i=76&&buff[75]=='P'; + pred_o=strlen(buff)>=80&&buff[79]=='P'; + } + for (j=0;j<4;j++) { + + /* read option for predicted value */ + if (j< 3&&(opt&1)&& pred_o) continue; + if (j< 3&&(opt&2)&&!pred_o) continue; + if (j==3&&(opt&1)&& pred_c) continue; + if (j==3&&(opt&2)&&!pred_c) continue; + + val=str2num(buff, 4+j*14,14); + std=str2num(buff,61+j* 3,j<3?2:3); + + if (buff[0]=='P') { /* position */ + if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { + peph.pos[sat-1][j]=val*(j<3?1000.0:1E-6); + v=1; /* valid epoch */ + } + if ((base=bfact[j<3?0:1])>0.0&&std>0.0) { + peph.std[sat-1][j]=(float)(pow(base,std)*(j<3?1E-3:1E-12)); + } + } + else if (v) { /* velocity */ + if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { + peph.vel[sat-1][j]=val*(j<3?0.1:1E-10); + } + if ((base=bfact[j<3?0:1])>0.0&&std>0.0) { + peph.vst[sat-1][j]=(float)(pow(base,std)*(j<3?1E-7:1E-16)); + } + } + } + } + if (v) { + if (!addpeph(nav,&peph)) return; + } + } +} +/* compare precise ephemeris -------------------------------------------------*/ +int cmppeph(const void *p1, const void *p2) +{ + peph_t *q1=(peph_t *)p1,*q2=(peph_t *)p2; + double tt=timediff(q1->time,q2->time); + return tt<-1E-9?-1:(tt>1E-9?1:q1->index-q2->index); +} +/* combine precise ephemeris -------------------------------------------------*/ +void combpeph(nav_t *nav, int opt) +{ + int i,j,k,m; + + trace(3,"combpeph: ne=%d\n",nav->ne); + + qsort(nav->peph,nav->ne,sizeof(peph_t),cmppeph); + + if (opt&4) return; + + for (i=0,j=1;jne;j++) { + + if (fabs(timediff(nav->peph[i].time,nav->peph[j].time))<1E-9) { + + for (k=0;kpeph[j].pos[k],4)<=0.0) continue; + for (m=0;m<4;m++) nav->peph[i].pos[k][m]=nav->peph[j].pos[k][m]; + for (m=0;m<4;m++) nav->peph[i].std[k][m]=nav->peph[j].std[k][m]; + for (m=0;m<4;m++) nav->peph[i].vel[k][m]=nav->peph[j].vel[k][m]; + for (m=0;m<4;m++) nav->peph[i].vst[k][m]=nav->peph[j].vst[k][m]; + } + } + else if (++ipeph[i]=nav->peph[j]; + } + nav->ne=i+1; + + trace(4,"combpeph: ne=%d\n",nav->ne); +} +/* read sp3 precise ephemeris file --------------------------------------------- +* read sp3 precise ephemeris/clock files and set them to navigation data +* args : char *file I sp3-c precise ephemeris file +* (wind-card * is expanded) +* nav_t *nav IO navigation data +* int opt I options (1: only observed + 2: only predicted + +* 4: not combined) +* return : none +* notes : see ref [1] +* precise ephemeris is appended and combined +* nav->peph and nav->ne must by properly initialized before calling the +* function +* only files with extensions of .sp3, .SP3, .eph* and .EPH* are read +*-----------------------------------------------------------------------------*/ +void readsp3(const char *file, nav_t *nav, int opt) +{ + FILE *fp; + gtime_t time={}; + double bfact[2]={}; + int i,j,n,ns,sats[MAXSAT]={}; + char *efiles[MAXEXFILE],*ext,type=' ',tsys[4]=""; + + trace(3,"readpephs: file=%s\n",file); + + for (i=0;i=0;i--) free(efiles[i]); + return; + } + } + /* expand wild card in file path */ + n=expath(file,efiles,MAXEXFILE); + + for (i=j=0;ine>0) combpeph(nav,opt); +} +/* read satellite antenna parameters ------------------------------------------- +* read satellite antenna parameters +* args : char *file I antenna parameter file +* gtime_t time I time +* nav_t *nav IO navigation data +* return : status (1:ok,0:error) +* notes : only support antex format for the antenna parameter file +*-----------------------------------------------------------------------------*/ +int readsap(const char *file, gtime_t time, nav_t *nav) +{ + pcvs_t pcvs={}; + pcv_t pcv0={},*pcv; + int i; + + trace(3,"readsap : file=%s time=%s\n",file,time_str(time,0)); + + if (!readpcv(file,&pcvs)) return 0; + + for (i=0;ipcvs[i]=pcv?*pcv:pcv0; + } + free(pcvs.pcv); + return 1; +} +/* read dcb parameters file --------------------------------------------------*/ +int readdcbf(const char *file, nav_t *nav, const sta_t *sta) +{ + FILE *fp; + double cbias; + char buff[256],str1[32],str2[32]=""; + int i,j,sat,type=0; + + trace(3,"readdcbf: file=%s\n",file); + + if (!(fp=fopen(file,"r"))) { + trace(2,"dcb parameters file open error: %s\n",file); + return 0; + } + while (fgets(buff,sizeof(buff),fp)) { + + if (strstr(buff,"DIFFERENTIAL (P1-P2) CODE BIASES")) type=1; + else if (strstr(buff,"DIFFERENTIAL (P1-C1) CODE BIASES")) type=2; + else if (strstr(buff,"DIFFERENTIAL (P2-C2) CODE BIASES")) type=3; + + if (!type||sscanf(buff,"%s %s",str1,str2)<1) continue; + + if ((cbias=str2num(buff,26,9))==0.0) continue; + + if (sta&&(!strcmp(str1,"G")||!strcmp(str1,"R"))) { /* receiver dcb */ + for (i=0;irbias[i][j][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ + } + } + else if ((sat=satid2no(str1))) { /* satellite dcb */ + nav->cbias[sat-1][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ + } + } + fclose(fp); + + return 1; +} +/* read dcb parameters --------------------------------------------------------- +* read differential code bias (dcb) parameters +* args : char *file I dcb parameters file (wild-card * expanded) +* nav_t *nav IO navigation data +* sta_t *sta I station info data to inport receiver dcb +* (NULL: no use) +* return : status (1:ok,0:error) +* notes : currently only p1-c1 bias of code *.dcb file +*-----------------------------------------------------------------------------*/ +int readdcb(const char *file, nav_t *nav, const sta_t *sta) +{ + int i,j,n; + char *efiles[MAXEXFILE]={}; + + trace(3,"readdcb : file=%s\n",file); + + for (i=0;icbias[i][j]=0.0; + } + for (i=0;i=0;i--) free(efiles[i]); + return 0; + } + } + n=expath(file,efiles,MAXEXFILE); + + for (i=0;inf>0&&fabs(timediff(ts,nav->fcb[nav->nf-1].ts))<=1e-3) { + for (i=0;i<3;i++) { + nav->fcb[nav->nf-1].bias[sat-1][i]=bias[i]; + nav->fcb[nav->nf-1].std [sat-1][i]=std [i]; + } + return 1; + } + if (nav->nf>=nav->nfmax) { + nav->nfmax=nav->nfmax<=0?2048:nav->nfmax*2; + if (!(nav_fcb=(fcbd_t *)realloc(nav->fcb,sizeof(fcbd_t)*nav->nfmax))) { + free(nav->fcb); nav->nf=nav->nfmax=0; + return 0; + } + nav->fcb=nav_fcb; + } + for (i=0;ifcb[nav->nf].bias[i][j]=nav->fcb[nav->nf].std[i][j]=0.0; + } + for (i=0;i<3;i++) { + nav->fcb[nav->nf].bias[sat-1][i]=bias[i]; + nav->fcb[nav->nf].std [sat-1][i]=std [i]; + } + nav->fcb[nav->nf ].ts=ts; + nav->fcb[nav->nf++].te=te; + return 1; +} +/* read satellite fcb file ---------------------------------------------------*/ +int readfcbf(const char *file, nav_t *nav) +{ + FILE *fp; + gtime_t ts,te; + double ep1[6],ep2[6],bias[3]={},std[3]={}; + char buff[1024],str[32],*p; + int sat; + + trace(3,"readfcbf: file=%s\n",file); + + if (!(fp=fopen(file,"r"))) { + trace(2,"fcb parameters file open error: %s\n",file); + return 0; + } + while (fgets(buff,sizeof(buff),fp)) { + if ((p=strchr(buff,'#'))) *p='\0'; + if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s" + "%lf %lf %lf %lf %lf %lf",ep1,ep1+1,ep1+2,ep1+3,ep1+4,ep1+5, + ep2,ep2+1,ep2+2,ep2+3,ep2+4,ep2+5,str,bias,std,bias+1,std+1, + bias+2,std+2)<17) continue; + if (!(sat=satid2no(str))) continue; + ts=epoch2time(ep1); + te=epoch2time(ep2); + if (!addfcb(nav,ts,te,sat,bias,std)) return 0; + } + fclose(fp); + return 1; +} +/* compare satellite fcb -----------------------------------------------------*/ +int cmpfcb(const void *p1, const void *p2) +{ + fcbd_t *q1=(fcbd_t *)p1,*q2=(fcbd_t *)p2; + double tt=timediff(q1->ts,q2->ts); + return tt<-1E-3?-1:(tt>1E-3?1:0); +} +/* read satellite fcb data ----------------------------------------------------- +* read satellite fractional cycle bias (dcb) parameters +* args : char *file I fcb parameters file (wild-card * expanded) +* nav_t *nav IO navigation data +* return : status (1:ok,0:error) +* notes : fcb data appended to navigation data +*-----------------------------------------------------------------------------*/ +int readfcb(const char *file, nav_t *nav) +{ + char *efiles[MAXEXFILE]={}; + int i,n; + + trace(3,"readfcb : file=%s\n",file); + + for (i=0;i=0;i--) free(efiles[i]); + return 0; + } + } + n=expath(file,efiles,MAXEXFILE); + + for (i=0;inf>1) { + qsort(nav->fcb,nav->nf,sizeof(fcbd_t),cmpfcb); + } + return 1; +} +/* polynomial interpolation by Neville's algorithm ---------------------------*/ +double interppol(const double *x, double *y, int n) +{ + int i,j; + + for (j=1;jnepeph[0].time)<-MAXDTE|| + timediff(time,nav->peph[nav->ne-1].time)>MAXDTE) { + trace(3,"no prec ephem %s sat=%2d\n",time_str(time,0),sat); + return 0; + } + /* binary search */ + for (i=0,j=nav->ne-1;ipeph[k].time,time)<0.0) i=k+1; else j=k; + } + index=i<=0?0:i-1; + + /* polynomial interpolation for orbit */ + i=index-(NMAX+1)/2; + if (i<0) i=0; else if (i+NMAX>=nav->ne) i=nav->ne-NMAX-1; + + for (j=0;j<=NMAX;j++) { + t[j]=timediff(nav->peph[i+j].time,time); + if (norm(nav->peph[i+j].pos[sat-1],3)<=0.0) { + trace(3,"prec ephem outage %s sat=%2d\n",time_str(time,0),sat); + return 0; + } + } + for (j=0;j<=NMAX;j++) { + pos=nav->peph[i+j].pos[sat-1]; +#if 0 + p[0][j]=pos[0]; + p[1][j]=pos[1]; +#else + /* correciton for earh rotation ver.2.4.0 */ + sinl=sin(OMGE*t[j]); + cosl=cos(OMGE*t[j]); + p[0][j]=cosl*pos[0]-sinl*pos[1]; + p[1][j]=sinl*pos[0]+cosl*pos[1]; +#endif + p[2][j]=pos[2]; + } + for (i=0;i<3;i++) { + rs[i]=interppol(t,p[i],NMAX+1); + } + if (vare) { + for (i=0;i<3;i++) s[i]=nav->peph[index].std[sat-1][i]; + std=norm(s,3); + + /* extrapolation error for orbit */ + if (t[0 ]>0.0) std+=EXTERR_EPH*SQR(t[0 ])/2.0; + else if (t[NMAX]<0.0) std+=EXTERR_EPH*SQR(t[NMAX])/2.0; + *vare=SQR(std); + } + /* linear interpolation for clock */ + t[0]=timediff(time,nav->peph[index ].time); + t[1]=timediff(time,nav->peph[index+1].time); + c[0]=nav->peph[index ].pos[sat-1][3]; + c[1]=nav->peph[index+1].pos[sat-1][3]; + + if (t[0]<=0.0) { + if ((dts[0]=c[0])!=0.0) { + std=nav->peph[index].std[sat-1][3]*CLIGHT-EXTERR_CLK*t[0]; + } + } + else if (t[1]>=0.0) { + if ((dts[0]=c[1])!=0.0) { + std=nav->peph[index+1].std[sat-1][3]*CLIGHT+EXTERR_CLK*t[1]; + } + } + else if (c[0]!=0.0&&c[1]!=0.0) { + dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); + i=t[0]<-t[1]?0:1; + std=nav->peph[index+i].std[sat-1][3]+EXTERR_CLK*fabs(t[i]); + } + else { + dts[0]=0.0; + } + if (varc) *varc=SQR(std); + return 1; +} +/* satellite clock by precise clock ------------------------------------------*/ +int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, + double *varc) +{ + double t[2],c[2],std; + int i,j,k,index; + + trace(4,"pephclk : time=%s sat=%2d\n",time_str(time,3),sat); + + if (nav->nc<2|| + timediff(time,nav->pclk[0].time)<-MAXDTE|| + timediff(time,nav->pclk[nav->nc-1].time)>MAXDTE) { + trace(3,"no prec clock %s sat=%2d\n",time_str(time,0),sat); + return 1; + } + /* binary search */ + for (i=0,j=nav->nc-1;ipclk[k].time,time)<0.0) i=k+1; else j=k; + } + index=i<=0?0:i-1; + + /* linear interpolation for clock */ + t[0]=timediff(time,nav->pclk[index ].time); + t[1]=timediff(time,nav->pclk[index+1].time); + c[0]=nav->pclk[index ].clk[sat-1][0]; + c[1]=nav->pclk[index+1].clk[sat-1][0]; + + if (t[0]<=0.0) { + if ((dts[0]=c[0])==0.0) return 0; + std=nav->pclk[index].std[sat-1][0]*CLIGHT-EXTERR_CLK*t[0]; + } + else if (t[1]>=0.0) { + if ((dts[0]=c[1])==0.0) return 0; + std=nav->pclk[index+1].std[sat-1][0]*CLIGHT+EXTERR_CLK*t[1]; + } + else if (c[0]!=0.0&&c[1]!=0.0) { + dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); + i=t[0]<-t[1]?0:1; + std=nav->pclk[index+i].std[sat-1][0]*CLIGHT+EXTERR_CLK*fabs(t[i]); + } + else { + trace(3,"prec clock outage %s sat=%2d\n",time_str(time,0),sat); + return 0; + } + if (varc) *varc=SQR(std); + return 1; +} +/* satellite antenna phase center offset --------------------------------------- +* compute satellite antenna phase center offset in ecef +* args : gtime_t time I time (gpst) +* double *rs I satellite position and velocity (ecef) +* {x,y,z,vx,vy,vz} (m|m/s) +* int sat I satellite number +* nav_t *nav I navigation data +* double *dant I satellite antenna phase center offset (ecef) +* {dx,dy,dz} (m) (iono-free LC value) +* return : none +*-----------------------------------------------------------------------------*/ +void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav, + double *dant) +{ + const double *lam=nav->lam[sat-1]; + const pcv_t *pcv=nav->pcvs+sat-1; + double ex[3],ey[3],ez[3],es[3],r[3],rsun[3],gmst,erpv[5]={}; + double gamma,C1,C2,dant1,dant2; + int i,j=0,k=1; + + trace(4,"satantoff: time=%s sat=%2d\n",time_str(time,3),sat); + + /* sun position in ecef */ + sunmoonpos(gpst2utc(time),erpv,rsun,NULL,&gmst); + + /* unit vectors of satellite fixed coordinates */ + for (i=0;i<3;i++) r[i]=-rs[i]; + if (!normv3(r,ez)) return; + for (i=0;i<3;i++) r[i]=rsun[i]-rs[i]; + if (!normv3(r,es)) return; + cross3(ez,es,r); + if (!normv3(r,ey)) return; + cross3(ey,ez,ex); + + if (NFREQ>=3&&(satsys(sat,NULL)&(SYS_GAL|SYS_SBS))) k=2; + + if (NFREQ<2||lam[j]==0.0||lam[k]==0.0) return; + + gamma=SQR(lam[k])/SQR(lam[j]); + C1=gamma/(gamma-1.0); + C2=-1.0 /(gamma-1.0); + + /* iono-free LC */ + for (i=0;i<3;i++) { + dant1=pcv->off[j][0]*ex[i]+pcv->off[j][1]*ey[i]+pcv->off[j][2]*ez[i]; + dant2=pcv->off[k][0]*ex[i]+pcv->off[k][1]*ey[i]+pcv->off[k][2]*ez[i]; + dant[i]=C1*dant1+C2*dant2; + } +} +/* satellite position/clock by precise ephemeris/clock ------------------------- +* compute satellite position/clock with precise ephemeris/clock +* args : gtime_t time I time (gpst) +* int sat I satellite number +* nav_t *nav I navigation data +* int opt I sat postion option +* (0: center of mass, 1: antenna phase center) +* double *rs O sat position and velocity (ecef) +* {x,y,z,vx,vy,vz} (m|m/s) +* double *dts O sat clock {bias,drift} (s|s/s) +* double *var IO sat position and clock error variance (m) +* (NULL: no output) +* return : status (1:ok,0:error or data outage) +* notes : clock includes relativistic correction but does not contain code bias +* before calling the function, nav->peph, nav->ne, nav->pclk and +* nav->nc must be set by calling readsp3(), readrnx() or readrnxt() +* if precise clocks are not set, clocks in sp3 are used instead +*-----------------------------------------------------------------------------*/ +int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, + double *rs, double *dts, double *var) +{ + double rss[3],rst[3],dtss[1],dtst[1],dant[3]={},vare=0.0,varc=0.0,tt=1E-3; + int i; + + trace(4,"peph2pos: time=%s sat=%2d opt=%d\n",time_str(time,3),sat,opt); + + if (sat<=0||MAXSAT + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + * + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +* +* references : +* [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c), +* 12 February, 2007 +* [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information, +* 27 August, 1998 +* [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996 +* [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed, +* Space Technology Library, 2004 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2009/01/18 1.0 new +* 2009/01/31 1.1 fix bug on numerical error to read sp3a ephemeris +* 2009/05/15 1.2 support glonass,galileo,qzs +* 2009/12/11 1.3 support wild-card expansion of file path +* 2010/07/21 1.4 added api: +* eci2ecef(),sunmoonpos(),peph2pos(),satantoff(), +* readdcb() +* changed api: +* readsp3() +* deleted api: +* eph2posp() +* 2010/09/09 1.5 fix problem when precise clock outage +* 2011/01/23 1.6 support qzss satellite code +* 2011/09/12 1.7 fix problem on precise clock outage +* move sunmmonpos() to rtkcmn.c +* 2011/12/01 1.8 modify api readsp3() +* precede later ephemeris if ephemeris is NULL +* move eci2ecef() to rtkcmn.c +* 2013/05/08 1.9 fix bug on computing std-dev of precise clocks +* 2013/11/20 1.10 modify option for api readsp3() +* 2014/04/03 1.11 accept extenstion including sp3,eph,SP3,EPH +* 2014/05/23 1.12 add function to read sp3 velocity records +* change api: satantoff() +* 2014/08/31 1.13 add member cov and vco in peph_t sturct +* 2014/10/13 1.14 fix bug on clock error variance in peph2pos() +* 2015/05/10 1.15 add api readfcb() +* modify api readdcb() +*-----------------------------------------------------------------------------*/ +#ifndef RTKLIB_PRECEPH_H_ +#define RTKLIB_PRECEPH_H_ + +#include "rtklib.h" +#include "rtklib_rtkcmn.h" + +#define SQR(x) ((x)*(x)) + +#define NMAX 10 /* order of polynomial interpolation */ +#define MAXDTE 900.0 /* max time difference to ephem time (s) */ +#define EXTERR_CLK 1E-3 /* extrapolation error for clock (m/s) */ +#define EXTERR_EPH 5E-7 /* extrapolation error for ephem (m/s^2) */ + +int code2sys(char code); +int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats, + double *bfact, char *tsys); +int addpeph(nav_t *nav, peph_t *peph); +void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, + char *tsys, int index, int opt, nav_t *nav); +int cmppeph(const void *p1, const void *p2); +void combpeph(nav_t *nav, int opt); +void readsp3(const char *file, nav_t *nav, int opt); +int readsap(const char *file, gtime_t time, nav_t *nav); +int readdcbf(const char *file, nav_t *nav, const sta_t *sta); +int readdcb(const char *file, nav_t *nav, const sta_t *sta); +int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat, + const double *bias, const double *std); +int readfcbf(const char *file, nav_t *nav); +int readdcb(const char *file, nav_t *nav, const sta_t *sta); +int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat, + const double *bias, const double *std); +int readfcbf(const char *file, nav_t *nav); +int cmpfcb(const void *p1, const void *p2); +int readfcb(const char *file, nav_t *nav); +double interppol(const double *x, double *y, int n); +int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs, + double *dts, double *vare, double *varc); + +int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, + double *varc); + +void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav, + double *dant); +int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, + double *rs, double *dts, double *var); + +#endif //RTKLIB_PRECEPH_H_ diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc new file mode 100644 index 000000000..7bd734016 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -0,0 +1,3718 @@ +/*! + * \file rtklib_rtkcmn.cc + * \brief rtklib common functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +* +* options : -DLAPACK use LAPACK/BLAS +* -DMKL use Intel MKL +* -DTRACE enable debug trace +* -DWIN32 use WIN32 API +* -DNOCALLOC no use calloc for zero matrix +* -DIERS_MODEL use GMF instead of NMF +* -DDLL built for shared library +* -DCPUTIME_IN_GPST cputime operated in gpst +* +* references : +* [1] IS-GPS-200D, Navstar GPS Space Segment/Navigation User Interfaces, +* 7 March, 2006 +* [2] RTCA/DO-229C, Minimum operational performanc standards for global +* positioning system/wide area augmentation system airborne equipment, +* RTCA inc, November 28, 2001 +* [3] M.Rothacher, R.Schmid, ANTEX: The Antenna Exchange Format Version 1.4, +* 15 September, 2010 +* [4] A.Gelb ed., Applied Optimal Estimation, The M.I.T Press, 1974 +* [5] A.E.Niell, Global mapping functions for the atmosphere delay at radio +* wavelengths, Jounal of geophysical research, 1996 +* [6] W.Gurtner and L.Estey, RINEX The Receiver Independent Exchange Format +* Version 3.00, November 28, 2007 +* [7] J.Kouba, A Guide to using International GNSS Service (IGS) products, +* May 2009 +* [8] China Satellite Navigation Office, BeiDou navigation satellite system +* signal in space interface control document, open service signal B1I +* (version 1.0), Dec 2012 +* [9] J.Boehm, A.Niell, P.Tregoning and H.Shuh, Global Mapping Function +* (GMF): A new empirical mapping function base on numerical weather +* model data, Geophysical Research Letters, 33, L07304, 2006 +* [10] GLONASS/GPS/Galileo/Compass/SBAS NV08C receiver series BINR interface +* protocol specification ver.1.3, August, 2012 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2007/01/12 1.0 new +* 2007/03/06 1.1 input initial rover pos of pntpos() +* update only effective states of filter() +* fix bug of atan2() domain error +* 2007/04/11 1.2 add function antmodel() +* add gdop mask for pntpos() +* change constant MAXDTOE value +* 2007/05/25 1.3 add function execcmd(),expandpath() +* 2008/06/21 1.4 add funciton sortobs(),uniqeph(),screent() +* replace geodist() by sagnac correction way +* 2008/10/29 1.5 fix bug of ionosphereic mapping function +* fix bug of seasonal variation term of tropmapf +* 2008/12/27 1.6 add function tickget(), sleepms(), tracenav(), +* xyz2enu(), satposv(), pntvel(), covecef() +* 2009/03/12 1.7 fix bug on error-stop when localtime() returns NULL +* 2009/03/13 1.8 fix bug on time adjustment for summer time +* 2009/04/10 1.9 add function adjgpsweek(),getbits(),getbitu() +* add function geph2pos() +* 2009/06/08 1.10 add function seph2pos() +* 2009/11/28 1.11 change function pntpos() +* add function tracegnav(),tracepeph() +* 2009/12/22 1.12 change default parameter of ionos std +* valid under second for timeget() +* 2010/07/28 1.13 fix bug in tropmapf() +* added api: +* obs2code(),code2obs(),cross3(),normv3(), +* gst2time(),time2gst(),time_str(),timeset(), +* deg2dms(),dms2deg(),searchpcv(),antmodel_s(), +* tracehnav(),tracepclk(),reppath(),reppaths(), +* createdir() +* changed api: +* readpcv(), +* deleted api: +* uniqeph() +* 2010/08/20 1.14 omit to include mkl header files +* fix bug on chi-sqr(n) table +* 2010/12/11 1.15 added api: +* freeobs(),freenav(),ionppp() +* 2011/05/28 1.16 fix bug on half-hour offset by time2epoch() +* added api: +* uniqnav() +* 2012/06/09 1.17 add a leap second after 2012-6-30 +* 2012/07/15 1.18 add api setbits(),setbitu(),utc2gmst() +* fix bug on interpolation of antenna pcv +* fix bug on str2num() for string with over 256 char +* add api readblq(),satexclude(),setcodepri(), +* getcodepri() +* change api obs2code(),code2obs(),antmodel() +* 2012/12/25 1.19 fix bug on satwavelen(),code2obs(),obs2code() +* add api testsnr() +* 2013/01/04 1.20 add api gpst2bdt(),bdt2gpst(),bdt2time(),time2bdt() +* readblq(),readerp(),geterp(),crc16() +* change api eci2ecef(),sunmoonpos() +* 2013/03/26 1.21 tickget() uses clock_gettime() for linux +* 2013/05/08 1.22 fix bug on nutation coefficients for ast_args() +* 2013/06/02 1.23 add #ifdef for undefined CLOCK_MONOTONIC_RAW +* 2013/09/01 1.24 fix bug on interpolation of satellite antenna pcv +* 2013/09/06 1.25 fix bug on extrapolation of erp +* 2014/04/27 1.26 add SYS_LEO for satellite system +* add BDS L1 code for RINEX 3.02 and RTCM 3.2 +* support BDS L1 in satwavelen() +* 2014/05/29 1.27 fix bug on obs2code() to search obs code table +* 2014/08/26 1.28 fix problem on output of uncompress() for tar file +* add function to swap trace file with keywords +* 2014/10/21 1.29 strtok() -> strtok_r() in expath() for thread-safe +* add bdsmodear in procopt_default +* 2015/03/19 1.30 fix bug on interpolation of erp values in geterp() +* add leap second insertion before 2015/07/01 00:00 +* add api read_leaps() +* 2015/05/31 1.31 delte api windupcorr() +* 2015/08/08 1.32 add compile option CPUTIME_IN_GPST +* add api add_fatal() +* support usno leapsec.dat for api read_leaps() +* 2016/01/23 1.33 enable septentrio +* 2016/02/05 1.34 support GLONASS for savenav(), loadnav() +* 2016/06/11 1.35 delete trace() in reppath() to avoid deadlock +* 2016/07/01 1.36 support IRNSS +* add leap second before 2017/1/1 00:00:00 +* 2016/07/29 1.37 rename api compress() -> rtk_uncompress() +* rename api crc16() -> rtk_crc16() +* rename api crc24q() -> rtk_crc24q() +* rename api crc32() -> rtk_crc32() +* 2016/08/20 1.38 fix type incompatibility in win64 environment +* change constant _POSIX_C_SOURCE 199309 -> 199506 +* 2016/08/21 1.39 fix bug on week overflow in time2gpst()/gpst2time() +* 2016/09/05 1.40 fix bug on invalid nav data read in readnav() +* 2016/09/17 1.41 suppress warnings +* 2016/09/19 1.42 modify api deg2dms() to consider numerical error +*-----------------------------------------------------------------------------*/ +#include +#include +#include +#include +#include +#include +#include +#include "rtklib_rtkcmn.h" + +const double gpst0[]={1980,1, 6,0,0,0}; /* gps time reference */ +const double gst0 []={1999,8,22,0,0,0}; /* galileo system time reference */ +const double bdt0 []={2006,1, 1,0,0,0}; /* beidou time reference */ + +double leaps[MAXLEAPS+1][7]={ /* leap seconds (y,m,d,h,m,s,utc-gpst) */ + {2017,1,1,0,0,0,-18}, + {2015,7,1,0,0,0,-17}, + {2012,7,1,0,0,0,-16}, + {2009,1,1,0,0,0,-15}, + {2006,1,1,0,0,0,-14}, + {1999,1,1,0,0,0,-13}, + {1997,7,1,0,0,0,-12}, + {1996,1,1,0,0,0,-11}, + {1994,7,1,0,0,0,-10}, + {1993,7,1,0,0,0, -9}, + {1992,7,1,0,0,0, -8}, + {1991,1,1,0,0,0, -7}, + {1990,1,1,0,0,0, -6}, + {1988,1,1,0,0,0, -5}, + {1985,7,1,0,0,0, -4}, + {1983,7,1,0,0,0, -3}, + {1982,7,1,0,0,0, -2}, + {1981,7,1,0,0,0, -1}, + {} +}; + +const prcopt_t prcopt_default={ /* defaults processing options */ + PMODE_SINGLE,0,2,SYS_GPS, /* mode,soltype,nf,navsys */ + 15.0*D2R,{}, /* elmin,snrmask */ + 0,1,1,1, /* sateph,modear,glomodear,bdsmodear */ + 5,0,10,1, /* maxout,minlock,minfix,armaxiter */ + 0,0,0,0, /* estion,esttrop,dynamics,tidecorr */ + 1,0,0,0,0, /* niter,codesmooth,intpref,sbascorr,sbassatsel */ + 0,0, /* rovpos,refpos */ + {100.0,100.0}, /* eratio[] */ + {100.0,0.003,0.003,0.0,1.0}, /* err[] */ + {30.0,0.03,0.3}, /* std[] */ + {1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */ + 5E-12, /* sclkstab */ + {3.0,0.9999,0.25,0.1,0.05}, /* thresar */ + 0.0,0.0,0.05, /* elmaskar,almaskhold,thresslip */ + 30.0,30.0,30.0, /* maxtdif,maxinno,maxgdop */ + {},{},{}, /* baseline,ru,rb */ + {"",""}, /* anttype */ + {},{},{}, /* antdel,pcv,exsats */ + {},{},{},{},{},{},{},{},{},{} + }; + +const solopt_t solopt_default={ /* defaults solution output options */ + SOLF_LLH,TIMES_GPST,1,3, /* posf,times,timef,timeu */ + 0,1,0,0,0,0, /* degf,outhead,outopt,datum,height,geoid */ + 0,0,0, /* solstatic,sstat,trace */ + {0.0,0.0}, /* nmeaintv */ + " ","" /* separator/program name */ +}; +const char *formatstrs[32]={ /* stream format strings */ + "RTCM 2", /* 0 */ + "RTCM 3", /* 1 */ + "NovAtel OEM6", /* 2 */ + "NovAtel OEM3", /* 3 */ + "u-blox", /* 4 */ + "Superstar II", /* 5 */ + "Hemisphere", /* 6 */ + "SkyTraq", /* 7 */ + "GW10", /* 8 */ + "Javad", /* 9 */ + "NVS BINR", /* 10 */ + "BINEX", /* 11 */ + "Trimble RT17", /* 12 */ + "Septentrio", /* 13 */ + "CMR/CMR+", /* 14 */ + "LEX Receiver", /* 15 */ + "RINEX", /* 16 */ + "SP3", /* 17 */ + "RINEX CLK", /* 18 */ + "SBAS", /* 19 */ + "NMEA 0183", /* 20 */ + NULL +}; +char obscodes[][3]={ /* observation code strings */ + + "" ,"1C","1P","1W","1Y", "1M","1N","1S","1L","1E", /* 0- 9 */ + "1A","1B","1X","1Z","2C", "2D","2S","2L","2X","2P", /* 10-19 */ + "2W","2Y","2M","2N","5I", "5Q","5X","7I","7Q","7X", /* 20-29 */ + "6A","6B","6C","6X","6Z", "6S","6L","8L","8Q","8X", /* 30-39 */ + "2I","2Q","6I","6Q","3I", "3Q","3X","1I","1Q","5A", /* 40-49 */ + "5B","5C","9A","9B","9C", "9X","" ,"" ,"" ,"" /* 50-59 */ +}; +unsigned char obsfreqs[]={ + /* 1:L1/E1, 2:L2/B1, 3:L5/E5a/L3, 4:L6/LEX/B3, 5:E5b/B2, 6:E5(a+b), 7:S */ + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 0- 9 */ + 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, /* 10-19 */ + 2, 2, 2, 2, 3, 3, 3, 5, 5, 5, /* 20-29 */ + 4, 4, 4, 4, 4, 4, 4, 6, 6, 6, /* 30-39 */ + 2, 2, 4, 4, 3, 3, 3, 1, 1, 3, /* 40-49 */ + 3, 3, 7, 7, 7, 7, 0, 0, 0, 0 /* 50-59 */ +}; +char codepris[7][MAXFREQ][16]={ /* code priority table */ + + /* L1/E1 L2/B1 L5/E5a/L3 L6/LEX/B3 E5b/B2 E5(a+b) S */ + {"CPYWMNSL","PYWCMNDSLX","IQX" ,"" ,"" ,"" ,"" }, /* GPS */ + {"PC" ,"PC" ,"IQX" ,"" ,"" ,"" ,"" }, /* GLO */ + {"CABXZ" ,"" ,"IQX" ,"ABCXZ" ,"IQX" ,"IQX" ,"" }, /* GAL */ + {"CSLXZ" ,"SLX" ,"IQX" ,"SLX" ,"" ,"" ,"" }, /* QZS */ + {"C" ,"" ,"IQX" ,"" ,"" ,"" ,"" }, /* SBS */ + {"IQX" ,"IQX" ,"IQX" ,"IQX" ,"IQX" ,"" ,"" }, /* BDS */ + {"" ,"" ,"ABCX" ,"" ,"" ,"" ,"ABCX"} /* IRN */ +}; +fatalfunc_t *fatalfunc=NULL; /* fatal callback function */ + +/* crc tables generated by util/gencrc ---------------------------------------*/ +const unsigned short tbl_CRC16[]={ + 0x0000,0x1021,0x2042,0x3063,0x4084,0x50A5,0x60C6,0x70E7, + 0x8108,0x9129,0xA14A,0xB16B,0xC18C,0xD1AD,0xE1CE,0xF1EF, + 0x1231,0x0210,0x3273,0x2252,0x52B5,0x4294,0x72F7,0x62D6, + 0x9339,0x8318,0xB37B,0xA35A,0xD3BD,0xC39C,0xF3FF,0xE3DE, + 0x2462,0x3443,0x0420,0x1401,0x64E6,0x74C7,0x44A4,0x5485, + 0xA56A,0xB54B,0x8528,0x9509,0xE5EE,0xF5CF,0xC5AC,0xD58D, + 0x3653,0x2672,0x1611,0x0630,0x76D7,0x66F6,0x5695,0x46B4, + 0xB75B,0xA77A,0x9719,0x8738,0xF7DF,0xE7FE,0xD79D,0xC7BC, + 0x48C4,0x58E5,0x6886,0x78A7,0x0840,0x1861,0x2802,0x3823, + 0xC9CC,0xD9ED,0xE98E,0xF9AF,0x8948,0x9969,0xA90A,0xB92B, + 0x5AF5,0x4AD4,0x7AB7,0x6A96,0x1A71,0x0A50,0x3A33,0x2A12, + 0xDBFD,0xCBDC,0xFBBF,0xEB9E,0x9B79,0x8B58,0xBB3B,0xAB1A, + 0x6CA6,0x7C87,0x4CE4,0x5CC5,0x2C22,0x3C03,0x0C60,0x1C41, + 0xEDAE,0xFD8F,0xCDEC,0xDDCD,0xAD2A,0xBD0B,0x8D68,0x9D49, + 0x7E97,0x6EB6,0x5ED5,0x4EF4,0x3E13,0x2E32,0x1E51,0x0E70, + 0xFF9F,0xEFBE,0xDFDD,0xCFFC,0xBF1B,0xAF3A,0x9F59,0x8F78, + 0x9188,0x81A9,0xB1CA,0xA1EB,0xD10C,0xC12D,0xF14E,0xE16F, + 0x1080,0x00A1,0x30C2,0x20E3,0x5004,0x4025,0x7046,0x6067, + 0x83B9,0x9398,0xA3FB,0xB3DA,0xC33D,0xD31C,0xE37F,0xF35E, + 0x02B1,0x1290,0x22F3,0x32D2,0x4235,0x5214,0x6277,0x7256, + 0xB5EA,0xA5CB,0x95A8,0x8589,0xF56E,0xE54F,0xD52C,0xC50D, + 0x34E2,0x24C3,0x14A0,0x0481,0x7466,0x6447,0x5424,0x4405, + 0xA7DB,0xB7FA,0x8799,0x97B8,0xE75F,0xF77E,0xC71D,0xD73C, + 0x26D3,0x36F2,0x0691,0x16B0,0x6657,0x7676,0x4615,0x5634, + 0xD94C,0xC96D,0xF90E,0xE92F,0x99C8,0x89E9,0xB98A,0xA9AB, + 0x5844,0x4865,0x7806,0x6827,0x18C0,0x08E1,0x3882,0x28A3, + 0xCB7D,0xDB5C,0xEB3F,0xFB1E,0x8BF9,0x9BD8,0xABBB,0xBB9A, + 0x4A75,0x5A54,0x6A37,0x7A16,0x0AF1,0x1AD0,0x2AB3,0x3A92, + 0xFD2E,0xED0F,0xDD6C,0xCD4D,0xBDAA,0xAD8B,0x9DE8,0x8DC9, + 0x7C26,0x6C07,0x5C64,0x4C45,0x3CA2,0x2C83,0x1CE0,0x0CC1, + 0xEF1F,0xFF3E,0xCF5D,0xDF7C,0xAF9B,0xBFBA,0x8FD9,0x9FF8, + 0x6E17,0x7E36,0x4E55,0x5E74,0x2E93,0x3EB2,0x0ED1,0x1EF0 +}; +const unsigned int tbl_CRC24Q[]={ + 0x000000,0x864CFB,0x8AD50D,0x0C99F6,0x93E6E1,0x15AA1A,0x1933EC,0x9F7F17, + 0xA18139,0x27CDC2,0x2B5434,0xAD18CF,0x3267D8,0xB42B23,0xB8B2D5,0x3EFE2E, + 0xC54E89,0x430272,0x4F9B84,0xC9D77F,0x56A868,0xD0E493,0xDC7D65,0x5A319E, + 0x64CFB0,0xE2834B,0xEE1ABD,0x685646,0xF72951,0x7165AA,0x7DFC5C,0xFBB0A7, + 0x0CD1E9,0x8A9D12,0x8604E4,0x00481F,0x9F3708,0x197BF3,0x15E205,0x93AEFE, + 0xAD50D0,0x2B1C2B,0x2785DD,0xA1C926,0x3EB631,0xB8FACA,0xB4633C,0x322FC7, + 0xC99F60,0x4FD39B,0x434A6D,0xC50696,0x5A7981,0xDC357A,0xD0AC8C,0x56E077, + 0x681E59,0xEE52A2,0xE2CB54,0x6487AF,0xFBF8B8,0x7DB443,0x712DB5,0xF7614E, + 0x19A3D2,0x9FEF29,0x9376DF,0x153A24,0x8A4533,0x0C09C8,0x00903E,0x86DCC5, + 0xB822EB,0x3E6E10,0x32F7E6,0xB4BB1D,0x2BC40A,0xAD88F1,0xA11107,0x275DFC, + 0xDCED5B,0x5AA1A0,0x563856,0xD074AD,0x4F0BBA,0xC94741,0xC5DEB7,0x43924C, + 0x7D6C62,0xFB2099,0xF7B96F,0x71F594,0xEE8A83,0x68C678,0x645F8E,0xE21375, + 0x15723B,0x933EC0,0x9FA736,0x19EBCD,0x8694DA,0x00D821,0x0C41D7,0x8A0D2C, + 0xB4F302,0x32BFF9,0x3E260F,0xB86AF4,0x2715E3,0xA15918,0xADC0EE,0x2B8C15, + 0xD03CB2,0x567049,0x5AE9BF,0xDCA544,0x43DA53,0xC596A8,0xC90F5E,0x4F43A5, + 0x71BD8B,0xF7F170,0xFB6886,0x7D247D,0xE25B6A,0x641791,0x688E67,0xEEC29C, + 0x3347A4,0xB50B5F,0xB992A9,0x3FDE52,0xA0A145,0x26EDBE,0x2A7448,0xAC38B3, + 0x92C69D,0x148A66,0x181390,0x9E5F6B,0x01207C,0x876C87,0x8BF571,0x0DB98A, + 0xF6092D,0x7045D6,0x7CDC20,0xFA90DB,0x65EFCC,0xE3A337,0xEF3AC1,0x69763A, + 0x578814,0xD1C4EF,0xDD5D19,0x5B11E2,0xC46EF5,0x42220E,0x4EBBF8,0xC8F703, + 0x3F964D,0xB9DAB6,0xB54340,0x330FBB,0xAC70AC,0x2A3C57,0x26A5A1,0xA0E95A, + 0x9E1774,0x185B8F,0x14C279,0x928E82,0x0DF195,0x8BBD6E,0x872498,0x016863, + 0xFAD8C4,0x7C943F,0x700DC9,0xF64132,0x693E25,0xEF72DE,0xE3EB28,0x65A7D3, + 0x5B59FD,0xDD1506,0xD18CF0,0x57C00B,0xC8BF1C,0x4EF3E7,0x426A11,0xC426EA, + 0x2AE476,0xACA88D,0xA0317B,0x267D80,0xB90297,0x3F4E6C,0x33D79A,0xB59B61, + 0x8B654F,0x0D29B4,0x01B042,0x87FCB9,0x1883AE,0x9ECF55,0x9256A3,0x141A58, + 0xEFAAFF,0x69E604,0x657FF2,0xE33309,0x7C4C1E,0xFA00E5,0xF69913,0x70D5E8, + 0x4E2BC6,0xC8673D,0xC4FECB,0x42B230,0xDDCD27,0x5B81DC,0x57182A,0xD154D1, + 0x26359F,0xA07964,0xACE092,0x2AAC69,0xB5D37E,0x339F85,0x3F0673,0xB94A88, + 0x87B4A6,0x01F85D,0x0D61AB,0x8B2D50,0x145247,0x921EBC,0x9E874A,0x18CBB1, + 0xE37B16,0x6537ED,0x69AE1B,0xEFE2E0,0x709DF7,0xF6D10C,0xFA48FA,0x7C0401, + 0x42FA2F,0xC4B6D4,0xC82F22,0x4E63D9,0xD11CCE,0x575035,0x5BC9C3,0xDD8538 +}; + + +extern "C" { + void dgemm_(char *, char *, int *, int *, int *, double *, double *, + int *, double *, int *, double *, double *, int *); + extern void dgetrf_(int *, int *, double *, int *, int *, int *); + extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); + extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, + int *, int *); +} + + +/* function prototypes -------------------------------------------------------*/ + + +#ifdef IERS_MODEL +extern int gmf_(double *mjd, double *lat, double *lon, double *hgt, double *zd, + double *gmfh, double *gmfw); +#endif + +/* fatal error ---------------------------------------------------------------*/ +void fatalerr(const char *format, ...) +{ + char msg[1024]; + va_list ap; + va_start(ap,format); vsprintf(msg,format,ap); va_end(ap); + fprintf(stderr,"%s",msg); + exit(-9); +} + +/* satellite system+prn/slot number to satellite number ------------------------ +* convert satellite system+prn/slot number to satellite number +* args : int sys I satellite system (SYS_GPS,SYS_GLO,...) +* int prn I satellite prn/slot number +* return : satellite number (0:error) +*-----------------------------------------------------------------------------*/ +int satno(int sys, int prn) +{ + if (prn<=0) return 0; + switch (sys) { + case SYS_GPS: + if (prnexsats[sat-1]==1) return 1; /* excluded satellite */ + if (opt->exsats[sat-1]==2) return 0; /* included satellite */ + if (!(sys&opt->navsys)) return 1; /* unselected sat sys */ + } + if (sys==SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */ + if (svh) { + trace(3,"unhealthy satellite: sat=%3d svh=%02X\n",sat,svh); + return 1; + } + return 0; +} +/* test SNR mask --------------------------------------------------------------- +* test SNR mask +* args : int base I rover or base-station (0:rover,1:base station) +* int freq I frequency (0:L1,1:L2,2:L3,...) +* double el I elevation angle (rad) +* double snr I C/N0 (dBHz) +* snrmask_t *mask I SNR mask +* return : status (1:masked,0:unmasked) +*-----------------------------------------------------------------------------*/ +int testsnr(int base, int freq, double el, double snr, + const snrmask_t *mask) +{ + double minsnr,a; + int i; + + if (!mask->ena[base]||freq<0||freq>=NFREQ) return 0; + + a=(el*R2D+5.0)/10.0; + i=(int)floor(a); a-=i; + if (i<1) minsnr=mask->mask[freq][0]; + else if (i>8) minsnr=mask->mask[freq][8]; + else minsnr=(1.0-a)*mask->mask[freq][i-1]+a*mask->mask[freq][i]; + + return snr>(7-i%8))&1u); + return bits; +} +int getbits(const unsigned char *buff, int pos, int len) +{ + unsigned int bits=getbitu(buff,pos,len); + if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits; + return (int)(bits|(~0u<>=1) { + if (data&mask) buff[i/8]|=1u<<(7-i%8); else buff[i/8]&=~(1u<<(7-i%8)); + } +} +void setbits(unsigned char *buff, int pos, int len, int data) +{ + if (data<0) data|=1<<(len-1); else data&=~(1<<(len-1)); /* set sign bit */ + setbitu(buff,pos,len,(unsigned int)data); +} +/* crc-32 parity --------------------------------------------------------------- +* compute crc-32 parity for novatel raw +* args : unsigned char *buff I data +* int len I data length (bytes) +* return : crc-32 parity +* notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC +*-----------------------------------------------------------------------------*/ +unsigned int rtk_crc32(const unsigned char *buff, int len) +{ + unsigned int crc=0; + int i,j; + + trace(4,"rtk_crc32: len=%d\n",len); + + for (i=0;i>1)^POLYCRC32; else crc>>=1; + } + } + return crc; +} +/* crc-24q parity -------------------------------------------------------------- +* compute crc-24q parity for sbas, rtcm3 +* args : unsigned char *buff I data +* int len I data length (bytes) +* return : crc-24Q parity +* notes : see reference [2] A.4.3.3 Parity +*-----------------------------------------------------------------------------*/ +unsigned int rtk_crc24q(const unsigned char *buff, int len) +{ + unsigned int crc=0; + int i; + + trace(4,"rtk_crc24q: len=%d\n",len); + + for (i=0;i>16)^buff[i]]; + return crc; +} +/* crc-16 parity --------------------------------------------------------------- +* compute crc-16 parity for binex, nvs +* args : unsigned char *buff I data +* int len I data length (bytes) +* return : crc-16 parity +* notes : see reference [10] A.3. +*-----------------------------------------------------------------------------*/ +unsigned short rtk_crc16(const unsigned char *buff, int len) +{ + unsigned short crc=0; + int i; + + trace(4,"rtk_crc16: len=%d\n",len); + + for (i=0;i>8)^buff[i])&0xFF]; + } + return crc; +} +/* decode navigation data word ------------------------------------------------- +* check party and decode navigation data word +* args : unsigned int word I navigation data word (2+30bit) +* (previous word D29*-30* + current word D1-30) +* unsigned char *data O decoded navigation data without parity +* (8bitx3) +* return : status (1:ok,0:parity error) +* notes : see reference [1] 20.3.5.2 user parity algorithm +*-----------------------------------------------------------------------------*/ +int decode_word(unsigned int word, unsigned char *data) +{ + const unsigned int hamming[]={ + 0xBB1F3480,0x5D8F9A40,0xAEC7CD00,0x5763E680,0x6BB1F340,0x8B7A89C0 + }; + unsigned int parity=0,w; + int i; + + trace(5,"decodeword: word=%08x\n",word); + + if (word&0x40000000) word^=0x3FFFFFC0; + + for (i=0;i<6;i++) { + parity<<=1; + for (w=(word&hamming[i])>>6;w;w>>=1) parity^=w&1; + } + if (parity!=(word&0x3F)) return 0; + + for (i=0;i<3;i++) data[i]=(unsigned char)(word>>(22-i*8)); + return 1; +} +/* new matrix ------------------------------------------------------------------ +* allocate memory of matrix +* args : int n,m I number of rows and columns of matrix +* return : matrix pointer (if n<=0 or m<=0, return NULL) +*-----------------------------------------------------------------------------*/ +double *mat(int n, int m) +{ + double *p; + + if (n<=0||m<=0) return NULL; + if (!(p=(double *)malloc(sizeof(double)*n*m))) { + fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); + } + return p; +} +/* new integer matrix ---------------------------------------------------------- +* allocate memory of integer matrix +* args : int n,m I number of rows and columns of matrix +* return : matrix pointer (if n<=0 or m<=0, return NULL) +*-----------------------------------------------------------------------------*/ +int *imat(int n, int m) +{ + int *p; + + if (n<=0||m<=0) return NULL; + if (!(p=(int *)malloc(sizeof(int)*n*m))) { + fatalerr("integer matrix memory allocation error: n=%d,m=%d\n",n,m); + } + return p; +} +/* zero matrix ----------------------------------------------------------------- +* generate new zero matrix +* args : int n,m I number of rows and columns of matrix +* return : matrix pointer (if n<=0 or m<=0, return NULL) +*-----------------------------------------------------------------------------*/ +double *zeros(int n, int m) +{ + double *p; + +#if NOCALLOC + if ((p=mat(n,m))) for (n=n*m-1;n>=0;n--) p[n]=0.0; +#else + if (n<=0||m<=0) return NULL; + if (!(p=(double *)calloc(sizeof(double),n*m))) { + fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); + } +#endif + return p; +} +/* identity matrix ------------------------------------------------------------- +* generate new identity matrix +* args : int n I number of rows and columns of matrix +* return : matrix pointer (if n<=0, return NULL) +*-----------------------------------------------------------------------------*/ +double *eye(int n) +{ + double *p; + int i; + + if ((p=zeros(n,n))) for (i=0;i=0) c+=a[n]*b[n]; + return c; +} +/* euclid norm ----------------------------------------------------------------- +* euclid norm of vector +* args : double *a I vector a (n x 1) +* int n I size of vector a +* return : || a || +*-----------------------------------------------------------------------------*/ +double norm(const double *a, int n) +{ + return sqrt(dot(a,a,n)); +} +/* outer product of 3d vectors ------------------------------------------------- +* outer product of 3d vectors +* args : double *a,*b I vector a,b (3 x 1) +* double *c O outer product (a x b) (3 x 1) +* return : none +*-----------------------------------------------------------------------------*/ +void cross3(const double *a, const double *b, double *c) +{ + c[0]=a[1]*b[2]-a[2]*b[1]; + c[1]=a[2]*b[0]-a[0]*b[2]; + c[2]=a[0]*b[1]-a[1]*b[0]; +} +/* normalize 3d vector --------------------------------------------------------- +* normalize 3d vector +* args : double *a I vector a (3 x 1) +* double *b O normlized vector (3 x 1) || b || = 1 +* return : status (1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int normv3(const double *a, double *b) +{ + double r; + if ((r=norm(a,3))<=0.0) return 0; + b[0]=a[0]/r; + b[1]=a[1]/r; + b[2]=a[2]/r; + return 1; +} +/* copy matrix ----------------------------------------------------------------- +* copy matrix +* args : double *A O destination matrix A (n x m) +* double *B I source matrix B (n x m) +* int n,m I number of rows and columns of matrix +* return : none +*-----------------------------------------------------------------------------*/ +void matcpy(double *A, const double *B, int n, int m) +{ + memcpy(A,B,sizeof(double)*n*m); +} +/* matrix routines -----------------------------------------------------------*/ + + +/* multiply matrix (wrapper of blas dgemm) ------------------------------------- +* multiply matrix by matrix (C=alpha*A*B+beta*C) +* args : char *tr I transpose flags ("N":normal,"T":transpose) +* int n,k,m I size of (transposed) matrix A,B +* double alpha I alpha +* double *A,*B I (transposed) matrix A (n x m), B (m x k) +* double beta I beta +* double *C IO matrix C (n x k) +* return : none +*-----------------------------------------------------------------------------*/ +void matmul(const char *tr, int n, int k, int m, double alpha, + const double *A, const double *B, double beta, double *C) +{ + int lda=tr[0]=='T'?m:n,ldb=tr[1]=='T'?k:m; + + dgemm_((char *)tr,(char *)tr+1,&n,&k,&m,&alpha,(double *)A,&lda,(double *)B, + &ldb,&beta,C,&n); +} +/* inverse of matrix ----------------------------------------------------------- +* inverse of matrix (A=A^-1) +* args : double *A IO matrix (n x n) +* int n I size of matrix A +* return : status (0:ok,0>:error) +*-----------------------------------------------------------------------------*/ +int matinv(double *A, int n) +{ + double *work; + int info,lwork=n*16,*ipiv=imat(n,1); + + work=mat(lwork,1); + dgetrf_(&n,&n,A,&n,ipiv,&info); + if (!info) dgetri_(&n,A,&n,ipiv,work,&lwork,&info); + free(ipiv); free(work); + return info; +} +/* solve linear equation ------------------------------------------------------- +* solve linear equation (X=A\Y or X=A'\Y) +* args : char *tr I transpose flag ("N":normal,"T":transpose) +* double *A I input matrix A (n x n) +* double *Y I input matrix Y (n x m) +* int n,m I size of matrix A,Y +* double *X O X=A\Y or X=A'\Y (n x m) +* return : status (0:ok,0>:error) +* notes : matirix stored by column-major order (fortran convention) +* X can be same as Y +*-----------------------------------------------------------------------------*/ +int solve(const char *tr, const double *A, const double *Y, int n, + int m, double *X) +{ + double *B=mat(n,n); + int info,*ipiv=imat(n,1); + + matcpy(B,A,n,n); + matcpy(X,Y,n,m); + dgetrf_(&n,&n,B,&n,ipiv,&info); + if (!info) dgetrs_((char *)tr,&n,&m,B,&n,ipiv,X,&n,&info); + free(ipiv); free(B); + return info; +} + + +/* end of matrix routines ----------------------------------------------------*/ + +/* least square estimation ----------------------------------------------------- +* least square estimation by solving normal equation (x=(A*A')^-1*A*y) +* args : double *A I transpose of (weighted) design matrix (n x m) +* double *y I (weighted) measurements (m x 1) +* int n,m I number of parameters and measurements (n<=m) +* double *x O estmated parameters (n x 1) +* double *Q O esimated parameters covariance matrix (n x n) +* return : status (0:ok,0>:error) +* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2)) +* matirix stored by column-major order (fortran convention) +*-----------------------------------------------------------------------------*/ +int lsq(const double *A, const double *y, int n, int m, double *x, + double *Q) +{ + double *Ay; + int info; + + if (m0.0) ix[k++]=i; + x_=mat(k,1); xp_=mat(k,1); P_=mat(k,k); Pp_=mat(k,k); H_=mat(k,m); + for (i=0;i:error) +* notes : see reference [4] 5.2 +* matirix stored by column-major order (fortran convention) +*-----------------------------------------------------------------------------*/ +int smoother(const double *xf, const double *Qf, const double *xb, + const double *Qb, int n, double *xs, double *Qs) +{ + double *invQf=mat(n,n),*invQb=mat(n,n),*xx=mat(n,1); + int i,info=-1; + + matcpy(invQf,Qf,n,n); + matcpy(invQb,Qb,n,n); + if (!matinv(invQf,n)&&!matinv(invQb,n)) { + for (i=0;i=0;s++) *p++=*s=='d'||*s=='D'?'E':*s; *p='\0'; + return sscanf(str,"%lf",&value)==1?value:0.0; +} +/* string to time -------------------------------------------------------------- +* convert substring in string to gtime_t struct +* args : char *s I string ("... yyyy mm dd hh mm ss ...") +* int i,n I substring position and width +* gtime_t *t O gtime_t struct +* return : status (0:ok,0>:error) +*-----------------------------------------------------------------------------*/ +int str2time(const char *s, int i, int n, gtime_t *t) +{ + double ep[6]; + char str[256],*p=str; + + if (i<0||(int)strlen(s)=0;) *p++=*s++; *p='\0'; + if (sscanf(str,"%lf %lf %lf %lf %lf %lf",ep,ep+1,ep+2,ep+3,ep+4,ep+5)<6) + return -1; + if (ep[0]<100.0) ep[0]+=ep[0]<80.0?2000.0:1900.0; + *t=epoch2time(ep); + return 0; +} +/* convert calendar day/time to time ------------------------------------------- +* convert calendar day/time to gtime_t struct +* args : double *ep I day/time {year,month,day,hour,min,sec} +* return : gtime_t struct +* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) +*-----------------------------------------------------------------------------*/ +gtime_t epoch2time(const double *ep) +{ + const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335}; + gtime_t time={}; + int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2]; + + if (year<1970||2099=3?1:0); + sec=(int)floor(ep[5]); + time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec; + time.sec=ep[5]-sec; + return time; +} +/* time to calendar day/time --------------------------------------------------- +* convert gtime_t struct to calendar day/time +* args : gtime_t t I gtime_t struct +* double *ep O day/time {year,month,day,hour,min,sec} +* return : none +* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) +*-----------------------------------------------------------------------------*/ +void time2epoch(gtime_t t, double *ep) +{ + const int mday[]={ /* # of days in a month */ + 31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31, + 31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31 + }; + int days,sec,mon,day; + + /* leap year if year%4==0 in 1901-2099 */ + days=(int)(t.time/86400); + sec=(int)(t.time-(time_t)days*86400); + for (day=days%1461,mon=0;mon<48;mon++) { + if (day>=mday[mon]) day-=mday[mon]; else break; + } + ep[0]=1970+days/1461*4+mon/12; ep[1]=mon%12+1; ep[2]=day+1; + ep[3]=sec/3600; ep[4]=sec%3600/60; ep[5]=sec%60+t.sec; +} +/* gps time to time ------------------------------------------------------------ +* convert week and tow in gps time to gtime_t struct +* args : int week I week number in gps time +* double sec I time of week in gps time (s) +* return : gtime_t struct +*-----------------------------------------------------------------------------*/ +gtime_t gpst2time(int week, double sec) +{ + gtime_t t=epoch2time(gpst0); + + if (sec<-1E9||1E9tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday; + ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1E-6; + } + time=epoch2time(ep); + +#ifdef CPUTIME_IN_GPST /* cputime operated in gpst */ + time=gpst2utc(time); +#endif + return time; +} +/* set current time in utc ----------------------------------------------------- +* set current time in utc +* args : gtime_t I current time in utc +* return : none +* notes : just set time offset between cpu time and current time +* the time offset is reflected to only timeget() +* not reentrant +*----------------------------------------------------------------------------- +void timeset(gtime_t t) +{ + timeoffset_+=timediff(t,timeget()); +} +*/ +/* read leap seconds table by text -------------------------------------------*/ +int read_leaps_text(FILE *fp) +{ + char buff[256],*p; + int i,n=0,ep[6],ls; + + rewind(fp); + + while (fgets(buff,sizeof(buff),fp)&&n=13) continue; + ls[n][0]=y; + ls[n][1]=m; + ls[n][2]=d; + ls[n++][6]=(char)(19.0-tai_utc); + } + for (i=0;i0;i++) { + tu=timeadd(t,leaps[i][6]); + if (timediff(tu,epoch2time(leaps[i]))>=0.0) return tu; + } + return t; +} +/* utc to gpstime -------------------------------------------------------------- +* convert utc to gpstime considering leap seconds +* args : gtime_t t I time expressed in utc +* return : time expressed in gpstime +* notes : ignore slight time offset under 100 ns +*-----------------------------------------------------------------------------*/ +gtime_t utc2gpst(gtime_t t) +{ + int i; + + for (i=0;leaps[i][0]>0;i++) { + if (timediff(t,epoch2time(leaps[i]))>=0.0) return timeadd(t,-leaps[i][6]); + } + return t; +} +/* gpstime to bdt -------------------------------------------------------------- +* convert gpstime to bdt (beidou navigation satellite system time) +* args : gtime_t t I time expressed in gpstime +* return : time expressed in bdt +* notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC +* no leap seconds in BDT +* ignore slight time offset under 100 ns +*-----------------------------------------------------------------------------*/ +gtime_t gpst2bdt(gtime_t t) +{ + return timeadd(t,-14.0); +} +/* bdt to gpstime -------------------------------------------------------------- +* convert bdt (beidou navigation satellite system time) to gpstime +* args : gtime_t t I time expressed in bdt +* return : time expressed in gpstime +* notes : see gpst2bdt() +*-----------------------------------------------------------------------------*/ +gtime_t bdt2gpst(gtime_t t) +{ + return timeadd(t,14.0); +} +/* time to day and sec -------------------------------------------------------*/ +double time2sec(gtime_t time, gtime_t *day) +{ + double ep[6],sec; + time2epoch(time,ep); + sec=ep[3]*3600.0+ep[4]*60.0+ep[5]; + ep[3]=ep[4]=ep[5]=0.0; + *day=epoch2time(ep); + return sec; +} +/* utc to gmst ----------------------------------------------------------------- +* convert utc to gmst (Greenwich mean sidereal time) +* args : gtime_t t I time expressed in utc +* double ut1_utc I UT1-UTC (s) +* return : gmst (rad) +*-----------------------------------------------------------------------------*/ +double utc2gmst(gtime_t t, double ut1_utc) +{ + const double ep2000[]={2000,1,1,12,0,0}; + gtime_t tut,tut0; + double ut,t1,t2,t3,gmst0,gmst; + + tut=timeadd(t,ut1_utc); + ut=time2sec(tut,&tut0); + t1=timediff(tut0,epoch2time(ep2000))/86400.0/36525.0; + t2=t1*t1; t3=t2*t1; + gmst0=24110.54841+8640184.812866*t1+0.093104*t2-6.2E-6*t3; + gmst=gmst0+1.002737909350795*ut; + + return fmod(gmst,86400.0)*PI/43200.0; /* 0 <= gmst <= 2*PI */ +} +/* time to string -------------------------------------------------------------- +* convert gtime_t struct to string +* args : gtime_t t I gtime_t struct +* char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") +* int n I number of decimals +* return : none +*-----------------------------------------------------------------------------*/ +void time2str(gtime_t t, char *s, int n) +{ + double ep[6]; + + if (n<0) n=0; else if (n>12) n=12; + if (1.0-t.sec<0.5/pow(10.0,n)) {t.time++; t.sec=0.0;}; + time2epoch(t,ep); + sprintf(s,"%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f",ep[0],ep[1],ep[2], + ep[3],ep[4],n<=0?2:n+3,n<=0?0:n,ep[5]); +} +/* get time string ------------------------------------------------------------- +* get time string +* args : gtime_t t I gtime_t struct +* int n I number of decimals +* return : time string +* notes : not reentrant, do not use multiple in a function +*-----------------------------------------------------------------------------*/ +char *time_str(gtime_t t, int n) +{ + static char buff[64]; + time2str(t,buff,n); + return buff; +} +/* time to day of year --------------------------------------------------------- +* convert time to day of year +* args : gtime_t t I gtime_t struct +* return : day of year (days) +*-----------------------------------------------------------------------------*/ +double time2doy(gtime_t t) +{ + double ep[6]; + + time2epoch(t,ep); + ep[1]=ep[2]=1.0; ep[3]=ep[4]=ep[5]=0.0; + return timediff(t,epoch2time(ep))/86400.0+1.0; +} +/* adjust gps week number ------------------------------------------------------ +* adjust gps week number using cpu time +* args : int week I not-adjusted gps week number +* return : adjusted gps week number +*-----------------------------------------------------------------------------*/ +int adjgpsweek(int week) +{ + int w; + (void)time2gpst(utc2gpst(timeget()),&w); + if (w<1560) w=1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */ + return week+(w-week+512)/1024*1024; +} +/* get tick time --------------------------------------------------------------- +* get current tick in ms +* args : none +* return : current tick in ms +*-----------------------------------------------------------------------------*/ +unsigned int tickget(void) +{ + + struct timespec tp={}; + struct timeval tv={}; + +#ifdef CLOCK_MONOTONIC_RAW + /* linux kernel > 2.6.28 */ + if (!clock_gettime(CLOCK_MONOTONIC_RAW,&tp)) { + return tp.tv_sec*1000u+tp.tv_nsec/1000000u; + } + else { + gettimeofday(&tv,NULL); + return tv.tv_sec*1000u+tv.tv_usec/1000u; + } +#else + gettimeofday(&tv,NULL); + return tv.tv_sec*1000u+tv.tv_usec/1000u; +#endif +} +/* sleep ms -------------------------------------------------------------------- +* sleep ms +* args : int ms I miliseconds to sleep (<0:no sleep) +* return : none +*-----------------------------------------------------------------------------*/ +void sleepms(int ms) +{ + + struct timespec ts; + if (ms<=0) return; + ts.tv_sec=(time_t)(ms/1000); + ts.tv_nsec=(long)(ms%1000*1000000); + nanosleep(&ts,NULL); +} +/* convert degree to deg-min-sec ----------------------------------------------- +* convert degree to degree-minute-second +* args : double deg I degree +* double *dms O degree-minute-second {deg,min,sec} +* int ndec I number of decimals of second +* return : none +*-----------------------------------------------------------------------------*/ +void deg2dms(double deg, double *dms, int ndec) +{ + double sign=deg<0.0?-1.0:1.0,a=fabs(deg); + double unit=pow(0.1,ndec); + dms[0]=floor(a); a=(a-dms[0])*60.0; + dms[1]=floor(a); a=(a-dms[1])*60.0; + dms[2]=floor(a/unit+0.5)*unit; + if (dms[2]>=60.0) { + dms[2]=0.0; + dms[1]+=1.0; + if (dms[1]>=60.0) { + dms[1]=0.0; + dms[0]+=1.0; + } + } + dms[0]*=sign; +} +/* convert deg-min-sec to degree ----------------------------------------------- +* convert degree-minute-second to degree +* args : double *dms I degree-minute-second {deg,min,sec} +* return : degree +*-----------------------------------------------------------------------------*/ +double dms2deg(const double *dms) +{ + double sign=dms[0]<0.0?-1.0:1.0; + return sign*(fabs(dms[0])+dms[1]/60.0+dms[2]/3600.0); +} +/* transform ecef to geodetic postion ------------------------------------------ +* transform ecef position to geodetic position +* args : double *r I ecef position {x,y,z} (m) +* double *pos O geodetic position {lat,lon,h} (rad,m) +* return : none +* notes : WGS84, ellipsoidal height +*-----------------------------------------------------------------------------*/ +void ecef2pos(const double *r, double *pos) +{ + double e2=FE_WGS84*(2.0-FE_WGS84),r2=dot(r,r,2),z,zk,v=RE_WGS84,sinp; + + for (z=r[2],zk=0.0;fabs(z-zk)>=1E-4;) { + zk=z; + sinp=z/sqrt(r2+z*z); + v=RE_WGS84/sqrt(1.0-e2*sinp*sinp); + z=r[2]+v*e2*sinp; + } + pos[0]=r2>1E-12?atan(z/sqrt(r2)):(r[2]>0.0?PI/2.0:-PI/2.0); + pos[1]=r2>1E-12?atan2(r[1],r[0]):0.0; + pos[2]=sqrt(r2+z*z)-v; +} +/* transform geodetic to ecef position ----------------------------------------- +* transform geodetic position to ecef position +* args : double *pos I geodetic position {lat,lon,h} (rad,m) +* double *r O ecef position {x,y,z} (m) +* return : none +* notes : WGS84, ellipsoidal height +*-----------------------------------------------------------------------------*/ +void pos2ecef(const double *pos, double *r) +{ + double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]); + double e2=FE_WGS84*(2.0-FE_WGS84),v=RE_WGS84/sqrt(1.0-e2*sinp*sinp); + + r[0]=(v+pos[2])*cosp*cosl; + r[1]=(v+pos[2])*cosp*sinl; + r[2]=(v*(1.0-e2)+pos[2])*sinp; +} +/* ecef to local coordinate transfromation matrix ------------------------------ +* compute ecef to local coordinate transfromation matrix +* args : double *pos I geodetic position {lat,lon} (rad) +* double *E O ecef to local coord transformation matrix (3x3) +* return : none +* notes : matirix stored by column-major order (fortran convention) +*-----------------------------------------------------------------------------*/ +void xyz2enu(const double *pos, double *E) +{ + double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]); + + E[0]=-sinl; E[3]=cosl; E[6]=0.0; + E[1]=-sinp*cosl; E[4]=-sinp*sinl; E[7]=cosp; + E[2]=cosp*cosl; E[5]=cosp*sinl; E[8]=sinp; +} +/* transform ecef vector to local tangental coordinate ------------------------- +* transform ecef vector to local tangental coordinate +* args : double *pos I geodetic position {lat,lon} (rad) +* double *r I vector in ecef coordinate {x,y,z} +* double *e O vector in local tangental coordinate {e,n,u} +* return : none +*-----------------------------------------------------------------------------*/ +void ecef2enu(const double *pos, const double *r, double *e) +{ + double E[9]; + + xyz2enu(pos,E); + matmul("NN",3,1,3,1.0,E,r,0.0,e); +} +/* transform local vector to ecef coordinate ----------------------------------- +* transform local tangental coordinate vector to ecef +* args : double *pos I geodetic position {lat,lon} (rad) +* double *e I vector in local tangental coordinate {e,n,u} +* double *r O vector in ecef coordinate {x,y,z} +* return : none +*-----------------------------------------------------------------------------*/ +void enu2ecef(const double *pos, const double *e, double *r) +{ + double E[9]; + + xyz2enu(pos,E); + matmul("TN",3,1,3,1.0,E,e,0.0,r); +} +/* transform covariance to local tangental coordinate -------------------------- +* transform ecef covariance to local tangental coordinate +* args : double *pos I geodetic position {lat,lon} (rad) +* double *P I covariance in ecef coordinate +* double *Q O covariance in local tangental coordinate +* return : none +*-----------------------------------------------------------------------------*/ +void covenu(const double *pos, const double *P, double *Q) +{ + double E[9],EP[9]; + + xyz2enu(pos,E); + matmul("NN",3,3,3,1.0,E,P,0.0,EP); + matmul("NT",3,3,3,1.0,EP,E,0.0,Q); +} +/* transform local enu coordinate covariance to xyz-ecef ----------------------- +* transform local enu covariance to xyz-ecef coordinate +* args : double *pos I geodetic position {lat,lon} (rad) +* double *Q I covariance in local enu coordinate +* double *P O covariance in xyz-ecef coordinate +* return : none +*-----------------------------------------------------------------------------*/ +void covecef(const double *pos, const double *Q, double *P) +{ + double E[9],EQ[9]; + + xyz2enu(pos,E); + matmul("TN",3,3,3,1.0,E,Q,0.0,EQ); + matmul("NN",3,3,3,1.0,EQ,E,0.0,P); +} + + +/* astronomical arguments: f={l,l',F,D,OMG} (rad) ----------------------------*/ +void ast_args(double t, double *f) +{ + static const double fc[][5]={ /* coefficients for iau 1980 nutation */ + { 134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470}, + { 357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149}, + { 93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417}, + { 297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169}, + { 125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939} + }; + double tt[4]; + int i,j; + + for (tt[0]=t,i=1;i<4;i++) tt[i]=tt[i-1]*t; + for (i=0;i<5;i++) { + f[i]=fc[i][0]*3600.0; + for (j=0;j<4;j++) f[i]+=fc[i][j+1]*tt[j]; + f[i]=fmod(f[i]*AS2R,2.0*PI); + } +} +/* iau 1980 nutation ---------------------------------------------------------*/ +void nut_iau1980(double t, const double *f, double *dpsi, double *deps) +{ + static const double nut[106][10]={ + { 0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9}, + { 0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1}, + { 0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5}, + { 0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5}, + { 0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1}, + { 1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0}, + { 0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6}, + { 0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0}, + { 1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1}, + { 0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3}, + { -1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0}, + { -1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0}, + { 1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0}, + { 0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0}, + { -1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0}, + { -1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0}, + { 1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0}, + { -2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0}, + { 0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0}, + { 2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0}, + { 2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0}, + { 1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0}, + { 0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0}, + { -1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0}, + { 0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0}, + { 0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0}, + { -1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0}, + { 0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0}, + { 1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0}, + { 0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0}, + { 2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0}, + { -1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0}, + { 1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0}, + { 0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0}, + { 0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0}, + { 1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0}, + { -2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0}, + { 0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0}, + { 2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0}, + { 1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0}, + { 0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0}, + { 0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0}, + { 2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0}, + { 1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0}, + { 1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0}, + { 0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0}, + { 0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0}, + { 1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0}, + { 2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0}, + { 0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0}, + { 1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0}, + { 1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0}, + { -1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0}, + { 0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0}, + { 1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0}, + { 3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0}, + { 1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0}, + { -1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0}, + { 1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0}, + { -1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0}, + { 0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0}, + { -2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0}, + { 2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0}, + { 3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0}, + { 1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0}, + { 0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0}, + { 1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0}, + { 1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0}, + { 1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0}, + { 0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0}, + { 0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0}, + { 0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0}, + { 1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0}, + { 2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0}, + { 0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0}, + { 0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0}, + { -2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0}, + { 2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0}, + { 0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0}, + { 0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0}, + { 0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0}, + { 1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0}, + { 3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0}, + { -2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0}, + { -1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0}, + { 0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0}, + { -1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0}, + { 2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0}, + { 2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0}, + { 2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0}, + { 2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0}, + { 1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0}, + { -1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0}, + { -1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0} + }; + double ang; + int i,j; + + *dpsi=*deps=0.0; + + for (i=0;i<106;i++) { + ang=0.0; + for (j=0;j<5;j++) ang+=nut[i][j]*f[j]; + *dpsi+=(nut[i][6]+nut[i][7]*t)*sin(ang); + *deps+=(nut[i][8]+nut[i][9]*t)*cos(ang); + } + *dpsi*=1E-4*AS2R; /* 0.1 mas -> rad */ + *deps*=1E-4*AS2R; +} +/* eci to ecef transformation matrix ------------------------------------------- +* compute eci to ecef transformation matrix +* args : gtime_t tutc I time in utc +* double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) +* double *U O eci to ecef transformation matrix (3 x 3) +* double *gmst IO greenwich mean sidereal time (rad) +* (NULL: no output) +* return : none +* note : see ref [3] chap 5 +* not thread-safe +*-----------------------------------------------------------------------------*/ +void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst) +{ + const double ep2000[]={2000,1,1,12,0,0}; + static gtime_t tutc_; + static double U_[9],gmst_; + gtime_t tgps; + double eps,ze,th,z,t,t2,t3,dpsi,deps,gast,f[5]; + double R1[9],R2[9],R3[9],R[9],W[9],N[9],P[9],NP[9]; + int i; + + trace(4,"eci2ecef: tutc=%s\n",time_str(tutc,3)); + + if (fabs(timediff(tutc,tutc_))<0.01) { /* read cache */ + for (i=0;i<9;i++) U[i]=U_[i]; + if (gmst) *gmst=gmst_; + return; + } + tutc_=tutc; + + /* terrestrial time */ + tgps=utc2gpst(tutc_); + t=(timediff(tgps,epoch2time(ep2000))+19.0+32.184)/86400.0/36525.0; + t2=t*t; t3=t2*t; + + /* astronomical arguments */ + ast_args(t,f); + + /* iau 1976 precession */ + ze=(2306.2181*t+0.30188*t2+0.017998*t3)*AS2R; + th=(2004.3109*t-0.42665*t2-0.041833*t3)*AS2R; + z =(2306.2181*t+1.09468*t2+0.018203*t3)*AS2R; + eps=(84381.448-46.8150*t-0.00059*t2+0.001813*t3)*AS2R; + Rz(-z,R1); Ry(th,R2); Rz(-ze,R3); + matmul("NN",3,3,3,1.0,R1,R2,0.0,R); + matmul("NN",3,3,3,1.0,R, R3,0.0,P); /* P=Rz(-z)*Ry(th)*Rz(-ze) */ + + /* iau 1980 nutation */ + nut_iau1980(t,f,&dpsi,&deps); + Rx(-eps-deps,R1); Rz(-dpsi,R2); Rx(eps,R3); + matmul("NN",3,3,3,1.0,R1,R2,0.0,R); + matmul("NN",3,3,3,1.0,R ,R3,0.0,N); /* N=Rx(-eps)*Rz(-dspi)*Rx(eps) */ + + /* greenwich aparent sidereal time (rad) */ + gmst_=utc2gmst(tutc_,erpv[2]); + gast=gmst_+dpsi*cos(eps); + gast+=(0.00264*sin(f[4])+0.000063*sin(2.0*f[4]))*AS2R; + + /* eci to ecef transformation matrix */ + Ry(-erpv[0],R1); Rx(-erpv[1],R2); Rz(gast,R3); + matmul("NN",3,3,3,1.0,R1,R2,0.0,W ); + matmul("NN",3,3,3,1.0,W ,R3,0.0,R ); /* W=Ry(-xp)*Rx(-yp) */ + matmul("NN",3,3,3,1.0,N ,P ,0.0,NP); + matmul("NN",3,3,3,1.0,R ,NP,0.0,U_); /* U=W*Rz(gast)*N*P */ + + for (i=0;i<9;i++) U[i]=U_[i]; + if (gmst) *gmst=gmst_; + + trace(5,"gmst=%.12f gast=%.12f\n",gmst_,gast); + trace(5,"P=\n"); tracemat(5,P,3,3,15,12); + trace(5,"N=\n"); tracemat(5,N,3,3,15,12); + trace(5,"W=\n"); tracemat(5,W,3,3,15,12); + trace(5,"U=\n"); tracemat(5,U,3,3,15,12); +} +/* decode antenna parameter field --------------------------------------------*/ +int decodef(char *p, int n, double *v) +{ + int i; + + for (i=0;inmax<=pcvs->n) { + pcvs->nmax+=256; + if (!(pcvs_pcv=(pcv_t *)realloc(pcvs->pcv,sizeof(pcv_t)*pcvs->nmax))) { + trace(1,"addpcv: memory allocation error\n"); + free(pcvs->pcv); pcvs->pcv=NULL; pcvs->n=pcvs->nmax=0; + return; + } + pcvs->pcv=pcvs_pcv; + } + pcvs->pcv[pcvs->n++]=*pcv; +} +/* read ngs antenna parameter file -------------------------------------------*/ +int readngspcv(const char *file, pcvs_t *pcvs) +{ + FILE *fp; + static const pcv_t pcv0={}; + pcv_t pcv; + double neu[3]; + int n=0; + char buff[256]; + + if (!(fp=fopen(file,"r"))) { + trace(2,"ngs pcv file open error: %s\n",file); + return 0; + } + while (fgets(buff,sizeof(buff),fp)) { + + if (strlen(buff)>=62&&buff[61]=='|') continue; + + if (buff[0]!=' ') n=0; /* start line */ + if (++n==1) { + pcv=pcv0; + strncpy(pcv.type,buff,61); pcv.type[61]='\0'; + } + else if (n==2) { + if (decodef(buff,3,neu)<3) continue; + pcv.off[0][0]=neu[1]; + pcv.off[0][1]=neu[0]; + pcv.off[0][2]=neu[2]; + } + else if (n==3) decodef(buff,10,pcv.var[0]); + else if (n==4) decodef(buff,9,pcv.var[0]+10); + else if (n==5) { + if (decodef(buff,3,neu)<3) continue;; + pcv.off[1][0]=neu[1]; + pcv.off[1][1]=neu[0]; + pcv.off[1][2]=neu[2]; + } + else if (n==6) decodef(buff,10,pcv.var[1]); + else if (n==7) { + decodef(buff,9,pcv.var[1]+10); + addpcv(&pcv,pcvs); + } + } + fclose(fp); + + return 1; +} +/* read antex file ----------------------------------------------------------*/ +int readantex(const char *file, pcvs_t *pcvs) +{ + FILE *fp; + static const pcv_t pcv0={}; + pcv_t pcv; + double neu[3]; + int i,f,freq=0,state=0,freqs[]={1,2,5,6,7,8,0}; + char buff[256]; + + trace(3,"readantex: file=%s\n",file); + + if (!(fp=fopen(file,"r"))) { + trace(2,"antex pcv file open error: %s\n",file); + return 0; + } + while (fgets(buff,sizeof(buff),fp)) { + + if (strlen(buff)<60||strstr(buff+60,"COMMENT")) continue; + + if (strstr(buff+60,"START OF ANTENNA")) { + pcv=pcv0; + state=1; + } + if (strstr(buff+60,"END OF ANTENNA")) { + addpcv(&pcv,pcvs); + state=0; + } + if (!state) continue; + + if (strstr(buff+60,"TYPE / SERIAL NO")) { + strncpy(pcv.type,buff ,20); pcv.type[20]='\0'; + strncpy(pcv.code,buff+20,20); pcv.code[20]='\0'; + if (!strncmp(pcv.code+3," ",8)) { + pcv.sat=satid2no(pcv.code); + } + } + else if (strstr(buff+60,"VALID FROM")) { + if (!str2time(buff,0,43,&pcv.ts)) continue; + } + else if (strstr(buff+60,"VALID UNTIL")) { + if (!str2time(buff,0,43,&pcv.te)) continue; + } + else if (strstr(buff+60,"START OF FREQUENCY")) { + if (sscanf(buff+4,"%d",&f)<1) continue; + for (i=0;in;i++) { + pcv=pcvs->pcv+i; + trace(4,"sat=%2d type=%20s code=%s off=%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", + pcv->sat,pcv->type,pcv->code,pcv->off[0][0],pcv->off[0][1], + pcv->off[0][2],pcv->off[1][0],pcv->off[1][1],pcv->off[1][2]); + } + return stat; +} +/* search antenna parameter ---------------------------------------------------- +* read satellite antenna phase center position +* args : int sat I satellite number (0: receiver antenna) +* char *type I antenna type for receiver antenna +* gtime_t time I time to search parameters +* pcvs_t *pcvs IO antenna parameters +* return : antenna parameter (NULL: no antenna) +*-----------------------------------------------------------------------------*/ +pcv_t *searchpcv(int sat, const char *type, gtime_t time, + const pcvs_t *pcvs) +{ + pcv_t *pcv; + char buff[MAXANT],*types[2],*p; + int i,j,n=0; + + trace(3,"searchpcv: sat=%2d type=%s\n",sat,type); + + if (sat) { /* search satellite antenna */ + for (i=0;in;i++) { + pcv=pcvs->pcv+i; + if (pcv->sat!=sat) continue; + if (pcv->ts.time!=0&&timediff(pcv->ts,time)>0.0) continue; + if (pcv->te.time!=0&&timediff(pcv->te,time)<0.0) continue; + return pcv; + } + } + else { + strcpy(buff,type); + for (p=strtok(buff," ");p&&n<2;p=strtok(NULL," ")) types[n++]=p; + if (n<=0) return NULL; + + /* search receiver antenna with radome at first */ + for (i=0;in;i++) { + pcv=pcvs->pcv+i; + for (j=0;jtype,types[j])) break; + if (j>=n) return pcv; + } + /* search receiver antenna without radome */ + for (i=0;in;i++) { + pcv=pcvs->pcv+i; + if (strstr(pcv->type,types[0])!=pcv->type) continue; + + trace(2,"pcv without radome is used type=%s\n",type); + return pcv; + } + } + return NULL; +} +/* read station positions ------------------------------------------------------ +* read positions from station position file +* args : char *file I station position file containing +* lat(deg) lon(deg) height(m) name in a line +* char *rcvs I station name +* double *pos O station position {lat,lon,h} (rad/m) +* (all 0 if search error) +* return : none +*-----------------------------------------------------------------------------*/ +void readpos(const char *file, const char *rcv, double *pos) +{ + static double poss[2048][3]; + static char stas[2048][16]; + FILE *fp; + int i,j,len,np=0; + char buff[256],str[256]; + + trace(3,"readpos: file=%s\n",file); + + if (!(fp=fopen(file,"r"))) { + fprintf(stderr,"reference position file open error : %s\n",file); + return; + } + while (np<2048&&fgets(buff,sizeof(buff),fp)) { + if (buff[0]=='%'||buff[0]=='#') continue; + if (sscanf(buff,"%lf %lf %lf %s",&poss[np][0],&poss[np][1],&poss[np][2], + str)<4) continue; + strncpy(stas[np],str,15); stas[np++][15]='\0'; + } + fclose(fp); + len=(int)strlen(rcv); + for (i=0;in>=erp->nmax) { + erp->nmax=erp->nmax<=0?128:erp->nmax*2; + erp_data=(erpd_t *)realloc(erp->data,sizeof(erpd_t)*erp->nmax); + if (!erp_data) { + free(erp->data); erp->data=NULL; erp->n=erp->nmax=0; + fclose(fp); + return 0; + } + erp->data=erp_data; + } + erp->data[erp->n].mjd=v[0]; + erp->data[erp->n].xp=v[1]*1E-6*AS2R; + erp->data[erp->n].yp=v[2]*1E-6*AS2R; + erp->data[erp->n].ut1_utc=v[3]*1E-7; + erp->data[erp->n].lod=v[4]*1E-7; + erp->data[erp->n].xpr=v[12]*1E-6*AS2R; + erp->data[erp->n++].ypr=v[13]*1E-6*AS2R; + } + fclose(fp); + return 1; +} +/* get earth rotation parameter values ----------------------------------------- +* get earth rotation parameter values +* args : erp_t *erp I earth rotation parameters +* gtime_t time I time (gpst) +* double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) +* return : status (1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int geterp(const erp_t *erp, gtime_t time, double *erpv) +{ + const double ep[]={2000,1,1,12,0,0}; + double mjd,day,a; + int i,j,k; + + trace(4,"geterp:\n"); + + if (erp->n<=0) return 0; + + mjd=51544.5+(timediff(gpst2utc(time),epoch2time(ep)))/86400.0; + + if (mjd<=erp->data[0].mjd) { + day=mjd-erp->data[0].mjd; + erpv[0]=erp->data[0].xp +erp->data[0].xpr*day; + erpv[1]=erp->data[0].yp +erp->data[0].ypr*day; + erpv[2]=erp->data[0].ut1_utc-erp->data[0].lod*day; + erpv[3]=erp->data[0].lod; + return 1; + } + if (mjd>=erp->data[erp->n-1].mjd) { + day=mjd-erp->data[erp->n-1].mjd; + erpv[0]=erp->data[erp->n-1].xp +erp->data[erp->n-1].xpr*day; + erpv[1]=erp->data[erp->n-1].yp +erp->data[erp->n-1].ypr*day; + erpv[2]=erp->data[erp->n-1].ut1_utc-erp->data[erp->n-1].lod*day; + erpv[3]=erp->data[erp->n-1].lod; + return 1; + } + for (j=0,k=erp->n-1;jdata[i].mjd) k=i; else j=i; + } + if (erp->data[j].mjd==erp->data[j+1].mjd) { + a=0.5; + } + else { + a=(mjd-erp->data[j].mjd)/(erp->data[j+1].mjd-erp->data[j].mjd); + } + erpv[0]=(1.0-a)*erp->data[j].xp +a*erp->data[j+1].xp; + erpv[1]=(1.0-a)*erp->data[j].yp +a*erp->data[j+1].yp; + erpv[2]=(1.0-a)*erp->data[j].ut1_utc+a*erp->data[j+1].ut1_utc; + erpv[3]=(1.0-a)*erp->data[j].lod +a*erp->data[j+1].lod; + return 1; +} +/* compare ephemeris ---------------------------------------------------------*/ +int cmpeph(const void *p1, const void *p2) +{ + eph_t *q1=(eph_t *)p1,*q2=(eph_t *)p2; + return q1->ttr.time!=q2->ttr.time?(int)(q1->ttr.time-q2->ttr.time): + (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): + q1->sat-q2->sat); +} +/* sort and unique ephemeris -------------------------------------------------*/ +void uniqeph(nav_t *nav) +{ + eph_t *nav_eph; + int i,j; + + trace(3,"uniqeph: n=%d\n",nav->n); + + if (nav->n<=0) return; + + qsort(nav->eph,nav->n,sizeof(eph_t),cmpeph); + + for (i=1,j=0;in;i++) { + if (nav->eph[i].sat!=nav->eph[j].sat|| + nav->eph[i].iode!=nav->eph[j].iode) { + nav->eph[++j]=nav->eph[i]; + } + } + nav->n=j+1; + + if (!(nav_eph=(eph_t *)realloc(nav->eph,sizeof(eph_t)*nav->n))) { + trace(1,"uniqeph malloc error n=%d\n",nav->n); + free(nav->eph); nav->eph=NULL; nav->n=nav->nmax=0; + return; + } + nav->eph=nav_eph; + nav->nmax=nav->n; + + trace(4,"uniqeph: n=%d\n",nav->n); +} +/* compare glonass ephemeris -------------------------------------------------*/ +int cmpgeph(const void *p1, const void *p2) +{ + geph_t *q1=(geph_t *)p1,*q2=(geph_t *)p2; + return q1->tof.time!=q2->tof.time?(int)(q1->tof.time-q2->tof.time): + (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): + q1->sat-q2->sat); +} +/* sort and unique glonass ephemeris -----------------------------------------*/ +void uniqgeph(nav_t *nav) +{ + geph_t *nav_geph; + int i,j; + + trace(3,"uniqgeph: ng=%d\n",nav->ng); + + if (nav->ng<=0) return; + + qsort(nav->geph,nav->ng,sizeof(geph_t),cmpgeph); + + for (i=j=0;ing;i++) { + if (nav->geph[i].sat!=nav->geph[j].sat|| + nav->geph[i].toe.time!=nav->geph[j].toe.time|| + nav->geph[i].svh!=nav->geph[j].svh) { + nav->geph[++j]=nav->geph[i]; + } + } + nav->ng=j+1; + + if (!(nav_geph=(geph_t *)realloc(nav->geph,sizeof(geph_t)*nav->ng))) { + trace(1,"uniqgeph malloc error ng=%d\n",nav->ng); + free(nav->geph); nav->geph=NULL; nav->ng=nav->ngmax=0; + return; + } + nav->geph=nav_geph; + nav->ngmax=nav->ng; + + trace(4,"uniqgeph: ng=%d\n",nav->ng); +} +/* compare sbas ephemeris ----------------------------------------------------*/ +int cmpseph(const void *p1, const void *p2) +{ + seph_t *q1=(seph_t *)p1,*q2=(seph_t *)p2; + return q1->tof.time!=q2->tof.time?(int)(q1->tof.time-q2->tof.time): + (q1->t0.time!=q2->t0.time?(int)(q1->t0.time-q2->t0.time): + q1->sat-q2->sat); +} +/* sort and unique sbas ephemeris --------------------------------------------*/ +void uniqseph(nav_t *nav) +{ + seph_t *nav_seph; + int i,j; + + trace(3,"uniqseph: ns=%d\n",nav->ns); + + if (nav->ns<=0) return; + + qsort(nav->seph,nav->ns,sizeof(seph_t),cmpseph); + + for (i=j=0;ins;i++) { + if (nav->seph[i].sat!=nav->seph[j].sat|| + nav->seph[i].t0.time!=nav->seph[j].t0.time) { + nav->seph[++j]=nav->seph[i]; + } + } + nav->ns=j+1; + + if (!(nav_seph=(seph_t *)realloc(nav->seph,sizeof(seph_t)*nav->ns))) { + trace(1,"uniqseph malloc error ns=%d\n",nav->ns); + free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0; + return; + } + nav->seph=nav_seph; + nav->nsmax=nav->ns; + + trace(4,"uniqseph: ns=%d\n",nav->ns); +} +/* unique ephemerides ---------------------------------------------------------- +* unique ephemerides in navigation data and update carrier wave length +* args : nav_t *nav IO navigation data +* return : number of epochs +*-----------------------------------------------------------------------------*/ +void uniqnav(nav_t *nav) +{ + int i,j; + + trace(3,"uniqnav: neph=%d ngeph=%d nseph=%d\n",nav->n,nav->ng,nav->ns); + + /* unique ephemeris */ + uniqeph (nav); + uniqgeph(nav); + uniqseph(nav); + + /* update carrier wave length */ + for (i=0;ilam[i][j]=satwavelen(i+1,j,nav); + } +} +/* compare observation data -------------------------------------------------*/ +int cmpobs(const void *p1, const void *p2) +{ + obsd_t *q1=(obsd_t *)p1,*q2=(obsd_t *)p2; + double tt=timediff(q1->time,q2->time); + if (fabs(tt)>DTTOL) return tt<0?-1:1; + if (q1->rcv!=q2->rcv) return (int)q1->rcv-(int)q2->rcv; + return (int)q1->sat-(int)q2->sat; +} +/* sort and unique observation data -------------------------------------------- +* sort and unique observation data by time, rcv, sat +* args : obs_t *obs IO observation data +* return : number of epochs +*-----------------------------------------------------------------------------*/ +int sortobs(obs_t *obs) +{ + int i,j,n; + + trace(3,"sortobs: nobs=%d\n",obs->n); + + if (obs->n<=0) return 0; + + qsort(obs->data,obs->n,sizeof(obsd_t),cmpobs); + + /* delete duplicated data */ + for (i=j=0;in;i++) { + if (obs->data[i].sat!=obs->data[j].sat|| + obs->data[i].rcv!=obs->data[j].rcv|| + timediff(obs->data[i].time,obs->data[j].time)!=0.0) { + obs->data[++j]=obs->data[i]; + } + } + obs->n=j+1; + + for (i=n=0;in;i=j,n++) { + for (j=i+1;jn;j++) { + if (timediff(obs->data[j].time,obs->data[i].time)>DTTOL) break; + } + } + return n; +} +/* screen by time -------------------------------------------------------------- +* screening by time start, time end, and time interval +* args : gtime_t time I time +* gtime_t ts I time start (ts.time==0:no screening by ts) +* gtime_t te I time end (te.time==0:no screening by te) +* double tint I time interval (s) (0.0:no screen by tint) +* return : 1:on condition, 0:not on condition +*-----------------------------------------------------------------------------*/ +int screent(gtime_t time, gtime_t ts, gtime_t te, double tint) +{ + return (tint<=0.0||fmod(time2gpst(time,NULL)+DTTOL,tint)<=DTTOL*2.0)&& + (ts.time==0||timediff(time,ts)>=-DTTOL)&& + (te.time==0||timediff(time,te)< DTTOL); +} +/* read/save navigation data --------------------------------------------------- +* save or load navigation data +* args : char file I file path +* nav_t nav O/I navigation data +* return : status (1:ok,0:no file) +*-----------------------------------------------------------------------------*/ +int readnav(const char *file, nav_t *nav) +{ + FILE *fp; + eph_t eph0={}; + geph_t geph0={}; + char buff[4096],*p; + long toe_time,tof_time,toc_time,ttr_time; + int i,sat,prn; + + trace(3,"loadnav: file=%s\n",file); + + if (!(fp=fopen(file,"r"))) return 0; + + while (fgets(buff,sizeof(buff),fp)) { + if (!strncmp(buff,"IONUTC",6)) { + for (i=0;i<8;i++) nav->ion_gps[i]=0.0; + for (i=0;i<4;i++) nav->utc_gps[i]=0.0; + nav->leaps=0; + sscanf(buff,"IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d", + &nav->ion_gps[0],&nav->ion_gps[1],&nav->ion_gps[2],&nav->ion_gps[3], + &nav->ion_gps[4],&nav->ion_gps[5],&nav->ion_gps[6],&nav->ion_gps[7], + &nav->utc_gps[0],&nav->utc_gps[1],&nav->utc_gps[2],&nav->utc_gps[3], + &nav->leaps); + continue; + } + if ((p=strchr(buff,','))) *p='\0'; else continue; + if (!(sat=satid2no(buff))) continue; + if (satsys(sat,&prn)==SYS_GLO) { + nav->geph[prn-1]=geph0; + nav->geph[prn-1].sat=sat; + toe_time=tof_time=0; + sscanf(p+1,"%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf", + &nav->geph[prn-1].iode,&nav->geph[prn-1].frq,&nav->geph[prn-1].svh, + &nav->geph[prn-1].sva,&nav->geph[prn-1].age, + &toe_time,&tof_time, + &nav->geph[prn-1].pos[0],&nav->geph[prn-1].pos[1],&nav->geph[prn-1].pos[2], + &nav->geph[prn-1].vel[0],&nav->geph[prn-1].vel[1],&nav->geph[prn-1].vel[2], + &nav->geph[prn-1].acc[0],&nav->geph[prn-1].acc[1],&nav->geph[prn-1].acc[2], + &nav->geph[prn-1].taun ,&nav->geph[prn-1].gamn ,&nav->geph[prn-1].dtaun); + nav->geph[prn-1].toe.time=toe_time; + nav->geph[prn-1].tof.time=tof_time; + } + else { + nav->eph[sat-1]=eph0; + nav->eph[sat-1].sat=sat; + toe_time=toc_time=ttr_time=0; + sscanf(p+1,"%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d", + &nav->eph[sat-1].iode,&nav->eph[sat-1].iodc,&nav->eph[sat-1].sva , + &nav->eph[sat-1].svh , + &toe_time,&toc_time,&ttr_time, + &nav->eph[sat-1].A ,&nav->eph[sat-1].e ,&nav->eph[sat-1].i0 , + &nav->eph[sat-1].OMG0,&nav->eph[sat-1].omg ,&nav->eph[sat-1].M0 , + &nav->eph[sat-1].deln,&nav->eph[sat-1].OMGd,&nav->eph[sat-1].idot, + &nav->eph[sat-1].crc ,&nav->eph[sat-1].crs ,&nav->eph[sat-1].cuc , + &nav->eph[sat-1].cus ,&nav->eph[sat-1].cic ,&nav->eph[sat-1].cis , + &nav->eph[sat-1].toes,&nav->eph[sat-1].fit ,&nav->eph[sat-1].f0 , + &nav->eph[sat-1].f1 ,&nav->eph[sat-1].f2 ,&nav->eph[sat-1].tgd[0], + &nav->eph[sat-1].code, &nav->eph[sat-1].flag); + nav->eph[sat-1].toe.time=toe_time; + nav->eph[sat-1].toc.time=toc_time; + nav->eph[sat-1].ttr.time=ttr_time; + } + } + fclose(fp); + return 1; +} +int savenav(const char *file, const nav_t *nav) +{ + FILE *fp; + int i; + char id[32]; + + trace(3,"savenav: file=%s\n",file); + + if (!(fp=fopen(file,"w"))) return 0; + + for (i=0;ieph[i].ttr.time==0) continue; + satno2id(nav->eph[i].sat,id); + fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", + id,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].sva , + nav->eph[i].svh ,(int)nav->eph[i].toe.time, + (int)nav->eph[i].toc.time,(int)nav->eph[i].ttr.time, + nav->eph[i].A ,nav->eph[i].e ,nav->eph[i].i0 ,nav->eph[i].OMG0, + nav->eph[i].omg ,nav->eph[i].M0 ,nav->eph[i].deln,nav->eph[i].OMGd, + nav->eph[i].idot,nav->eph[i].crc,nav->eph[i].crs ,nav->eph[i].cuc , + nav->eph[i].cus ,nav->eph[i].cic,nav->eph[i].cis ,nav->eph[i].toes, + nav->eph[i].fit ,nav->eph[i].f0 ,nav->eph[i].f1 ,nav->eph[i].f2 , + nav->eph[i].tgd[0],nav->eph[i].code,nav->eph[i].flag); + } + for (i=0;igeph[i].tof.time==0) continue; + satno2id(nav->geph[i].sat,id); + fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", + id,nav->geph[i].iode,nav->geph[i].frq,nav->geph[i].svh, + nav->geph[i].sva,nav->geph[i].age,(int)nav->geph[i].toe.time, + (int)nav->geph[i].tof.time, + nav->geph[i].pos[0],nav->geph[i].pos[1],nav->geph[i].pos[2], + nav->geph[i].vel[0],nav->geph[i].vel[1],nav->geph[i].vel[2], + nav->geph[i].acc[0],nav->geph[i].acc[1],nav->geph[i].acc[2], + nav->geph[i].taun,nav->geph[i].gamn,nav->geph[i].dtaun); + } + fprintf(fp,"IONUTC,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%d", + nav->ion_gps[0],nav->ion_gps[1],nav->ion_gps[2],nav->ion_gps[3], + nav->ion_gps[4],nav->ion_gps[5],nav->ion_gps[6],nav->ion_gps[7], + nav->utc_gps[0],nav->utc_gps[1],nav->utc_gps[2],nav->utc_gps[3], + nav->leaps); + + fclose(fp); + return 1; +} +/* free observation data ------------------------------------------------------- +* free memory for observation data +* args : obs_t *obs IO observation data +* return : none +*-----------------------------------------------------------------------------*/ +void freeobs(obs_t *obs) +{ + free(obs->data); obs->data=NULL; obs->n=obs->nmax=0; +} +/* free navigation data --------------------------------------------------------- +* free memory for navigation data +* args : nav_t *nav IO navigation data +* int opt I option (or of followings) +* (0x01: gps/qzs ephmeris, 0x02: glonass ephemeris, +* 0x04: sbas ephemeris, 0x08: precise ephemeris, +* 0x10: precise clock 0x20: almanac, +* 0x40: tec data) +* return : none +*-----------------------------------------------------------------------------*/ +void freenav(nav_t *nav, int opt) +{ + if (opt&0x01) {free(nav->eph ); nav->eph =NULL; nav->n =nav->nmax =0;} + if (opt&0x02) {free(nav->geph); nav->geph=NULL; nav->ng=nav->ngmax=0;} + if (opt&0x04) {free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0;} + if (opt&0x08) {free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0;} + if (opt&0x10) {free(nav->pclk); nav->pclk=NULL; nav->nc=nav->ncmax=0;} + if (opt&0x20) {free(nav->alm ); nav->alm =NULL; nav->na=nav->namax=0;} + if (opt&0x40) {free(nav->tec ); nav->tec =NULL; nav->nt=nav->ntmax=0;} + if (opt&0x80) {free(nav->fcb ); nav->fcb =NULL; nav->nf=nav->nfmax=0;} +} +/* debug trace functions -----------------------------------------------------*/ +//#ifdef TRACE +// +//static FILE *fp_trace=NULL; /* file pointer of trace */ +//static char file_trace[1024]; /* trace file */ +//static int level_trace=0; /* level of trace */ +//static unsigned int tick_trace=0; /* tick time at traceopen (ms) */ +//static gtime_t time_trace={0}; /* time at traceopen */ +//static lock_t lock_trace; /* lock for trace */ +// +//static void traceswap(void) +//{ +// gtime_t time=utc2gpst(timeget()); +// char path[1024]; +// +// lock(&lock_trace); +// +// if ((int)(time2gpst(time ,NULL)/INT_SWAP_TRAC)== +// (int)(time2gpst(time_trace,NULL)/INT_SWAP_TRAC)) { +// unlock(&lock_trace); +// return; +// } +// time_trace=time; +// +// if (!reppath(file_trace,path,time,"","")) { +// unlock(&lock_trace); +// return; +// } +// if (fp_trace) fclose(fp_trace); +// +// if (!(fp_trace=fopen(path,"w"))) { +// fp_trace=stderr; +// } +// unlock(&lock_trace); +//} +//extern void traceopen(const char *file) +//{ +// gtime_t time=utc2gpst(timeget()); +// char path[1024]; +// +// reppath(file,path,time,"",""); +// if (!*path||!(fp_trace=fopen(path,"w"))) fp_trace=stderr; +// strcpy(file_trace,file); +// tick_trace=tickget(); +// time_trace=time; +// initlock(&lock_trace); +//} +//extern void traceclose(void) +//{ +// if (fp_trace&&fp_trace!=stderr) fclose(fp_trace); +// fp_trace=NULL; +// file_trace[0]='\0'; +//} +//extern void tracelevel(int level) +//{ +// level_trace=level; +//} +//extern void trace(int level, const char *format, ...) +//{ +// va_list ap; +// +// /* print error message to stderr */ +// if (level<=1) { +// va_start(ap,format); vfprintf(stderr,format,ap); va_end(ap); +// } +// if (!fp_trace||level>level_trace) return; +// traceswap(); +// fprintf(fp_trace,"%d ",level); +// va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap); +// fflush(fp_trace); +//} +//extern void tracet(int level, const char *format, ...) +//{ +// va_list ap; +// +// if (!fp_trace||level>level_trace) return; +// traceswap(); +// fprintf(fp_trace,"%d %9.3f: ",level,(tickget()-tick_trace)/1000.0); +// va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap); +// fflush(fp_trace); +//} +//extern void tracemat(int level, const double *A, int n, int m, int p, int q) +//{ +// if (!fp_trace||level>level_trace) return; +// matfprint(A,n,m,p,q,fp_trace); fflush(fp_trace); +//} +//extern void traceobs(int level, const obsd_t *obs, int n) +//{ +// char str[64],id[16]; +// int i; +// +// if (!fp_trace||level>level_trace) return; +// for (i=0;ilevel_trace) return; +// for (i=0;in;i++) { +// time2str(nav->eph[i].toe,s1,0); +// time2str(nav->eph[i].ttr,s2,0); +// satno2id(nav->eph[i].sat,id); +// fprintf(fp_trace,"(%3d) %-3s : %s %s %3d %3d %02x\n",i+1, +// id,s1,s2,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].svh); +// } +// fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gps[0], +// nav->ion_gps[1],nav->ion_gps[2],nav->ion_gps[3]); +// fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gps[4], +// nav->ion_gps[5],nav->ion_gps[6],nav->ion_gps[7]); +// fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gal[0], +// nav->ion_gal[1],nav->ion_gal[2],nav->ion_gal[3]); +//} +//extern void tracegnav(int level, const nav_t *nav) +//{ +// char s1[64],s2[64],id[16]; +// int i; +// +// if (!fp_trace||level>level_trace) return; +// for (i=0;ing;i++) { +// time2str(nav->geph[i].toe,s1,0); +// time2str(nav->geph[i].tof,s2,0); +// satno2id(nav->geph[i].sat,id); +// fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d %8.3f\n",i+1, +// id,s1,s2,nav->geph[i].frq,nav->geph[i].svh,nav->geph[i].taun*1E6); +// } +//} +//extern void tracehnav(int level, const nav_t *nav) +//{ +// char s1[64],s2[64],id[16]; +// int i; +// +// if (!fp_trace||level>level_trace) return; +// for (i=0;ins;i++) { +// time2str(nav->seph[i].t0,s1,0); +// time2str(nav->seph[i].tof,s2,0); +// satno2id(nav->seph[i].sat,id); +// fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d\n",i+1, +// id,s1,s2,nav->seph[i].svh,nav->seph[i].sva); +// } +//} +//extern void tracepeph(int level, const nav_t *nav) +//{ +// char s[64],id[16]; +// int i,j; +// +// if (!fp_trace||level>level_trace) return; +// +// for (i=0;ine;i++) { +// time2str(nav->peph[i].time,s,0); +// for (j=0;jpeph[i].index,id, +// nav->peph[i].pos[j][0],nav->peph[i].pos[j][1], +// nav->peph[i].pos[j][2],nav->peph[i].pos[j][3]*1E9, +// nav->peph[i].std[j][0],nav->peph[i].std[j][1], +// nav->peph[i].std[j][2],nav->peph[i].std[j][3]*1E9); +// } +// } +//} +//extern void tracepclk(int level, const nav_t *nav) +//{ +// char s[64],id[16]; +// int i,j; +// +// if (!fp_trace||level>level_trace) return; +// +// for (i=0;inc;i++) { +// time2str(nav->pclk[i].time,s,0); +// for (j=0;jpclk[i].index,id, +// nav->pclk[i].clk[j][0]*1E9,nav->pclk[i].std[j][0]*1E9); +// } +// } +//} +//extern void traceb(int level, const unsigned char *p, int n) +//{ +// int i; +// if (!fp_trace||level>level_trace) return; +// for (i=0;i:error) +*-----------------------------------------------------------------------------*/ +int execcmd(const char *cmd) +{ + + trace(3,"execcmd: cmd=%s\n",cmd); + + return system(cmd); +} + +/* create directory ------------------------------------------------------------ +* create directory if not exist +* args : char *path I file path to be saved +* return : none +* notes : not recursive. only one level +*-----------------------------------------------------------------------------*/ +void createdir(const char *path) +{ + char buff[1024],*p; + + tracet(3,"createdir: path=%s\n",path); + + strcpy(buff,path); + if (!(p=strrchr(buff,FILEPATHSEP))) return; + *p='\0'; + + mkdir(buff,0777); +} +/* replace string ------------------------------------------------------------*/ +int repstr(char *str, const char *pat, const char *rep) +{ + int len=(int)strlen(pat); + char buff[1024],*p,*q,*r; + + for (p=str,r=buff;*p;p=q+len) { + if (!(q=strstr(p,pat))) break; + strncpy(r,p,q-p); + r+=q-p; + r+=sprintf(r,"%s",rep); + } + if (p<=str) return 0; + strcpy(r,p); + strcpy(str,buff); + return 1; +} +/* replace keywords in file path ----------------------------------------------- +* replace keywords in file path with date, time, rover and base station id +* args : char *path I file path (see below) +* char *rpath O file path in which keywords replaced (see below) +* gtime_t time I time (gpst) (time.time==0: not replaced) +* char *rov I rover id string ("": not replaced) +* char *base I base station id string ("": not replaced) +* return : status (1:keywords replaced, 0:no valid keyword in the path, +* -1:no valid time) +* notes : the following keywords in path are replaced by date, time and name +* %Y -> yyyy : year (4 digits) (1900-2099) +* %y -> yy : year (2 digits) (00-99) +* %m -> mm : month (01-12) +* %d -> dd : day of month (01-31) +* %h -> hh : hours (00-23) +* %M -> mm : minutes (00-59) +* %S -> ss : seconds (00-59) +* %n -> ddd : day of year (001-366) +* %W -> wwww : gps week (0001-9999) +* %D -> d : day of gps week (0-6) +* %H -> h : hour code (a=0,b=1,c=2,...,x=23) +* %ha-> hh : 3 hours (00,03,06,...,21) +* %hb-> hh : 6 hours (00,06,12,18) +* %hc-> hh : 12 hours (00,12) +* %t -> mm : 15 minutes (00,15,30,45) +* %r -> rrrr : rover id +* %b -> bbbb : base station id +*-----------------------------------------------------------------------------*/ +int reppath(const char *path, char *rpath, gtime_t time, const char *rov, + const char *base) +{ + double ep[6],ep0[6]={2000,1,1,0,0,0}; + int week,dow,doy,stat=0; + char rep[64]; + + strcpy(rpath,path); + + if (!strstr(rpath,"%")) return 0; + if (*rov ) stat|=repstr(rpath,"%r",rov ); + if (*base) stat|=repstr(rpath,"%b",base); + if (time.time!=0) { + time2epoch(time,ep); + ep0[0]=ep[0]; + dow=(int)floor(time2gpst(time,&week)/86400.0); + doy=(int)floor(timediff(time,epoch2time(ep0))/86400.0)+1; + sprintf(rep,"%02d", ((int)ep[3]/3)*3); stat|=repstr(rpath,"%ha",rep); + sprintf(rep,"%02d", ((int)ep[3]/6)*6); stat|=repstr(rpath,"%hb",rep); + sprintf(rep,"%02d", ((int)ep[3]/12)*12); stat|=repstr(rpath,"%hc",rep); + sprintf(rep,"%04.0f",ep[0]); stat|=repstr(rpath,"%Y",rep); + sprintf(rep,"%02.0f",fmod(ep[0],100.0)); stat|=repstr(rpath,"%y",rep); + sprintf(rep,"%02.0f",ep[1]); stat|=repstr(rpath,"%m",rep); + sprintf(rep,"%02.0f",ep[2]); stat|=repstr(rpath,"%d",rep); + sprintf(rep,"%02.0f",ep[3]); stat|=repstr(rpath,"%h",rep); + sprintf(rep,"%02.0f",ep[4]); stat|=repstr(rpath,"%M",rep); + sprintf(rep,"%02.0f",floor(ep[5])); stat|=repstr(rpath,"%S",rep); + sprintf(rep,"%03d", doy); stat|=repstr(rpath,"%n",rep); + sprintf(rep,"%04d", week); stat|=repstr(rpath,"%W",rep); + sprintf(rep,"%d", dow); stat|=repstr(rpath,"%D",rep); + sprintf(rep,"%c", 'a'+(int)ep[3]); stat|=repstr(rpath,"%H",rep); + sprintf(rep,"%02d", ((int)ep[4]/15)*15); stat|=repstr(rpath,"%t",rep); + } + else if (strstr(rpath,"%ha")||strstr(rpath,"%hb")||strstr(rpath,"%hc")|| + strstr(rpath,"%Y" )||strstr(rpath,"%y" )||strstr(rpath,"%m" )|| + strstr(rpath,"%d" )||strstr(rpath,"%h" )||strstr(rpath,"%M" )|| + strstr(rpath,"%S" )||strstr(rpath,"%n" )||strstr(rpath,"%W" )|| + strstr(rpath,"%D" )||strstr(rpath,"%H" )||strstr(rpath,"%t" )) { + return -1; /* no valid time */ + } + return stat; +} +/* replace keywords in file path and generate multiple paths ------------------- +* replace keywords in file path with date, time, rover and base station id +* generate multiple keywords-replaced paths +* args : char *path I file path (see below) +* char *rpath[] O file paths in which keywords replaced +* int nmax I max number of output file paths +* gtime_t ts I time start (gpst) +* gtime_t te I time end (gpst) +* char *rov I rover id string ("": not replaced) +* char *base I base station id string ("": not replaced) +* return : number of replaced file paths +* notes : see reppath() for replacements of keywords. +* minimum interval of time replaced is 900s. +*-----------------------------------------------------------------------------*/ +int reppaths(const char *path, char *rpath[], int nmax, gtime_t ts, + gtime_t te, const char *rov, const char *base) +{ + gtime_t time; + double tow,tint=86400.0; + int i,n=0,week; + + trace(3,"reppaths: path =%s nmax=%d rov=%s base=%s\n",path,nmax,rov,base); + + if (ts.time==0||te.time==0||timediff(ts,te)>0.0) return 0; + + if (strstr(path,"%S")||strstr(path,"%M")||strstr(path,"%t")) tint=900.0; + else if (strstr(path,"%h")||strstr(path,"%H")) tint=3600.0; + + tow=time2gpst(ts,&week); + time=gpst2time(week,floor(tow/tint)*tint); + + while (timediff(time,te)<=0.0&&nng;i++) { + if (nav->geph[i].sat!=sat) continue; + return CLIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); + } + } + else if (frq==2) { /* L3 */ + return CLIGHT/FREQ3_GLO; + } + } + else if (sys==SYS_CMP) { + if (frq==0) return CLIGHT/FREQ1_CMP; /* B1 */ + else if (frq==1) return CLIGHT/FREQ2_CMP; /* B2 */ + else if (frq==2) return CLIGHT/FREQ3_CMP; /* B3 */ + } + else { + if (frq==0) return CLIGHT/FREQ1; /* L1/E1 */ + else if (frq==1) return CLIGHT/FREQ2; /* L2 */ + else if (frq==2) return CLIGHT/FREQ5; /* L5/E5a */ + else if (frq==3) return CLIGHT/FREQ6; /* L6/LEX */ + else if (frq==4) return CLIGHT/FREQ7; /* E5b */ + else if (frq==5) return CLIGHT/FREQ8; /* E5a+b */ + else if (frq==6) return CLIGHT/FREQ9; /* S */ + } + return 0.0; +} +/* geometric distance ---------------------------------------------------------- +* compute geometric distance and receiver-to-satellite unit vector +* args : double *rs I satellilte position (ecef at transmission) (m) +* double *rr I receiver position (ecef at reception) (m) +* double *e O line-of-sight vector (ecef) +* return : geometric distance (m) (0>:error/no satellite position) +* notes : distance includes sagnac effect correction +*-----------------------------------------------------------------------------*/ +double geodist(const double *rs, const double *rr, double *e) +{ + double r; + int i; + + if (norm(rs,3)-RE_WGS84) { + ecef2enu(pos,e,enu); + az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]); + if (az<0.0) az+=2*PI; + el=asin(enu[2]); + } + if (azel) {azel[0]=az; azel[1]=el;} + return el; +} +/* compute dops ---------------------------------------------------------------- +* compute DOP (dilution of precision) +* args : int ns I number of satellites +* double *azel I satellite azimuth/elevation angle (rad) +* double elmin I elevation cutoff angle (rad) +* double *dop O DOPs {GDOP,PDOP,HDOP,VDOP} +* return : none +* notes : dop[0]-[3] return 0 in case of dop computation error +*-----------------------------------------------------------------------------*/ + + +void dops(int ns, const double *azel, double elmin, double *dop) +{ + double H[4*MAXSAT],Q[16],cosel,sinel; + int i,n; + + for (i=0;i<4;i++) dop[i]=0.0; + for (i=n=0;i 0.416) phi= 0.416; + else if (phi<-0.416) phi=-0.416; + lam=pos[1]/PI+psi*sin(azel[0])/cos(phi*PI); + + /* geomagnetic latitude (semi-circle) */ + phi+=0.064*cos((lam-1.617)*PI); + + /* local time (s) */ + tt=43200.0*lam+time2gpst(t,&week); + tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 */ + + /* slant factor */ + f=1.0+16.0*pow(0.53-azel[1]/PI,3.0); + + /* ionospheric delay */ + amp=ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3])); + per=ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7])); + amp=amp< 0.0? 0.0:amp; + per=per<72000.0?72000.0:per; + x=2.0*PI*(tt-50400.0)/per; + + return CLIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9); +} +/* ionosphere mapping function ------------------------------------------------- +* compute ionospheric delay mapping function by single layer model +* args : double *pos I receiver position {lat,lon,h} (rad,m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* return : ionospheric mapping function +*-----------------------------------------------------------------------------*/ +double ionmapf(const double *pos, const double *azel) +{ + if (pos[2]>=HION) return 1.0; + return 1.0/cos(asin((RE_WGS84+pos[2])/(RE_WGS84+HION)*sin(PI/2.0-azel[1]))); +} +/* ionospheric pierce point position ------------------------------------------- +* compute ionospheric pierce point (ipp) position and slant factor +* args : double *pos I receiver position {lat,lon,h} (rad,m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* double re I earth radius (km) +* double hion I altitude of ionosphere (km) +* double *posp O pierce point position {lat,lon,h} (rad,m) +* return : slant factor +* notes : see ref [2], only valid on the earth surface +* fixing bug on ref [2] A.4.4.10.1 A-22,23 +*-----------------------------------------------------------------------------*/ +double ionppp(const double *pos, const double *azel, double re, + double hion, double *posp) +{ + double cosaz,rp,ap,sinap,tanap; + + rp=re/(re+hion)*cos(azel[1]); + ap=PI/2.0-azel[1]-asin(rp); + sinap=sin(ap); + tanap=tan(ap); + cosaz=cos(azel[0]); + posp[0]=asin(sin(pos[0])*cos(ap)+cos(pos[0])*sinap*cosaz); + + if ((pos[0]> 70.0*D2R&& tanap*cosaz>tan(PI/2.0-pos[0]))|| + (pos[0]<-70.0*D2R&&-tanap*cosaz>tan(PI/2.0+pos[0]))) { + posp[1]=pos[1]+PI-asin(sinap*sin(azel[0])/cos(posp[0])); + } + else { + posp[1]=pos[1]+asin(sinap*sin(azel[0])/cos(posp[0])); + } + return 1.0/sqrt(1.0-rp*rp); +} +/* troposphere model ----------------------------------------------------------- +* compute tropospheric delay by standard atmosphere and saastamoinen model +* args : gtime_t time I time +* double *pos I receiver position {lat,lon,h} (rad,m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* double humi I relative humidity +* return : tropospheric delay (m) +*-----------------------------------------------------------------------------*/ +double tropmodel(gtime_t time, const double *pos, const double *azel, + double humi) +{ + const double temp0=15.0; /* temparature at sea level */ + double hgt,pres,temp,e,z,trph,trpw; + + if (pos[2]<-100.0||1E44) return coef[4]; + return coef[i-1]*(1.0-lat/15.0+i)+coef[i]*(lat/15.0-i); +} +double mapf(double el, double a, double b, double c) +{ + double sinel=sin(el); + return (1.0+a/(1.0+b/(1.0+c)))/(sinel+(a/(sinel+b/(sinel+c)))); +} +double nmf(gtime_t time, const double pos[], const double azel[], + double *mapfw) +{ + /* ref [5] table 3 */ + /* hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75 */ + const double coef[][5]={ + { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, + { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, + { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, + + { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5}, + { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, + { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, + + { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, + { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, + { 4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2} + }; + const double aht[]={ 2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */ + + double y,cosy,ah[3],aw[3],dm,el=azel[1],lat=pos[0]*R2D,hgt=pos[2]; + int i; + + if (el<=0.0) { + if (mapfw) *mapfw=0.0; + return 0.0; + } + /* year from doy 28, added half a year for southern latitudes */ + y=(time2doy(time)-28.0)/365.25+(lat<0.0?0.5:0.0); + + cosy=cos(2.0*PI*y); + lat=fabs(lat); + + for (i=0;i<3;i++) { + ah[i]=interpc(coef[i ],lat)-interpc(coef[i+3],lat)*cosy; + aw[i]=interpc(coef[i+6],lat); + } + /* ellipsoidal height is used instead of height above sea level */ + dm=(1.0/sin(el)-mapf(el,aht[0],aht[1],aht[2]))*hgt/1E3; + + if (mapfw) *mapfw=mapf(el,aw[0],aw[1],aw[2]); + + return mapf(el,ah[0],ah[1],ah[2])+dm; +} +#endif /* !IERS_MODEL */ + +/* troposphere mapping function ------------------------------------------------ +* compute tropospheric mapping function by NMF +* args : gtime_t t I time +* double *pos I receiver position {lat,lon,h} (rad,m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* double *mapfw IO wet mapping function (NULL: not output) +* return : dry mapping function +* note : see ref [5] (NMF) and [9] (GMF) +* original JGR paper of [5] has bugs in eq.(4) and (5). the corrected +* paper is obtained from: +* ftp://web.haystack.edu/pub/aen/nmf/NMF_JGR.pdf +*-----------------------------------------------------------------------------*/ +double tropmapf(gtime_t time, const double pos[], const double azel[], + double *mapfw) +{ +#ifdef IERS_MODEL + const double ep[]={2000,1,1,12,0,0}; + double mjd,lat,lon,hgt,zd,gmfh,gmfw; +#endif + trace(4,"tropmapf: pos=%10.6f %11.6f %6.1f azel=%5.1f %4.1f\n", + pos[0]*R2D,pos[1]*R2D,pos[2],azel[0]*R2D,azel[1]*R2D); + + if (pos[2]<-1000.0||pos[2]>20000.0) { + if (mapfw) *mapfw=0.0; + return 0.0; + } +#ifdef IERS_MODEL + mjd=51544.5+(timediff(time,epoch2time(ep)))/86400.0; + lat=pos[0]; + lon=pos[1]; + hgt=pos[2]-geoidh(pos); /* height in m (mean sea level) */ + zd =PI/2.0-azel[1]; + + /* call GMF */ + gmf_(&mjd,&lat,&lon,&hgt,&zd,&gmfh,&gmfw); + + if (mapfw) *mapfw=gmfw; + return gmfh; +#else + return nmf(time,pos,azel,mapfw); /* NMF */ +#endif +} +/* interpolate antenna phase center variation --------------------------------*/ +double interpvar(double ang, const double *var) +{ + double a=ang/5.0; /* ang=0-90 */ + int i=(int)a; + if (i<0) return var[0]; else if (i>=18) return var[18]; + return var[i]*(1.0-a+i)+var[i+1]*(a-i); +} +/* receiver antenna model ------------------------------------------------------ +* compute antenna offset by antenna phase center parameters +* args : pcv_t *pcv I antenna phase center parameters +* double *azel I azimuth/elevation for receiver {az,el} (rad) +* int opt I option (0:only offset,1:offset+pcv) +* double *dant O range offsets for each frequency (m) +* return : none +* notes : current version does not support azimuth dependent terms +*-----------------------------------------------------------------------------*/ +void antmodel(const pcv_t *pcv, const double *del, const double *azel, + int opt, double *dant) +{ + double e[3],off[3],cosel=cos(azel[1]); + int i,j; + + trace(4,"antmodel: azel=%6.1f %4.1f opt=%d\n",azel[0]*R2D,azel[1]*R2D,opt); + + e[0]=sin(azel[0])*cosel; + e[1]=cos(azel[0])*cosel; + e[2]=sin(azel[1]); + + for (i=0;ioff[i][j]+del[j]; + + dant[i]=-dot(off,e,3)+(opt?interpvar(90.0-azel[1]*R2D,pcv->var[i]):0.0); + } + trace(5,"antmodel: dant=%6.3f %6.3f\n",dant[0],dant[1]); +} +/* satellite antenna model ------------------------------------------------------ +* compute satellite antenna phase center parameters +* args : pcv_t *pcv I antenna phase center parameters +* double nadir I nadir angle for satellite (rad) +* double *dant O range offsets for each frequency (m) +* return : none +*-----------------------------------------------------------------------------*/ +void antmodel_s(const pcv_t *pcv, double nadir, double *dant) +{ + int i; + + trace(4,"antmodel_s: nadir=%6.1f\n",nadir*R2D); + + for (i=0;ivar[i]); + } + trace(5,"antmodel_s: dant=%6.3f %6.3f\n",dant[0],dant[1]); +} +/* sun and moon position in eci (ref [4] 5.1.1, 5.2.1) -----------------------*/ +void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon) +{ + const double ep2000[]={2000,1,1,12,0,0}; + double t,f[5],eps,Ms,ls,rs,lm,pm,rm,sine,cose,sinp,cosp,sinl,cosl; + + trace(4,"sunmoonpos_eci: tut=%s\n",time_str(tut,3)); + + t=timediff(tut,epoch2time(ep2000))/86400.0/36525.0; + + /* astronomical arguments */ + ast_args(t,f); + + /* obliquity of the ecliptic */ + eps=23.439291-0.0130042*t; + sine=sin(eps*D2R); cose=cos(eps*D2R); + + /* sun position in eci */ + if (rsun) { + Ms=357.5277233+35999.05034*t; + ls=280.460+36000.770*t+1.914666471*sin(Ms*D2R)+0.019994643*sin(2.0*Ms*D2R); + rs=AU*(1.000140612-0.016708617*cos(Ms*D2R)-0.000139589*cos(2.0*Ms*D2R)); + sinl=sin(ls*D2R); cosl=cos(ls*D2R); + rsun[0]=rs*cosl; + rsun[1]=rs*cose*sinl; + rsun[2]=rs*sine*sinl; + + trace(5,"rsun =%.3f %.3f %.3f\n",rsun[0],rsun[1],rsun[2]); + } + /* moon position in eci */ + if (rmoon) { + lm=218.32+481267.883*t+6.29*sin(f[0])-1.27*sin(f[0]-2.0*f[3])+ + 0.66*sin(2.0*f[3])+0.21*sin(2.0*f[0])-0.19*sin(f[1])-0.11*sin(2.0*f[2]); + pm=5.13*sin(f[2])+0.28*sin(f[0]+f[2])-0.28*sin(f[2]-f[0])- + 0.17*sin(f[2]-2.0*f[3]); + rm=RE_WGS84/sin((0.9508+0.0518*cos(f[0])+0.0095*cos(f[0]-2.0*f[3])+ + 0.0078*cos(2.0*f[3])+0.0028*cos(2.0*f[0]))*D2R); + sinl=sin(lm*D2R); cosl=cos(lm*D2R); + sinp=sin(pm*D2R); cosp=cos(pm*D2R); + rmoon[0]=rm*cosp*cosl; + rmoon[1]=rm*(cose*cosp*sinl-sine*sinp); + rmoon[2]=rm*(sine*cosp*sinl+cose*sinp); + + trace(5,"rmoon=%.3f %.3f %.3f\n",rmoon[0],rmoon[1],rmoon[2]); + } +} +/* sun and moon position ------------------------------------------------------- +* get sun and moon position in ecef +* args : gtime_t tut I time in ut1 +* double *erpv I erp value {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) +* double *rsun IO sun position in ecef (m) (NULL: not output) +* double *rmoon IO moon position in ecef (m) (NULL: not output) +* double *gmst O gmst (rad) +* return : none +*-----------------------------------------------------------------------------*/ +void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, + double *rmoon, double *gmst) +{ + gtime_t tut; + double rs[3],rm[3],U[9],gmst_; + + trace(4,"sunmoonpos: tutc=%s\n",time_str(tutc,3)); + + tut=timeadd(tutc,erpv[2]); /* utc -> ut1 */ + + /* sun and moon position in eci */ + sunmoonpos_eci(tut,rsun?rs:NULL,rmoon?rm:NULL); + + /* eci to ecef transformation matrix */ + eci2ecef(tutc,erpv,U,&gmst_); + + /* sun and moon postion in ecef */ + if (rsun ) matmul("NN",3,1,3,1.0,U,rs,0.0,rsun ); + if (rmoon) matmul("NN",3,1,3,1.0,U,rm,0.0,rmoon); + if (gmst ) *gmst=gmst_; +} +/* carrier smoothing ----------------------------------------------------------- +* carrier smoothing by Hatch filter +* args : obs_t *obs IO raw observation data/smoothed observation data +* int ns I smoothing window size (epochs) +* return : none +*-----------------------------------------------------------------------------*/ +void csmooth(obs_t *obs, int ns) +{ + double Ps[2][MAXSAT][NFREQ]={},Lp[2][MAXSAT][NFREQ]={},dcp; + int i,j,s,r,n[2][MAXSAT][NFREQ]={}; + obsd_t *p; + + trace(3,"csmooth: nobs=%d,ns=%d\n",obs->n,ns); + + for (i=0;in;i++) { + p=&obs->data[i]; s=p->sat; r=p->rcv; + for (j=0;jP[j]==0.0||p->L[j]==0.0) continue; + if (p->LLI[j]) n[r-1][s-1][j]=0; + if (n[r-1][s-1][j]==0) Ps[r-1][s-1][j]=p->P[j]; + else { + dcp=lam_carr[j]*(p->L[j]-Lp[r-1][s-1][j]); + Ps[r-1][s-1][j]=p->P[j]/ns+(Ps[r-1][s-1][j]+dcp)*(ns-1)/ns; + } + if (++n[r-1][s-1][j]P[j]=0.0; else p->P[j]=Ps[r-1][s-1][j]; + Lp[r-1][s-1][j]=p->L[j]; + } + } +} +/* uncompress file ------------------------------------------------------------- +* uncompress (uncompress/unzip/uncompact hatanaka-compression/tar) file +* args : char *file I input file +* char *uncfile O uncompressed file +* return : status (-1:error,0:not compressed file,1:uncompress completed) +* note : creates uncompressed file in tempolary directory +* gzip and crx2rnx commands have to be installed in commands path +*-----------------------------------------------------------------------------*/ +int rtk_uncompress(const char *file, char *uncfile) +{ + int stat=0; + char *p,cmd[2048]="",tmpfile[1024]="",buff[1024],*fname,*dir=(char*)""; + + trace(3,"rtk_uncompress: file=%s\n",file); + + strcpy(tmpfile,file); + if (!(p=strrchr(tmpfile,'.'))) return 0; + + /* uncompress by gzip */ + if (!strcmp(p,".z" )||!strcmp(p,".Z" )|| + !strcmp(p,".gz" )||!strcmp(p,".GZ" )|| + !strcmp(p,".zip")||!strcmp(p,".ZIP")) { + + strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; + sprintf(cmd,"gzip -f -d -c \"%s\" > \"%s\"",tmpfile,uncfile); + + if (execcmd(cmd)) { + remove(uncfile); + return -1; + } + strcpy(tmpfile,uncfile); + stat=1; + } + /* extract tar file */ + if ((p=strrchr(tmpfile,'.'))&&!strcmp(p,".tar")) { + + strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; + strcpy(buff,tmpfile); + fname=buff; + if ((p=strrchr(buff,'/'))) { + *p='\0'; dir=fname; fname=p+1; + } + sprintf(cmd,"tar -C \"%s\" -xf \"%s\"",dir,tmpfile); + if (execcmd(cmd)) { + if (stat) remove(tmpfile); + return -1; + } + if (stat) remove(tmpfile); + stat=1; + } + /* extract hatanaka-compressed file by cnx2rnx */ + else if ((p=strrchr(tmpfile,'.'))&&strlen(p)>3&&(*(p+3)=='d'||*(p+3)=='D')) { + + strcpy(uncfile,tmpfile); + uncfile[p-tmpfile+3]=*(p+3)=='D'?'O':'o'; + sprintf(cmd,"crx2rnx < \"%s\" > \"%s\"",tmpfile,uncfile); + + if (execcmd(cmd)) { + remove(uncfile); + if (stat) remove(tmpfile); + return -1; + } + if (stat) remove(tmpfile); + stat=1; + } + trace(3,"rtk_uncompress: stat=%d\n",stat); + return stat; +} + +/* expand file path ------------------------------------------------------------ +* expand file path with wild-card (*) in file +* args : char *path I file path to expand (captal insensitive) +* char *paths O expanded file paths +* int nmax I max number of expanded file paths +* return : number of expanded file paths +* notes : the order of expanded files is alphabetical order +*-----------------------------------------------------------------------------*/ +int expath(const char *path, char *paths[], int nmax) +{ + int i,j,n=0; + char tmp[1024]; + struct dirent *d; + DIR *dp; + const char *file=path; + char dir[1024]="",s1[1024],s2[1024],*p,*q,*r; + + trace(3,"expath : path=%s nmax=%d\n",path,nmax); + + //TODO: Fix invalid conversion from ‘const char*’ to ‘char*’ + //if ((p=strrchr(path,'/'))||(p=strrchr(path,'\\'))) { + // file=p+1; strncpy(dir,path,p-path+1); dir[p-path+1]='\0'; + //} + if (!(dp=opendir(*dir?dir:"."))) return 0; + while ((d=readdir(dp))) { + if (*(d->d_name)=='.') continue; + sprintf(s1,"^%s$",d->d_name); + sprintf(s2,"^%s$",file); + for (p=s1;*p;p++) *p=(char)tolower((int)*p); + for (p=s2;*p;p++) *p=(char)tolower((int)*p); + + for (p=s1,q=strtok_r(s2,"*",&r);q;q=strtok_r(NULL,"*",&r)) { + if ((p=strstr(p,q))) p+=strlen(q); else break; + } + if (p&&nd_name); + } + closedir(dp); + /* sort paths in alphabetical order */ + for (i=0;i0) { + strcpy(tmp,paths[i]); + strcpy(paths[i],paths[j]); + strcpy(paths[j],tmp); + } + } + } + for (i=0;i + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + * + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +* +* options : -DLAPACK use LAPACK/BLAS +* -DMKL use Intel MKL +* -DTRACE enable debug trace +* -DWIN32 use WIN32 API +* -DNOCALLOC no use calloc for zero matrix +* -DIERS_MODEL use GMF instead of NMF +* -DDLL built for shared library +* -DCPUTIME_IN_GPST cputime operated in gpst +* +* references : +* [1] IS-GPS-200D, Navstar GPS Space Segment/Navigation User Interfaces, +* 7 March, 2006 +* [2] RTCA/DO-229C, Minimum operational performanc standards for global +* positioning system/wide area augmentation system airborne equipment, +* RTCA inc, November 28, 2001 +* [3] M.Rothacher, R.Schmid, ANTEX: The Antenna Exchange Format Version 1.4, +* 15 September, 2010 +* [4] A.Gelb ed., Applied Optimal Estimation, The M.I.T Press, 1974 +* [5] A.E.Niell, Global mapping functions for the atmosphere delay at radio +* wavelengths, Jounal of geophysical research, 1996 +* [6] W.Gurtner and L.Estey, RINEX The Receiver Independent Exchange Format +* Version 3.00, November 28, 2007 +* [7] J.Kouba, A Guide to using International GNSS Service (IGS) products, +* May 2009 +* [8] China Satellite Navigation Office, BeiDou navigation satellite system +* signal in space interface control document, open service signal B1I +* (version 1.0), Dec 2012 +* [9] J.Boehm, A.Niell, P.Tregoning and H.Shuh, Global Mapping Function +* (GMF): A new empirical mapping function base on numerical weather +* model data, Geophysical Research Letters, 33, L07304, 2006 +* [10] GLONASS/GPS/Galileo/Compass/SBAS NV08C receiver series BINR interface +* protocol specification ver.1.3, August, 2012 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2007/01/12 1.0 new +* 2007/03/06 1.1 input initial rover pos of pntpos() +* update only effective states of filter() +* fix bug of atan2() domain error +* 2007/04/11 1.2 add function antmodel() +* add gdop mask for pntpos() +* change constant MAXDTOE value +* 2007/05/25 1.3 add function execcmd(),expandpath() +* 2008/06/21 1.4 add funciton sortobs(),uniqeph(),screent() +* replace geodist() by sagnac correction way +* 2008/10/29 1.5 fix bug of ionosphereic mapping function +* fix bug of seasonal variation term of tropmapf +* 2008/12/27 1.6 add function tickget(), sleepms(), tracenav(), +* xyz2enu(), satposv(), pntvel(), covecef() +* 2009/03/12 1.7 fix bug on error-stop when localtime() returns NULL +* 2009/03/13 1.8 fix bug on time adjustment for summer time +* 2009/04/10 1.9 add function adjgpsweek(),getbits(),getbitu() +* add function geph2pos() +* 2009/06/08 1.10 add function seph2pos() +* 2009/11/28 1.11 change function pntpos() +* add function tracegnav(),tracepeph() +* 2009/12/22 1.12 change default parameter of ionos std +* valid under second for timeget() +* 2010/07/28 1.13 fix bug in tropmapf() +* added api: +* obs2code(),code2obs(),cross3(),normv3(), +* gst2time(),time2gst(),time_str(),timeset(), +* deg2dms(),dms2deg(),searchpcv(),antmodel_s(), +* tracehnav(),tracepclk(),reppath(),reppaths(), +* createdir() +* changed api: +* readpcv(), +* deleted api: +* uniqeph() +* 2010/08/20 1.14 omit to include mkl header files +* fix bug on chi-sqr(n) table +* 2010/12/11 1.15 added api: +* freeobs(),freenav(),ionppp() +* 2011/05/28 1.16 fix bug on half-hour offset by time2epoch() +* added api: +* uniqnav() +* 2012/06/09 1.17 add a leap second after 2012-6-30 +* 2012/07/15 1.18 add api setbits(),setbitu(),utc2gmst() +* fix bug on interpolation of antenna pcv +* fix bug on str2num() for string with over 256 char +* add api readblq(),satexclude(),setcodepri(), +* getcodepri() +* change api obs2code(),code2obs(),antmodel() +* 2012/12/25 1.19 fix bug on satwavelen(),code2obs(),obs2code() +* add api testsnr() +* 2013/01/04 1.20 add api gpst2bdt(),bdt2gpst(),bdt2time(),time2bdt() +* readblq(),readerp(),geterp(),crc16() +* change api eci2ecef(),sunmoonpos() +* 2013/03/26 1.21 tickget() uses clock_gettime() for linux +* 2013/05/08 1.22 fix bug on nutation coefficients for ast_args() +* 2013/06/02 1.23 add #ifdef for undefined CLOCK_MONOTONIC_RAW +* 2013/09/01 1.24 fix bug on interpolation of satellite antenna pcv +* 2013/09/06 1.25 fix bug on extrapolation of erp +* 2014/04/27 1.26 add SYS_LEO for satellite system +* add BDS L1 code for RINEX 3.02 and RTCM 3.2 +* support BDS L1 in satwavelen() +* 2014/05/29 1.27 fix bug on obs2code() to search obs code table +* 2014/08/26 1.28 fix problem on output of uncompress() for tar file +* add function to swap trace file with keywords +* 2014/10/21 1.29 strtok() -> strtok_r() in expath() for thread-safe +* add bdsmodear in procopt_default +* 2015/03/19 1.30 fix bug on interpolation of erp values in geterp() +* add leap second insertion before 2015/07/01 00:00 +* add api read_leaps() +* 2015/05/31 1.31 delte api windupcorr() +* 2015/08/08 1.32 add compile option CPUTIME_IN_GPST +* add api add_fatal() +* support usno leapsec.dat for api read_leaps() +* 2016/01/23 1.33 enable septentrio +* 2016/02/05 1.34 support GLONASS for savenav(), loadnav() +* 2016/06/11 1.35 delete trace() in reppath() to avoid deadlock +* 2016/07/01 1.36 support IRNSS +* add leap second before 2017/1/1 00:00:00 +* 2016/07/29 1.37 rename api compress() -> rtk_uncompress() +* rename api crc16() -> rtk_crc16() +* rename api crc24q() -> rtk_crc24q() +* rename api crc32() -> rtk_crc32() +* 2016/08/20 1.38 fix type incompatibility in win64 environment +* change constant _POSIX_C_SOURCE 199309 -> 199506 +* 2016/08/21 1.39 fix bug on week overflow in time2gpst()/gpst2time() +* 2016/09/05 1.40 fix bug on invalid nav data read in readnav() +* 2016/09/17 1.41 suppress warnings +* 2016/09/19 1.42 modify api deg2dms() to consider numerical error +*-----------------------------------------------------------------------------*/ +#ifndef RTKLIB_RTKCMN_H_ +#define RTKLIB_RTKCMN_H_ + +#include "rtklib.h" + + +void fatalerr(const char *format, ...); +int satno(int sys, int prn); +int satsys(int sat, int *prn); +int satid2no(const char *id); +void satno2id(int sat, char *id); +int satexclude(int sat, int svh, const prcopt_t *opt); +int testsnr(int base, int freq, double el, double snr,const snrmask_t *mask); +unsigned char obs2code(const char *obs, int *freq); +char *code2obs(unsigned char code, int *freq); +void setcodepri(int sys, int freq, const char *pri); +int getcodepri(int sys, unsigned char code, const char *opt); +unsigned int getbitu(const unsigned char *buff, int pos, int len); +int getbits(const unsigned char *buff, int pos, int len); +void setbitu(unsigned char *buff, int pos, int len, unsigned int data); +void setbits(unsigned char *buff, int pos, int len, int data); +unsigned int rtk_crc32(const unsigned char *buff, int len); +unsigned int rtk_crc24q(const unsigned char *buff, int len); +unsigned short rtk_crc16(const unsigned char *buff, int len); +int decode_word(unsigned int word, unsigned char *data); +double *mat(int n, int m); +int *imat(int n, int m); +double *zeros(int n, int m); +double *eye(int n); +double dot(const double *a, const double *b, int n); +double norm(const double *a, int n); +void cross3(const double *a, const double *b, double *c); +int normv3(const double *a, double *b); +void matcpy(double *A, const double *B, int n, int m); +void matmul(const char *tr, int n, int k, int m, double alpha, + const double *A, const double *B, double beta, double *C); +int matinv(double *A, int n); +int solve(const char *tr, const double *A, const double *Y, int n, + int m, double *X); +int lsq(const double *A, const double *y, int n, int m, double *x, + double *Q); +int filter_(const double *x, const double *P, const double *H, + const double *v, const double *R, int n, int m, + double *xp, double *Pp); +int filter(double *x, double *P, const double *H, const double *v, + const double *R, int n, int m); +int smoother(const double *xf, const double *Qf, const double *xb, + const double *Qb, int n, double *xs, double *Qs); +void matfprint(const double A[], int n, int m, int p, int q, FILE *fp); +void matprint(const double A[], int n, int m, int p, int q); +double str2num(const char *s, int i, int n); +int str2time(const char *s, int i, int n, gtime_t *t); +gtime_t epoch2time(const double *ep); +void time2epoch(gtime_t t, double *ep); +gtime_t gpst2time(int week, double sec); +double time2gpst(gtime_t t, int *week); +gtime_t gst2time(int week, double sec); +double time2gst(gtime_t t, int *week); +gtime_t bdt2time(int week, double sec); +double time2bdt(gtime_t t, int *week); +gtime_t timeadd(gtime_t t, double sec); +double timediff(gtime_t t1, gtime_t t2); +gtime_t timeget(void); +//void timeset(gtime_t t); +int read_leaps_text(FILE *fp); +int read_leaps_usno(FILE *fp); +int read_leaps(const char *file); +gtime_t gpst2utc(gtime_t t); +gtime_t utc2gpst(gtime_t t); +gtime_t gpst2bdt(gtime_t t); +gtime_t bdt2gpst(gtime_t t); +double time2sec(gtime_t time, gtime_t *day); +double utc2gmst(gtime_t t, double ut1_utc); +void time2str(gtime_t t, char *s, int n); +char *time_str(gtime_t t, int n); +double time2doy(gtime_t t); +int adjgpsweek(int week); +unsigned int tickget(void); +void sleepms(int ms); +void deg2dms(double deg, double *dms, int ndec); +double dms2deg(const double *dms); +void ecef2pos(const double *r, double *pos); +void pos2ecef(const double *pos, double *r); +void xyz2enu(const double *pos, double *E); +void ecef2enu(const double *pos, const double *r, double *e); +void enu2ecef(const double *pos, const double *e, double *r); +void covenu(const double *pos, const double *P, double *Q); +void covecef(const double *pos, const double *Q, double *P); + +/* coordinate rotation matrix ------------------------------------------------*/ +#define Rx(t,X) do { \ + (X)[0]=1.0; (X)[1]=(X)[2]=(X)[3]=(X)[6]=0.0; \ + (X)[4]=(X)[8]=cos(t); (X)[7]=sin(t); (X)[5]=-(X)[7]; \ +} while (0) + +#define Ry(t,X) do { \ + (X)[4]=1.0; (X)[1]=(X)[3]=(X)[5]=(X)[7]=0.0; \ + (X)[0]=(X)[8]=cos(t); (X)[2]=sin(t); (X)[6]=-(X)[2]; \ +} while (0) + +#define Rz(t,X) do { \ + (X)[8]=1.0; (X)[2]=(X)[5]=(X)[6]=(X)[7]=0.0; \ + (X)[0]=(X)[4]=cos(t); (X)[3]=sin(t); (X)[1]=-(X)[3]; \ +} while (0) + +void ast_args(double t, double *f); +void nut_iau1980(double t, const double *f, double *dpsi, double *deps); +void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst); +int decodef(char *p, int n, double *v); +void addpcv(const pcv_t *pcv, pcvs_t *pcvs); +int readngspcv(const char *file, pcvs_t *pcvs); +int readantex(const char *file, pcvs_t *pcvs); +int readpcv(const char *file, pcvs_t *pcvs); +pcv_t *searchpcv(int sat, const char *type, gtime_t time, + const pcvs_t *pcvs); +void readpos(const char *file, const char *rcv, double *pos); +int readblqrecord(FILE *fp, double *odisp); +int readblq(const char *file, const char *sta, double *odisp); +int readerp(const char *file, erp_t *erp); +int geterp(const erp_t *erp, gtime_t time, double *erpv); +int cmpeph(const void *p1, const void *p2); +void uniqeph(nav_t *nav); +int cmpgeph(const void *p1, const void *p2); +void uniqgeph(nav_t *nav); +int cmpseph(const void *p1, const void *p2); +void uniqseph(nav_t *nav); +void uniqnav(nav_t *nav); +int cmpobs(const void *p1, const void *p2); +int sortobs(obs_t *obs); +int screent(gtime_t time, gtime_t ts, gtime_t te, double tint); +int readnav(const char *file, nav_t *nav); +int savenav(const char *file, const nav_t *nav); +void freeobs(obs_t *obs); +void freenav(nav_t *nav, int opt); + +void traceopen(const char *file); +void traceclose(void); +void tracelevel(int level); +void trace (int level, const char *format, ...); +void tracet (int level, const char *format, ...); +void tracemat(int level, const double *A, int n, int m, int p, int q); +void traceobs(int level, const obsd_t *obs, int n); +void tracenav(int level, const nav_t *nav); +void tracegnav(int level, const nav_t *nav); +void tracehnav(int level, const nav_t *nav); +void tracepeph(int level, const nav_t *nav); +void tracepclk(int level, const nav_t *nav); +void traceb (int level, const unsigned char *p, int n); + +int execcmd(const char *cmd); +void createdir(const char *path); +int repstr(char *str, const char *pat, const char *rep); +int reppath(const char *path, char *rpath, gtime_t time, const char *rov, + const char *base); +int reppaths(const char *path, char *rpath[], int nmax, gtime_t ts, + gtime_t te, const char *rov, const char *base); +double satwavelen(int sat, int frq, const nav_t *nav); +double geodist(const double *rs, const double *rr, double *e); +double satazel(const double *pos, const double *e, double *azel); + +#define SQRT(x) ((x)<0.0?0.0:sqrt(x)); +void dops(int ns, const double *azel, double elmin, double *dop); +double ionmodel(gtime_t t, const double *ion, const double *pos, + const double *azel); +double ionmapf(const double *pos, const double *azel); +double ionppp(const double *pos, const double *azel, double re, + double hion, double *posp); +double tropmodel(gtime_t time, const double *pos, const double *azel, + double humi); +double interpc(const double coef[], double lat); +double mapf(double el, double a, double b, double c); +double nmf(gtime_t time, const double pos[], const double azel[], + double *mapfw); +double tropmapf(gtime_t time, const double pos[], const double azel[], + double *mapfw); +double interpvar(double ang, const double *var); + +void antmodel(const pcv_t *pcv, const double *del, const double *azel, + int opt, double *dant); + +void antmodel_s(const pcv_t *pcv, double nadir, double *dant); +void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon); +void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, + double *rmoon, double *gmst); +void csmooth(obs_t *obs, int ns); +int rtk_uncompress(const char *file, char *uncfile); +int expath(const char *path, char *paths[], int nmax); + +#endif /* RTKLIB_RTKCMN_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.cc b/src/algorithms/libs/rtklib/rtklib_sbas.cc new file mode 100644 index 000000000..623bd9cf0 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_sbas.cc @@ -0,0 +1,910 @@ +/*! + * \file rtklib_sbas.cc + * \brief sbas functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +* +* option : -DRRCENA enable rrc correction +* +* references : +* [1] RTCA/DO-229C, Minimum operational performanc standards for global +* positioning system/wide area augmentation system airborne equipment, +* RTCA inc, November 28, 2001 +* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service +* Interface Specification for QZSS, Japan Aerospace Exploration Agency, +* July 31, 2009 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2007/10/14 1.0 new +* 2009/01/24 1.1 modify sbspntpos() api +* improve fast/ion correction update +* 2009/04/08 1.2 move function crc24q() to rcvlog.c +* support glonass, galileo and qzss +* 2009/06/08 1.3 modify sbsupdatestat() +* delete sbssatpos() +* 2009/12/12 1.4 support glonass +* 2010/01/22 1.5 support ems (egnos message service) format +* 2010/06/10 1.6 added api: +* sbssatcorr(),sbstropcorr(),sbsioncorr(), +* sbsupdatecorr() +* changed api: +* sbsreadmsgt(),sbsreadmsg() +* deleted api: +* sbspntpos(),sbsupdatestat() +* 2010/08/16 1.7 not reject udre==14 or give==15 correction message +* (2.4.0_p4) +* 2011/01/15 1.8 use api ionppp() +* add prn mask of qzss for qzss L1SAIF +* 2016/07/29 1.9 crc24q() -> rtk_crc24q() +*-----------------------------------------------------------------------------*/ +#include "rtklib_sbas.h" + + +/* extract field from line ---------------------------------------------------*/ +char *getfield(char *p, int pos) +{ + for (pos--;pos>0;pos--,p++) if (!(p=strchr(p,','))) return NULL; + return p; +} +/* variance of fast correction (udre=UDRE+1) ---------------------------------*/ +double varfcorr(int udre) +{ + const double var[14]={ + 0.052,0.0924,0.1444,0.283,0.4678,0.8315,1.2992,1.8709,2.5465,3.326, + 5.1968,20.7870,230.9661,2078.695 + }; + return 0msg,13+i,1)) { + if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */ + else if (i<= 61) sat=satno(SYS_GLO,i-37); /* 38- 61: glonass */ + else if (i<=119) sat=0; /* 62-119: future gnss */ + else if (i<=138) sat=satno(SYS_SBS,i); /* 120-138: geo/waas */ + else if (i<=182) sat=0; /* 139-182: reserved */ + else if (i<=192) sat=satno(SYS_SBS,i+10); /* 183-192: qzss ref [2] */ + else if (i<=202) sat=satno(SYS_QZS,i); /* 193-202: qzss ref [2] */ + else sat=0; /* 203- : reserved */ + sbssat->sat[n++].sat=sat; + } + } + sbssat->iodp=getbitu(msg->msg,224,2); + sbssat->nsat=n; + + trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp); + return 1; +} +/* decode type 2-5,0: fast corrections ---------------------------------------*/ +int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i,j,iodf,type,udre; + double prc,dt; + gtime_t t0; + + trace(4,"decode_sbstype2:\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)) return 0; + + type=getbitu(msg->msg, 8,6); + iodf=getbitu(msg->msg,14,2); + + for (i=0;i<13;i++) { + if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break; + udre=getbitu(msg->msg,174+4*i,4); + t0 =sbssat->sat[j].fcorr.t0; + prc=sbssat->sat[j].fcorr.prc; + sbssat->sat[j].fcorr.t0=gpst2time(msg->week,msg->tow); + sbssat->sat[j].fcorr.prc=getbits(msg->msg,18+i*12,12)*0.125f; + sbssat->sat[j].fcorr.udre=udre+1; + dt=timediff(sbssat->sat[j].fcorr.t0,t0); + if (t0.time==0||dt<=0.0||18.0sat[j].fcorr.ai==0) { + sbssat->sat[j].fcorr.rrc=0.0; + sbssat->sat[j].fcorr.dt=0.0; + } + else { + sbssat->sat[j].fcorr.rrc=(sbssat->sat[j].fcorr.prc-prc)/dt; + sbssat->sat[j].fcorr.dt=dt; + } + sbssat->sat[j].fcorr.iodf=iodf; + } + trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf); + return 1; +} +/* decode type 6: integrity info ---------------------------------------------*/ +int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i,iodf[4],udre; + + trace(4,"decode_sbstype6:\n"); + + for (i=0;i<4;i++) { + iodf[i]=getbitu(msg->msg,14+i*2,2); + } + for (i=0;insat&&isat[i].fcorr.iodf!=iodf[i/13]) continue; + udre=getbitu(msg->msg,22+i*4,4); + sbssat->sat[i].fcorr.udre=udre+1; + } + trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]); + return 1; +} +/* decode type 7: fast correction degradation factor -------------------------*/ +int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i; + + trace(4,"decode_sbstype7\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)) return 0; + + sbssat->tlat=getbitu(msg->msg,14,4); + + for (i=0;insat&&isat[i].fcorr.ai=getbitu(msg->msg,22+i*4,4); + } + return 1; +} +/* decode type 9: geo navigation message -------------------------------------*/ +int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) +{ + seph_t seph={}; + int i,sat,t; + + trace(4,"decode_sbstype9:\n"); + + if (!(sat=satno(SYS_SBS,msg->prn))) { + trace(2,"invalid prn in sbas type 9: prn=%3d\n",msg->prn); + return 0; + } + t=(int)getbitu(msg->msg,22,13)*16-(int)msg->tow%86400; + if (t<=-43200) t+=86400; + else if (t> 43200) t-=86400; + seph.sat=sat; + seph.t0 =gpst2time(msg->week,msg->tow+t); + seph.tof=gpst2time(msg->week,msg->tow); + seph.sva=getbitu(msg->msg,35,4); + seph.svh=seph.sva==15?1:0; /* unhealthy if ura==15 */ + + seph.pos[0]=getbits(msg->msg, 39,30)*0.08; + seph.pos[1]=getbits(msg->msg, 69,30)*0.08; + seph.pos[2]=getbits(msg->msg, 99,25)*0.4; + seph.vel[0]=getbits(msg->msg,124,17)*0.000625; + seph.vel[1]=getbits(msg->msg,141,17)*0.000625; + seph.vel[2]=getbits(msg->msg,158,18)*0.004; + seph.acc[0]=getbits(msg->msg,176,10)*0.0000125; + seph.acc[1]=getbits(msg->msg,186,10)*0.0000125; + seph.acc[2]=getbits(msg->msg,196,10)*0.0000625; + + seph.af0=getbits(msg->msg,206,12)*P2_31; + seph.af1=getbits(msg->msg,218, 8)*P2_39/2.0; + + i=msg->prn-MINPRNSBS; + if (!nav->seph||fabs(timediff(nav->seph[i].t0,seph.t0))<1E-3) { /* not change */ + return 0; + } + nav->seph[NSATSBS+i]=nav->seph[i]; /* previous */ + nav->seph[i]=seph; /* current */ + + trace(5,"decode_sbstype9: prn=%d\n",msg->prn); + return 1; +} +/* decode type 18: ionospheric grid point masks ------------------------------*/ +int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion) +{ + const sbsigpband_t *p; + int i,j,n,m,band=getbitu(msg->msg,18,4); + + trace(4,"decode_sbstype18:\n"); + + if (0<=band&&band<= 8) {p=igpband1[band ]; m=8;} + else if (9<=band&&band<=10) {p=igpband2[band-9]; m=5;} + else return 0; + + sbsion[band].iodi=(short)getbitu(msg->msg,22,2); + + for (i=1,n=0;i<=201;i++) { + if (!getbitu(msg->msg,23+i,1)) continue; + for (j=0;jmsg,p,6); + + trace(4,"decode_longcorr0:\n"); + + if (n==0||n>MAXSAT) return 0; + + sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); + + for (i=0;i<3;i++) { + sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+9*i,9)*0.125; + sbssat->sat[n-1].lcorr.dvel[i]=0.0; + } + sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+41,10)*P2_31; + sbssat->sat[n-1].lcorr.daf1=0.0; + sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow); + + trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat); + return 1; +} +/* decode half long term correction (vel code=1) -----------------------------*/ +int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat) +{ + int i,n=getbitu(msg->msg,p,6),t; + + trace(4,"decode_longcorr1:\n"); + + if (n==0||n>MAXSAT) return 0; + + sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); + + for (i=0;i<3;i++) { + sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+i*11,11)*0.125; + sbssat->sat[n-1].lcorr.dvel[i]=getbits(msg->msg,p+58+i* 8, 8)*P2_11; + } + sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+47,11)*P2_31; + sbssat->sat[n-1].lcorr.daf1=getbits(msg->msg,p+82, 8)*P2_39; + t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400; + if (t<=-43200) t+=86400; + else if (t> 43200) t-=86400; + sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t); + + trace(5,"decode_longcorr1: sat=%2d\n",sbssat->sat[n-1].sat); + return 1; +} +/* decode half long term correction ------------------------------------------*/ +int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat) +{ + trace(4,"decode_longcorrh:\n"); + + if (getbitu(msg->msg,p,1)==0) { /* vel code=0 */ + if (sbssat->iodp==(int)getbitu(msg->msg,p+103,2)) { + return decode_longcorr0(msg,p+ 1,sbssat)&& + decode_longcorr0(msg,p+52,sbssat); + } + } + else if (sbssat->iodp==(int)getbitu(msg->msg,p+104,2)) { + return decode_longcorr1(msg,p+1,sbssat); + } + return 0; +} +/* decode type 24: mixed fast/long term correction ---------------------------*/ +int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i,j,iodf,blk,udre; + + trace(4,"decode_sbstype24:\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg,110,2)) return 0; /* check IODP */ + + blk =getbitu(msg->msg,112,2); + iodf=getbitu(msg->msg,114,2); + + for (i=0;i<6;i++) { + if ((j=13*blk+i)>=sbssat->nsat) break; + udre=getbitu(msg->msg,86+4*i,4); + + sbssat->sat[j].fcorr.t0 =gpst2time(msg->week,msg->tow); + sbssat->sat[j].fcorr.prc =getbits(msg->msg,14+i*12,12)*0.125f; + sbssat->sat[j].fcorr.udre=udre+1; + sbssat->sat[j].fcorr.iodf=iodf; + } + return decode_longcorrh(msg,120,sbssat); +} +/* decode type 25: long term satellite error correction ----------------------*/ +int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + trace(4,"decode_sbstype25:\n"); + + return decode_longcorrh(msg,14,sbssat)&&decode_longcorrh(msg,120,sbssat); +} +/* decode type 26: ionospheric deley corrections -----------------------------*/ +int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion) +{ + int i,j,block,delay,give,band=getbitu(msg->msg,14,4); + + trace(4,"decode_sbstype26:\n"); + + if (band>MAXBAND||sbsion[band].iodi!=(int)getbitu(msg->msg,217,2)) return 0; + + block=getbitu(msg->msg,18,4); + + for (i=0;i<15;i++) { + if ((j=block*15+i)>=sbsion[band].nigp) continue; + give=getbitu(msg->msg,22+i*13+9,4); + + delay=getbitu(msg->msg,22+i*13,9); + sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow); + sbsion[band].igp[j].delay=delay==0x1FF?0.0f:delay*0.125f; + sbsion[band].igp[j].give=give+1; + + if (sbsion[band].igp[j].give>=16) { + sbsion[band].igp[j].give=0; + } + } + trace(5,"decode_sbstype26: band=%d block=%d\n",band,block); + return 1; +} +/* update sbas corrections ----------------------------------------------------- +* update sbas correction parameters in navigation data with a sbas message +* args : sbsmg_t *msg I sbas message +* nav_t *nav IO navigation data +* return : message type (-1: error or not supported type) +* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) +* seph[prn-MINPRNSBS+1] : sat prn current epehmeris +* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris +*-----------------------------------------------------------------------------*/ +int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav) +{ + int type=getbitu(msg->msg,8,6),stat=-1; + + trace(3,"sbsupdatecorr: type=%d\n",type); + + if (msg->week==0) return -1; + + switch (type) { + case 0: stat=decode_sbstype2 (msg,&nav->sbssat); break; + case 1: stat=decode_sbstype1 (msg,&nav->sbssat); break; + case 2: + case 3: + case 4: + case 5: stat=decode_sbstype2 (msg,&nav->sbssat); break; + case 6: stat=decode_sbstype6 (msg,&nav->sbssat); break; + case 7: stat=decode_sbstype7 (msg,&nav->sbssat); break; + case 9: stat=decode_sbstype9 (msg,nav); break; + case 18: stat=decode_sbstype18(msg,nav ->sbsion); break; + case 24: stat=decode_sbstype24(msg,&nav->sbssat); break; + case 25: stat=decode_sbstype25(msg,&nav->sbssat); break; + case 26: stat=decode_sbstype26(msg,nav ->sbsion); break; + case 63: break; /* null message */ + + /*default: trace(2,"unsupported sbas message: type=%d\n",type); break;*/ + } + return stat?type:-1; +} +/* read sbas log file --------------------------------------------------------*/ +void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te, + sbs_t *sbs) +{ + sbsmsg_t *sbs_msgs; + int i,week,prn,ch,msg; + unsigned int b; + double tow,ep[6]={}; + char buff[256],*p; + gtime_t time; + FILE *fp; + + trace(3,"readmsgs: file=%s sel=%d\n",file,sel); + + if (!(fp=fopen(file,"r"))) { + trace(2,"sbas message file open error: %s\n",file); + return; + } + while (fgets(buff,sizeof(buff),fp)) { + if (sscanf(buff,"%d %lf %d",&week,&tow,&prn)==3&&(p=strstr(buff,": "))) { + p+=2; /* rtklib form */ + } + else if (sscanf(buff,"%d %lf %lf %lf %lf %lf %lf %d", + &prn,ep,ep+1,ep+2,ep+3,ep+4,ep+5,&msg)==8) { + /* ems (EGNOS Message Service) form */ + ep[0]+=ep[0]<70.0?2000.0:1900.0; + tow=time2gpst(epoch2time(ep),&week); + p=buff+(msg>=10?25:24); + } + else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) { /* NovAtel OEM4/V */ + if (!(p=getfield(buff,6))) continue; + if (sscanf(p,"%d,%lf",&week,&tow)<2) continue; + if (!(p=strchr(++p,';'))) continue; + if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue; + if (!(p=getfield(p,4))) continue; + } + else if (!strncmp(buff,"$FRMA",5)) { /* NovAtel OEM3 */ + if (!(p=getfield(buff,2))) continue; + if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue; + if (!(p=getfield(p,6))) continue; + if (weekn>=sbs->nmax) { + sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2; + if (!(sbs_msgs=(sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) { + trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax); + free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0; + return; + } + sbs->msgs=sbs_msgs; + } + sbs->msgs[sbs->n].week=week; + sbs->msgs[sbs->n].tow=(int)(tow+0.5); + sbs->msgs[sbs->n].prn=prn; + for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0; + for (i=0;*(p-1)&&*p&&i<29;p+=2,i++) { + if (sscanf(p,"%2X",&b)==1) sbs->msgs[sbs->n].msg[i]=(unsigned char)b; + } + sbs->msgs[sbs->n++].msg[28]&=0xC0; + } + fclose(fp); +} +/* compare sbas messages -----------------------------------------------------*/ +int cmpmsgs(const void *p1, const void *p2) +{ + sbsmsg_t *q1=(sbsmsg_t *)p1,*q2=(sbsmsg_t *)p2; + return q1->week!=q2->week?q1->week-q2->week: + (q1->towtow?-1:(q1->tow>q2->tow?1:q1->prn-q2->prn)); +} +/* read sbas message file ------------------------------------------------------ +* read sbas message file +* args : char *file I sbas message file (wind-card * is expanded) +* int sel I sbas satellite prn number selection (0:all) +* (gtime_t ts I start time) +* (gtime_t te I end time ) +* sbs_t *sbs IO sbas messages +* return : number of sbas messages +* notes : sbas message are appended and sorted. before calling the funciton, +* sbs->n, sbs->nmax and sbs->msgs must be set properly. (initially +* sbs->n=sbs->nmax=0, sbs->msgs=NULL) +* only the following file extentions after wild card expanded are valid +* to read. others are skipped +* .sbs, .SBS, .ems, .EMS +*-----------------------------------------------------------------------------*/ +int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te, + sbs_t *sbs) +{ + char *efiles[MAXEXFILE]={},*ext; + int i,n; + + trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel); + + for (i=0;i=0;i--) free(efiles[i]); + return 0; + } + } + /* expand wild card in file path */ + n=expath(file,efiles,MAXEXFILE); + + for (i=0;in>0) { + qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs); + } + return sbs->n; +} +int sbsreadmsg(const char *file, int sel, sbs_t *sbs) +{ + gtime_t ts={},te={}; + + trace(3,"sbsreadmsg: file=%s sel=%d\n",file,sel); + + return sbsreadmsgt(file,sel,ts,te,sbs); +} +/* output sbas messages -------------------------------------------------------- +* output sbas message record to output file in rtklib sbas log format +* args : FILE *fp I output file pointer +* sbsmsg_t *sbsmsg I sbas messages +* return : none +*-----------------------------------------------------------------------------*/ +void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg) +{ + int i,type=sbsmsg->msg[1]>>2; + + trace(4,"sbsoutmsg:\n"); + + fprintf(fp,"%4d %6d %3d %2d : ",sbsmsg->week,sbsmsg->tow,sbsmsg->prn,type); + for (i=0;i<29;i++) fprintf(fp,"%02X",sbsmsg->msg[i]); + fprintf(fp,"\n"); +} +/* search igps ---------------------------------------------------------------*/ +void searchigp(gtime_t time, const double *pos, const sbsion_t *ion, + const sbsigp_t **igp, double *x, double *y) +{ + int i,latp[2],lonp[4]; + double lat=pos[0]*R2D,lon=pos[1]*R2D; + const sbsigp_t *p; + + trace(4,"searchigp: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); + + if (lon>=180.0) lon-=360.0; + if (-55.0<=lat&&lat<55.0) { + latp[0]=(int)floor(lat/5.0)*5; + latp[1]=latp[0]+5; + lonp[0]=lonp[1]=(int)floor(lon/5.0)*5; + lonp[2]=lonp[3]=lonp[0]+5; + *x=(lon-lonp[0])/5.0; + *y=(lat-latp[0])/5.0; + } + else { + latp[0]=(int)floor((lat-5.0)/10.0)*10+5; + latp[1]=latp[0]+10; + lonp[0]=lonp[1]=(int)floor(lon/10.0)*10; + lonp[2]=lonp[3]=lonp[0]+10; + *x=(lon-lonp[0])/10.0; + *y=(lat-latp[0])/10.0; + if (75.0<=lat&&lat<85.0) { + lonp[1]=(int)floor(lon/90.0)*90; + lonp[3]=lonp[1]+90; + } + else if (-85.0<=lat&&lat<-75.0) { + lonp[0]=(int)floor((lon-50.0)/90.0)*90+40; + lonp[2]=lonp[0]+90; + } + else if (lat>=85.0) { + for (i=0;i<4;i++) lonp[i]=(int)floor(lon/90.0)*90; + } + else if (lat<-85.0) { + for (i=0;i<4;i++) lonp[i]=(int)floor((lon-50.0)/90.0)*90+40; + } + } + for (i=0;i<4;i++) if (lonp[i]==180) lonp[i]=-180; + for (i=0;i<=MAXBAND;i++) { + for (p=ion[i].igp;pt0.time==0) continue; + if (p->lat==latp[0]&&p->lon==lonp[0]&&p->give>0) igp[0]=p; + else if (p->lat==latp[1]&&p->lon==lonp[1]&&p->give>0) igp[1]=p; + else if (p->lat==latp[0]&&p->lon==lonp[2]&&p->give>0) igp[2]=p; + else if (p->lat==latp[1]&&p->lon==lonp[3]&&p->give>0) igp[3]=p; + if (igp[0]&&igp[1]&&igp[2]&&igp[3]) return; + } + } +} +/* sbas ionospheric delay correction ------------------------------------------- +* compute sbas ionosphric delay correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* double *pos I receiver position {lat,lon,height} (rad/m) +* double *azel I satellite azimuth/elavation angle (rad) +* double *delay O slant ionospheric delay (L1) (m) +* double *var O variance of ionospheric delay (m^2) +* return : status (1:ok, 0:no correction) +* notes : before calling the function, sbas ionosphere correction parameters +* in navigation data (nav->sbsion) must be set by callig +* sbsupdatecorr() +*-----------------------------------------------------------------------------*/ +int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, double *delay, double *var) +{ + const double re=6378.1363,hion=350.0; + int i,err=0; + double fp,posp[2],x=0.0,y=0.0,t,w[4]={}; + const sbsigp_t *igp[4]={}; /* {ws,wn,es,en} */ + + trace(4,"sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, + azel[0]*R2D,azel[1]*R2D); + + *delay=*var=0.0; + if (pos[2]<-100.0||azel[1]<=0) return 1; + + /* ipp (ionospheric pierce point) position */ + fp=ionppp(pos,azel,re,hion,posp); + + /* search igps around ipp */ + searchigp(time,posp,nav->sbsion,igp,&x,&y); + + /* weight of igps */ + if (igp[0]&&igp[1]&&igp[2]&&igp[3]) { + w[0]=(1.0-x)*(1.0-y); w[1]=(1.0-x)*y; w[2]=x*(1.0-y); w[3]=x*y; + } + else if (igp[0]&&igp[1]&&igp[2]) { + w[1]=y; w[2]=x; + if ((w[0]=1.0-w[1]-w[2])<0.0) err=1; + } + else if (igp[0]&&igp[2]&&igp[3]) { + w[0]=1.0-x; w[3]=y; + if ((w[2]=1.0-w[0]-w[3])<0.0) err=1; + } + else if (igp[0]&&igp[1]&&igp[3]) { + w[0]=1.0-y; w[3]=x; + if ((w[1]=1.0-w[0]-w[3])<0.0) err=1; + } + else if (igp[1]&&igp[2]&&igp[3]) { + w[1]=1.0-x; w[2]=1.0-y; + if ((w[3]=1.0-w[1]-w[2])<0.0) err=1; + } + else err=1; + + if (err) { + trace(2,"no sbas iono correction: lat=%3.0f lon=%4.0f\n",posp[0]*R2D, + posp[1]*R2D); + return 0; + } + for (i=0;i<4;i++) { + if (!igp[i]) continue; + t=timediff(time,igp[i]->t0); + *delay+=w[i]*igp[i]->delay; + *var+=w[i]*varicorr(igp[i]->give)*9E-8*fabs(t); + } + *delay*=fp; *var*=fp*fp; + + trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var)); + return 1; +} +/* get meterological parameters ----------------------------------------------*/ +void getmet(double lat, double *met) +{ + static const double metprm[][10]={ /* lat=15,30,45,60,75 */ + {1013.25,299.65,26.31,6.30E-3,2.77, 0.00, 0.00,0.00,0.00E-3,0.00}, + {1017.25,294.15,21.79,6.05E-3,3.15, -3.75, 7.00,8.85,0.25E-3,0.33}, + {1015.75,283.15,11.66,5.58E-3,2.57, -2.25,11.00,7.24,0.32E-3,0.46}, + {1011.75,272.15, 6.78,5.39E-3,1.81, -1.75,15.00,5.36,0.81E-3,0.74}, + {1013.00,263.65, 4.11,4.53E-3,1.55, -0.50,14.50,3.39,0.62E-3,0.30} + }; + int i,j; + double a; + lat=fabs(lat); + if (lat<=15.0) for (i=0;i<10;i++) met[i]=metprm[0][i]; + else if (lat>=75.0) for (i=0;i<10;i++) met[i]=metprm[4][i]; + else { + j=(int)(lat/15.0); a=(lat-j*15.0)/15.0; + for (i=0;i<10;i++) met[i]=(1.0-a)*metprm[j-1][i]+a*metprm[j][i]; + } +} +/* tropospheric delay correction ----------------------------------------------- +* compute sbas tropospheric delay correction (mops model) +* args : gtime_t time I time +* double *pos I receiver position {lat,lon,height} (rad/m) +* double *azel I satellite azimuth/elavation (rad) +* double *var O variance of troposphric error (m^2) +* return : slant tropospheric delay (m) +*-----------------------------------------------------------------------------*/ +double sbstropcorr(gtime_t time, const double *pos, const double *azel, + double *var) +{ + const double k1=77.604,k2=382000.0,rd=287.054,gm=9.784,g=9.80665; + static double pos_[3]={},zh=0.0,zw=0.0; + int i; + double c,met[10],sinel=sin(azel[1]),h=pos[2],m; + + trace(4,"sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, + azel[0]*R2D,azel[1]*R2D); + + if (pos[2]<-100.0||10000.01E-7||fabs(pos[1]-pos_[1])>1E-7|| + fabs(pos[2]-pos_[2])>1.0) { + getmet(pos[0]*R2D,met); + c=cos(2.0*PI*(time2doy(time)-(pos[0]>=0.0?28.0:211.0))/365.25); + for (i=0;i<5;i++) met[i]-=met[i+5]*c; + zh=1E-6*k1*rd*met[0]/gm; + zw=1E-6*k2*rd/(gm*(met[4]+1.0)-met[3]*rd)*met[2]/met[1]; + zh*=pow(1.0-met[3]*h/met[1],g/(rd*met[3])); + zw*=pow(1.0-met[3]*h/met[1],(met[4]+1.0)*g/(rd*met[3])-1.0); + for (i=0;i<3;i++) pos_[i]=pos[i]; + } + m=1.001/sqrt(0.002001+sinel*sinel); + *var=0.12*0.12*m*m; + return (zh+zw)*m; +} +/* long term correction ------------------------------------------------------*/ +int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *drs, double *ddts) +{ + const sbssatp_t *p; + double t; + int i; + + trace(3,"sbslongcorr: sat=%2d\n",sat); + + for (p=sbssat->sat;psat+sbssat->nsat;p++) { + if (p->sat!=sat||p->lcorr.t0.time==0) continue; + t=timediff(time,p->lcorr.t0); + if (fabs(t)>MAXSBSAGEL) { + trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", + time_str(time,0),sat,t); + return 0; + } + for (i=0;i<3;i++) drs[i]=p->lcorr.dpos[i]+p->lcorr.dvel[i]*t; + *ddts=p->lcorr.daf0+p->lcorr.daf1*t; + + trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", + sat,drs[0],drs[1],drs[2],*ddts*CLIGHT); + + return 1; + } + /* if sbas satellite without correction, no correction applied */ + if (satsys(sat,NULL)==SYS_SBS) return 1; + + trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); + return 0; +} +/* fast correction -----------------------------------------------------------*/ +int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *prc, double *var) +{ + const sbssatp_t *p; + double t; + + trace(3,"sbsfastcorr: sat=%2d\n",sat); + + for (p=sbssat->sat;psat+sbssat->nsat;p++) { + if (p->sat!=sat) continue; + if (p->fcorr.t0.time==0) break; + t=timediff(time,p->fcorr.t0)+sbssat->tlat; + + /* expire age of correction or UDRE==14 (not monitored) */ + if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue; + *prc=p->fcorr.prc; +#ifdef RRCENA + if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) { + *prc+=p->fcorr.rrc*t; + } +#endif + *var=varfcorr(p->fcorr.udre)+degfcorr(p->fcorr.ai)*t*t/2.0; + + trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat, + *prc,sqrt(*var),t); + return 1; + } + trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat); + return 0; +} +/* sbas satellite ephemeris and clock correction ------------------------------- +* correct satellite position and clock bias with sbas satellite corrections +* args : gtime_t time I reception time +* int sat I satellite +* nav_t *nav I navigation data +* double *rs IO sat position and corrected {x,y,z} (ecef) (m) +* double *dts IO sat clock bias and corrected (s) +* double *var O sat position and clock variance (m^2) +* return : status (1:ok,0:no correction) +* notes : before calling the function, sbas satellite correction parameters +* in navigation data (nav->sbssat) must be set by callig +* sbsupdatecorr(). +* satellite clock correction include long-term correction and fast +* correction. +* sbas clock correction is usually based on L1C/A code. TGD or DCB has +* to be considered for other codes +*-----------------------------------------------------------------------------*/ +int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs, + double *dts, double *var) +{ + double drs[3]={},dclk=0.0,prc=0.0; + int i; + + trace(3,"sbssatcorr : sat=%2d\n",sat); + + /* sbas long term corrections */ + if (!sbslongcorr(time,sat,&nav->sbssat,drs,&dclk)) { + return 0; + } + /* sbas fast corrections */ + if (!sbsfastcorr(time,sat,&nav->sbssat,&prc,var)) { + return 0; + } + for (i=0;i<3;i++) rs[i]+=drs[i]; + + dts[0]+=dclk+prc/CLIGHT; + + trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n", + sat,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var); + + return 1; +} +/* decode sbas message --------------------------------------------------------- +* decode sbas message frame words and check crc +* args : gtime_t time I reception time +* int prn I sbas satellite prn number +* unsigned int *word I message frame words (24bit x 10) +* sbsmsg_t *sbsmsg O sbas message +* return : status (1:ok,0:crc error) +*-----------------------------------------------------------------------------*/ +int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, + sbsmsg_t *sbsmsg) +{ + int i,j; + unsigned char f[29]; + double tow; + + trace(5,"sbsdecodemsg: prn=%d\n",prn); + + if (time.time==0) return 0; + tow=time2gpst(time,&sbsmsg->week); + sbsmsg->tow=(int)(tow+DTTOL); + sbsmsg->prn=prn; + for (i=0;i<7;i++) for (j=0;j<4;j++) { + sbsmsg->msg[i*4+j]=(unsigned char)(words[i]>>((3-j)*8)); + } + sbsmsg->msg[28]=(unsigned char)(words[7]>>18)&0xC0; + for (i=28;i>0;i--) f[i]=(sbsmsg->msg[i]>>6)+(sbsmsg->msg[i-1]<<2); + f[0]=sbsmsg->msg[0]>>6; + + return rtk_crc24q(f,29)==(words[7]&0xFFFFFF); /* check crc */ +} diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.h b/src/algorithms/libs/rtklib/rtklib_sbas.h new file mode 100644 index 000000000..f3ec8ee51 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_sbas.h @@ -0,0 +1,187 @@ +/*! + * \file rtklib_sbas.h + * \brief sbas functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * 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, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * +* +* option : -DRRCENA enable rrc correction +* +* references : +* [1] RTCA/DO-229C, Minimum operational performanc standards for global +* positioning system/wide area augmentation system airborne equipment, +* RTCA inc, November 28, 2001 +* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service +* Interface Specification for QZSS, Japan Aerospace Exploration Agency, +* July 31, 2009 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2007/10/14 1.0 new +* 2009/01/24 1.1 modify sbspntpos() api +* improve fast/ion correction update +* 2009/04/08 1.2 move function crc24q() to rcvlog.c +* support glonass, galileo and qzss +* 2009/06/08 1.3 modify sbsupdatestat() +* delete sbssatpos() +* 2009/12/12 1.4 support glonass +* 2010/01/22 1.5 support ems (egnos message service) format +* 2010/06/10 1.6 added api: +* sbssatcorr(),sbstropcorr(),sbsioncorr(), +* sbsupdatecorr() +* changed api: +* sbsreadmsgt(),sbsreadmsg() +* deleted api: +* sbspntpos(),sbsupdatestat() +* 2010/08/16 1.7 not reject udre==14 or give==15 correction message +* (2.4.0_p4) +* 2011/01/15 1.8 use api ionppp() +* add prn mask of qzss for qzss L1SAIF +* 2016/07/29 1.9 crc24q() -> rtk_crc24q() +*-----------------------------------------------------------------------------*/ + +#ifndef RTKLIB_SBAS_H_ +#define RTKLIB_SBAS_H_ + +#include "rtklib.h" +#include "rtklib_rtkcmn.h" +/* constants -----------------------------------------------------------------*/ + +#define WEEKOFFSET 1024 /* gps week offset for NovAtel OEM-3 */ + +/* sbas igp definition -------------------------------------------------------*/ +static const short +x1[]={-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, + 25, 30, 35, 40, 45, 50, 55, 65, 75, 85}, +x2[]={-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, + 35, 40, 45, 50, 55}, +x3[]={-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, + 25, 30, 35, 40, 45, 50, 55, 65, 75}, +x4[]={-85,-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, + 20, 25, 30, 35, 40, 45, 50, 55, 65, 75}, +x5[]={-180,-175,-170,-165,-160,-155,-150,-145,-140,-135,-130,-125,-120,-115, + -110,-105,-100,- 95,- 90,- 85,- 80,- 75,- 70,- 65,- 60,- 55,- 50,- 45, + - 40,- 35,- 30,- 25,- 20,- 15,- 10,- 5, 0, 5, 10, 15, 20, 25, + 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, + 100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, + 170, 175}, +x6[]={-180,-170,-160,-150,-140,-130,-120,-110,-100,- 90,- 80,- 70,- 60,- 50, + - 40,- 30,- 20,- 10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, + 100, 110, 120, 130, 140, 150, 160, 170}, +x7[]={-180,-150,-120,- 90,- 60,- 30, 0, 30, 60, 90, 120, 150}, +x8[]={-170,-140,-110,- 80,- 50,- 20, 10, 40, 70, 100, 130, 160}; + +const sbsigpband_t igpband1[9][8]={ /* band 0-8 */ + {{-180,x1, 1, 28},{-175,x2, 29, 51},{-170,x3, 52, 78},{-165,x2, 79,101}, + {-160,x3,102,128},{-155,x2,129,151},{-150,x3,152,178},{-145,x2,179,201}}, + {{-140,x4, 1, 28},{-135,x2, 29, 51},{-130,x3, 52, 78},{-125,x2, 79,101}, + {-120,x3,102,128},{-115,x2,129,151},{-110,x3,152,178},{-105,x2,179,201}}, + {{-100,x3, 1, 27},{- 95,x2, 28, 50},{- 90,x1, 51, 78},{- 85,x2, 79,101}, + {- 80,x3,102,128},{- 75,x2,129,151},{- 70,x3,152,178},{- 65,x2,179,201}}, + {{- 60,x3, 1, 27},{- 55,x2, 28, 50},{- 50,x4, 51, 78},{- 45,x2, 79,101}, + {- 40,x3,102,128},{- 35,x2,129,151},{- 30,x3,152,178},{- 25,x2,179,201}}, + {{- 20,x3, 1, 27},{- 15,x2, 28, 50},{- 10,x3, 51, 77},{- 5,x2, 78,100}, + { 0,x1,101,128},{ 5,x2,129,151},{ 10,x3,152,178},{ 15,x2,179,201}}, + {{ 20,x3, 1, 27},{ 25,x2, 28, 50},{ 30,x3, 51, 77},{ 35,x2, 78,100}, + { 40,x4,101,128},{ 45,x2,129,151},{ 50,x3,152,178},{ 55,x2,179,201}}, + {{ 60,x3, 1, 27},{ 65,x2, 28, 50},{ 70,x3, 51, 77},{ 75,x2, 78,100}, + { 80,x3,101,127},{ 85,x2,128,150},{ 90,x1,151,178},{ 95,x2,179,201}}, + {{ 100,x3, 1, 27},{ 105,x2, 28, 50},{ 110,x3, 51, 77},{ 115,x2, 78,100}, + { 120,x3,101,127},{ 125,x2,128,150},{ 130,x4,151,178},{ 135,x2,179,201}}, + {{ 140,x3, 1, 27},{ 145,x2, 28, 50},{ 150,x3, 51, 77},{ 155,x2, 78,100}, + { 160,x3,101,127},{ 165,x2,128,150},{ 170,x3,151,177},{ 175,x2,178,200}} +}; +const sbsigpband_t igpband2[2][5]={ /* band 9-10 */ + {{ 60,x5, 1, 72},{ 65,x6, 73,108},{ 70,x6,109,144},{ 75,x6,145,180}, + { 85,x7,181,192}}, + {{- 60,x5, 1, 72},{- 65,x6, 73,108},{- 70,x6,109,144},{- 75,x6,145,180}, + {- 85,x8,181,192}} +}; + + +char *getfield(char *p, int pos); +double varfcorr(int udre); +double varicorr(int give); +double degfcorr(int ai); +int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav); +int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion); +int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat); +int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat); +int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat); +int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion); +int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); +void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,sbs_t *sbs); +int cmpmsgs(const void *p1, const void *p2); +int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te, + sbs_t *sbs); +int sbsreadmsg(const char *file, int sel, sbs_t *sbs); +void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg); +void searchigp(gtime_t time, const double *pos, const sbsion_t *ion, + const sbsigp_t **igp, double *x, double *y); +int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, double *delay, double *var); + +void getmet(double lat, double *met); +double sbstropcorr(gtime_t time, const double *pos, const double *azel, + double *var); +int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *drs, double *ddts); +int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *prc, double *var); + +int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs, + double *dts, double *var); +int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, + sbsmsg_t *sbsmsg); + + +#endif /* RTKLIB_SBAS_H_ */ diff --git a/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc index 0264acdba..41d2dcf38 100644 --- a/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc @@ -34,9 +34,6 @@ #include #include #include "concurrent_queue.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" #include "sbas_ephemeris.h" #include "configuration_interface.h" #include "sbas_l1_telemetry_decoder_cc.h" diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc index 9b054106d..591ccbe69 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc @@ -140,7 +140,7 @@ int sbas_l1_telemetry_decoder_cc::general_work (int noutput_items __attribute__( // compute message sample stamp // and fill messages in SBAS raw message objects - std::vector sbas_raw_msgs; + //std::vector sbas_raw_msgs; for(std::vector::const_iterator it = valid_msgs.begin(); it != valid_msgs.end(); ++it) { @@ -156,17 +156,17 @@ int sbas_l1_telemetry_decoder_cc::general_work (int noutput_items __attribute__( << " relative_preamble_start=" << it->first << " message_sample_offset=" << message_sample_offset << ")"; - Sbas_Raw_Msg sbas_raw_msg(message_sample_stamp, this->d_satellite.get_PRN(), it->second); - sbas_raw_msgs.push_back(sbas_raw_msg); + //Sbas_Raw_Msg sbas_raw_msg(message_sample_stamp, this->d_satellite.get_PRN(), it->second); + //sbas_raw_msgs.push_back(sbas_raw_msg); } // parse messages // and send them to the SBAS raw message queue - for(std::vector::iterator it = sbas_raw_msgs.begin(); it != sbas_raw_msgs.end(); it++) - { - std::cout << "SBAS message type " << it->get_msg_type() << " from PRN" << it->get_prn() << " received" << std::endl; - sbas_telemetry_data.update(*it); - } + //for(std::vector::iterator it = sbas_raw_msgs.begin(); it != sbas_raw_msgs.end(); it++) + // { + //std::cout << "SBAS message type " << it->get_msg_type() << " from PRN" << it->get_prn() << " received" << std::endl; + //sbas_telemetry_data.update(*it); + // } // clear all processed samples in the input buffer d_sample_buf.clear(); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h index 6fc2d5699..84138491a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h @@ -41,7 +41,6 @@ #include #include "gnss_satellite.h" #include "viterbi_decoder.h" -#include "sbas_telemetry_data.h" class sbas_l1_telemetry_decoder_cc; @@ -159,8 +158,6 @@ private: void zerropad_back_and_convert_to_bytes(const std::vector msg_candidate, std::vector &bytes); } d_crc_verifier; - - Sbas_Telemetry_Data sbas_telemetry_data; }; #endif diff --git a/src/core/receiver/CMakeLists.txt b/src/core/receiver/CMakeLists.txt index e06c2c4f0..99ec3b69d 100644 --- a/src/core/receiver/CMakeLists.txt +++ b/src/core/receiver/CMakeLists.txt @@ -71,6 +71,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${ARMADILLO_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 734496a7b..b8f63e138 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -92,6 +92,7 @@ #include "sbas_l1_telemetry_decoder.h" #include "hybrid_observables.h" #include "hybrid_pvt.h" +#include "rtklib_pvt.h" #if OPENCL_BLOCKS #include "gps_l1_ca_pcps_opencl_acquisition.h" @@ -1072,6 +1073,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("RTKLIB_PVT") == 0) + { + std::unique_ptr block_(new RtklibPvt(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else { // Log fatal. This causes execution to stop. diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index a22c7d9c2..68b6862cf 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -33,9 +33,6 @@ set(SYSTEM_PARAMETERS_SOURCES galileo_iono.cc galileo_navigation_message.cc sbas_ephemeris.cc - sbas_ionospheric_correction.cc - sbas_satellite_correction.cc - sbas_telemetry_data.cc galileo_fnav_message.cc gps_cnav_ephemeris.cc gps_cnav_navigation_message.cc @@ -48,6 +45,8 @@ set(SYSTEM_PARAMETERS_SOURCES include_directories( $(CMAKE_CURRENT_SOURCE_DIR) ${CMAKE_SOURCE_DIR}/src/core/receiver + ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${GLOG_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} @@ -58,6 +57,6 @@ file(GLOB SYSTEM_PARAMETERS_HEADERS "*.h") list(SORT SYSTEM_PARAMETERS_HEADERS) add_library(gnss_system_parameters ${SYSTEM_PARAMETERS_SOURCES} ${SYSTEM_PARAMETERS_HEADERS}) source_group(Headers FILES ${SYSTEM_PARAMETERS_HEADERS}) -add_dependencies(gnss_system_parameters glog-${glog_RELEASE}) -target_link_libraries(gnss_system_parameters ${Boost_LIBRARIES}) +add_dependencies(gnss_system_parameters rtklib_lib glog-${glog_RELEASE}) +target_link_libraries(gnss_system_parameters rtklib_lib ${Boost_LIBRARIES}) diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index 28ec5e0a2..df5d1c04f 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -32,6 +32,7 @@ #ifndef GNSS_SDR_GPS_CNAV_EPHEMERIS_H_ #define GNSS_SDR_GPS_CNAV_EPHEMERIS_H_ +#include "GPS_L2C.h" #include "boost/assign.hpp" #include diff --git a/src/core/system_parameters/sbas_ionospheric_correction.cc b/src/core/system_parameters/sbas_ionospheric_correction.cc deleted file mode 100644 index 3243c32ed..000000000 --- a/src/core/system_parameters/sbas_ionospheric_correction.cc +++ /dev/null @@ -1,467 +0,0 @@ -/*! - * \file sbas_ionospheric_correction.cc - * \brief Implementation of the SBAS ionosphere correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "sbas_ionospheric_correction.h" -#include -#include -#include - - -enum V_Log_Level {EVENT = 2, // logs important events which don't occur every update() call - FLOW = 3, // logs the function calls of block processing functions - MORE = 4}; // very detailed stuff - - -void Sbas_Ionosphere_Correction::print(std::ostream &out) -{ - for(std::vector::const_iterator it_band = d_bands.begin(); it_band != d_bands.end(); ++it_band) - { - int band = it_band - d_bands.begin(); - out << "<> Band" << band << ":" << std::endl; - for(std::vector::const_iterator it_igp = it_band->d_igps.begin(); it_igp != it_band->d_igps.end(); ++it_igp) - { - int igp = it_igp-it_band->d_igps.begin(); - out << "<> -IGP" << igp << ":"; - //std::cout << " valid=" << it_igp->d_valid; - out << " t0=" << it_igp->t0; - out << " lat=" << it_igp->d_latitude; - out << " lon=" << it_igp->d_longitude; - out << " give=" << it_igp->d_give; - out << " delay=" << it_igp->d_delay; - out << std::endl; - } - } -} - - -/* Applies SBAS ionosphric delay correction - * \param[out] delay Slant ionospheric delay (L1) (m) - * \param[out] var Variance of ionospheric delay (m^2) - * \param[in] sample_stamp Sample stamp of observable on which the correction will be applied - * \param[in] longitude_d Receiver's longitude in terms of WGS84 (degree) - * \param[in] latitude_d Receiver's latitude in terms of WGS84 (degree) - * \param[in] azimuth_d Satellite azimuth/elavation angle (rad). Azimuth is the angle of - * the satellite from the user�s location measured clockwise from north - * \param[in] elevation_d Elevation is the angle of the satellite from the user's location measured - * with respect to the local-tangent-plane - */ -bool Sbas_Ionosphere_Correction::apply(double sample_stamp, - double latitude_d, - double longitude_d, - double azimut_d, - double elevation_d, - double &delay, - double &var) -{ - const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E - int result; - double pos[3]; - double azel[2]; - - // convert receiver position from degrees to rad - pos[0] = latitude_d * GPS_PI / 180.0; - pos[1] = longitude_d * GPS_PI / 180.0; - pos[2] = 0; // is not used by sbsioncorr, for ionocorrection is a fixed earth radius assumed - - // convert satellite azimut and elevation from degrees to rad , use topocent to obtain it in pvt block - azel[0] = azimut_d * GPS_PI / 180.0; - azel[1] = elevation_d * GPS_PI / 180.0; - - result = sbsioncorr(sample_stamp, pos, azel, &delay, &var); - return (bool)result; -} - - - -/* geometric distance ---------------------------------------------------------- -* compute geometric distance and receiver-to-satellite unit vector -* args : double *rs I satellilte position (ecef at transmission) (m) -* double *rr I receiver position (ecef at reception) (m) -* double *e O line-of-sight vector (ecef) -* return : geometric distance (m) (0>:error/no satellite position) -* notes : distance includes sagnac effect correction -*-----------------------------------------------------------------------------*/ -//extern double geodist(const double *rs, const double *rr, double *e) -//{ -// double r; -// int i; -// -// if (norm(rs,3)= 0) c += a[n]*b[n]; - return c; -} - - - - -/* multiply matrix -----------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::matmul(const char *tr, int n, int k, int m, double alpha, - const double *A, const double *B, double beta, double *C) -{ - double d; - int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4); - - for (i = 0; i < n; i++) for (j = 0; j < k; j++) - { - d = 0.0; - switch (f) - { - case 1: for (x = 0; x < m; x++) d += A[i + x*n]*B[x + j*m]; break; - case 2: for (x = 0; x < m; x++) d += A[i + x*n]*B[j + x*k]; break; - case 3: for (x = 0; x < m; x++) d += A[x + i*m]*B[x + j*m]; break; - case 4: for (x = 0; x < m; x++) d += A[x + i*m]*B[j + x*k]; break; - } - if (beta == 0.0) - { - C[i + j*n] = alpha*d; - } - else - { - C[i + j*n] = alpha*d + beta*C[i + j*n]; - } - } -} - - -/* ecef to local coordinate transfromation matrix ------------------------------ -* compute ecef to local coordinate transfromation matrix -* args : double *pos I geodetic position {lat,lon} (rad) -* double *E O ecef to local coord transformation matrix (3x3) -* return : none -* notes : matrix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::xyz2enu(const double *pos, double *E) -{ - double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); - E[0] = -sinl; E[3] = cosl; E[6] = 0.0; - E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp; - E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp; -} - - -/* transform ecef vector to local tangential coordinate ------------------------- -* transform ecef vector to local tangential coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *r I vector in ecef coordinate {x,y,z} -* double *e O vector in local tangental coordinate {e,n,u} -* return : none -*-----------------------------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, double *e) -{ - double E[9]; - xyz2enu(pos, E); - matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e); -} - - - -const double PI = 3.1415926535897932; /* pi */ - - - -/* satellite azimuth/elevation angle ------------------------------------------- -* compute satellite azimuth/elevation angle -* args : double *pos I geodetic position {lat,lon,h} (rad,m) -* double *e I receiver-to-satellilte unit vevtor (ecef) -* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output) -* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2) -* return : elevation angle (rad) -*-----------------------------------------------------------------------------*/ -double Sbas_Ionosphere_Correction::satazel(const double *pos, const double *e, double *azel) -{ - const double RE_WGS84 = 6378137.0; /* earth semimajor axis (WGS84) (m) */ - - double az = 0.0, el = PI/2.0, enu[3]; - - if (pos[2] > -RE_WGS84) - { - ecef2enu(pos, e, enu); - az = dot(enu, enu, 2) < 1E-12 ? 0.0 : atan2(enu[0], enu[1]); - if (az < 0.0) az += 2*PI; - el = asin(enu[2]); - } - if (azel) - { - azel[0] = az; - azel[1] = el; - } - return el; -} - -/* debug trace function -----------------------------------------------------*/ -void Sbas_Ionosphere_Correction::trace(int level, const char *format, ...) -{ - va_list ap; - char str[1000]; - - va_start(ap,format); vsprintf(str,format,ap); va_end(ap); - VLOG(FLOW) << "<> " << std::string(str); -} - -/* ionospheric pierce point position ------------------------------------------- -* compute ionospheric pierce point (ipp) position and slant factor -* args : double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double re I earth radius (km) -* double hion I altitude of ionosphere (km) -* double *posp O pierce point position {lat,lon,h} (rad,m) -* return : slant factor -* notes : see ref [2], only valid on the earth surface -* fixing bug on ref [2] A.4.4.10.1 A-22,23 -*-----------------------------------------------------------------------------*/ -double Sbas_Ionosphere_Correction::ionppp(const double *pos, const double *azel, - double re, double hion, double *posp) -{ - double cosaz, rp, ap, sinap, tanap; - const double D2R = (PI/180.0); /* deg to rad */ - - rp = re/(re + hion)*cos(azel[1]); - ap = PI/2.0 - azel[1] - asin(rp); - sinap = sin(ap); - tanap = tan(ap); - cosaz = cos(azel[0]); - posp[0] = asin(sin(pos[0])*cos(ap) + cos(pos[0])*sinap*cosaz); - - if ((pos[0] > 70.0*D2R && tanap*cosaz > tan(PI/2.0 - pos[0])) || - (pos[0] < -70.0*D2R && - tanap*cosaz > tan(PI/2.0 + pos[0]))) - { - posp[1] = pos[1] + PI - asin(sinap*sin(azel[0])/cos(posp[0])); - } - else - { - posp[1] = pos[1] + asin(sinap*sin(azel[0])/cos(posp[0])); - } - return 1.0 / sqrt(1.0 - rp*rp); -} - - -/* variance of ionosphere correction (give=GIVEI) --------------------------*/ -double Sbas_Ionosphere_Correction::varicorr(int give) -{ - const double var[15]={ - 0.0084, 0.0333, 0.0749, 0.1331, 0.2079, 0.2994, 0.4075, 0.5322, 0.6735, 0.8315, - 1.1974, 1.8709, 3.326, 20.787, 187.0826 - }; - return 0 <= give && give < 15 ? var[give]:0.0; -} - - -/* search igps ---------------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, double *x, double *y) -{ - int i; - int latp[2]; - int lonp[4]; - const double R2D = (180.0/PI); /* rad to deg */ - - double lat = pos[0]*R2D; - double lon = pos[1]*R2D; - - trace(4,"searchigp: pos=%.3f %.3f",pos[0]*R2D, pos[1]*R2D); - - // round the pierce point position to the next IGP grid point - if (lon >= 180.0) lon -= 360.0; - if (-55.0 <= lat && lat < 55.0) - { - latp[0] = (int)floor(lat/5.0)*5; - latp[1] = latp[0] + 5; - lonp[0] = lonp[1] = (int)floor(lon/5.0)*5; - lonp[2] = lonp[3] = lonp[0] + 5; - *x = (lon - lonp[0])/5.0; - *y = (lat - latp[0])/5.0; - } - else - { - latp[0] = (int)floor((lat-5.0)/10.0)*10+5; - latp[1] = latp[0] + 10; - lonp[0] = lonp[1] = (int)floor(lon/10.0)*10; - lonp[2] = lonp[3] = lonp[0] + 10; - *x = (lon - lonp[0])/10.0; - *y = (lat - latp[0])/10.0; - if (75.0 <= lat && lat < 85.0) - { - lonp[1] = (int)floor(lon/90.0)*90; - lonp[3] = lonp[1] + 90; - } - else if (-85.0 <= lat && lat < -75.0) - { - lonp[0] = (int)floor((lon - 50.0)/90.0)*90 + 40; - lonp[2] = lonp[0] + 90; - } - else if (lat >= 85.0) - { - for (i = 0; i < 4; i++) lonp[i] = (int)floor(lon/90.0)*90; - } - else if (lat <- 85.0) - { - for (i = 0; i < 4; i++) lonp[i] = (int)floor((lon - 50.0)/90.0)*90 + 40; - } - } - - for (i = 0; i < 4; i++) if (lonp[i] == 180) lonp[i] = -180; - - // find the correction data for the grid points in latp[] and lonp[] - // iterate over bands - for (std::vector::const_iterator band_it = this->d_bands.begin(); band_it != d_bands.end(); ++band_it) - { - //VLOG(MORE) << "band=" << band_it-d_bands.begin() << std::endl; - // iterate over IGPs in band_it - for (std::vector::const_iterator igp_it = band_it->d_igps.begin(); igp_it != band_it->d_igps.end(); ++igp_it) - { - std::stringstream ss; - int give = igp_it->d_give; - ss << "IGP: give=" << give; - if(give < 15) // test if valid correction data is sent for current IGP - { - int lat = igp_it->d_latitude; - int lon = igp_it->d_longitude; - ss << " lat=" << lat << " lon=" << lon; - if (lat == latp[0] && lon == lonp[0]) igp[0] = igp_it.base(); - else if (lat == latp[1] && lon == lonp[1]) igp[1] = igp_it.base(); - else if (lat == latp[0] && lon == lonp[2]) igp[2] = igp_it.base(); - else if (lat == latp[1] && lon == lonp[3]) igp[3] = igp_it.base(); - } - //VLOG(MORE) << ss.str(); - } - } - //VLOG(MORE) << "igp[0:3]={" << igp[0] << "," << igp[1] << "," << igp[2] << "," << igp[3] << "}"; -} - - - -/* sbas ionospheric delay correction ------------------------------------------- -* compute sbas ionosphric delay correction -* args : long sample_stamp I sample stamp of observable on which the correction will be applied -* sbsion_t *ion I ionospheric correction data (implicit) -* double *pos I receiver position {lat,lon,height} (rad/m) -* double *azel I satellite azimuth/elavation angle (rad) -* double *delay O slant ionospheric delay (L1) (m) -* double *var O variance of ionospheric delay (m^2) -* return : status (1:ok, 0:no correction) -* notes : before calling the function, sbas ionosphere correction parameters -* in navigation data (nav->sbsion) must be set by calling -* sbsupdatecorr() -*-----------------------------------------------------------------------------*/ -int Sbas_Ionosphere_Correction::sbsioncorr(const double sample_stamp, const double *pos, - const double *azel, double *delay, double *var) -{ - const double re = 6378.1363; - const double hion = 350.0; - int err = 0; - double fp; - double posp[2]; - double x = 0.0; - double y = 0.0; - double t; - double w[4] = {0}; - const Igp *igp[4] = {0}; /* {ws,wn,es,en} */ - const double R2D = (180.0/PI); /* rad to deg */ - - trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f", pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D); - - *delay = *var = 0.0; - if (pos[2] < -100.0 || azel[1] <= 0) return 1; - - /* ipp (ionospheric pierce point) position */ - fp = ionppp(pos, azel, re, hion, posp); - - /* search igps around ipp */ - searchigp(posp, igp, &x, &y); - - VLOG(FLOW) << "<> SBAS iono correction:" << " igp[0]=" << igp[0] << " igp[1]=" << igp[1] - << " igp[2]=" << igp[2] << " igp[3]=" << igp[3] << " x=" << x << " y=" << y; - - /* weight of igps */ - if (igp[0] && igp[1] && igp[2] && igp[3]) - { - w[0] = (1.0 - x)*(1.0 - y); - w[1] = (1.0 - x)*y; - w[2] = x*(1.0 - y); - w[3] = x*y; - } - else if (igp[0] && igp[1] && igp[2]) - { - w[1] = y; - w[2] = x; - if ((w[0] = 1.0 - w[1] - w[2]) < 0.0) err = 1; - } - else if (igp[0] && igp[2] && igp[3]) - { - w[0] = 1.0 - x; - w[3] = y; - if ((w[2] = 1.0 - w[0] -w[3]) < 0.0) err = 1; - } - else if (igp[0] && igp[1] && igp[3]) - { - w[0] = 1.0 - y; - w[3] = x; - if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1; - } - else if (igp[1]&&igp[2]&&igp[3]) - { - w[1] = 1.0 - x; - w[2] = 1.0 - y; - if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1; - } - else err = 1; - - if (err) - { - trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f", posp[0]*R2D, posp[1]*R2D); - return 0; - } - for (int i = 0; i <4 ; i++) - { - if (!igp[i]) continue; - t = (sample_stamp - igp[i]->t0); // time diff between now and reception of the igp data in seconds - *delay += w[i]*igp[i]->d_delay; - *var += w[i] * varicorr(igp[i]->d_give) * 9E-8 * fabs(t); - } - *delay *= fp; - *var *= fp*fp; - - trace(5, "sbsioncorr: dion=%7.2f sig=%7.2f", *delay, sqrt(*var)); - return 1; -} diff --git a/src/core/system_parameters/sbas_ionospheric_correction.h b/src/core/system_parameters/sbas_ionospheric_correction.h deleted file mode 100644 index e2988fa18..000000000 --- a/src/core/system_parameters/sbas_ionospheric_correction.h +++ /dev/null @@ -1,203 +0,0 @@ -/*! - * \file sbas_ionospheric_correction.h - * \brief Interface of the SBAS ionosphere correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef SBAS_IONOSPHERIC_CORRECTION_H_ -#define SBAS_IONOSPHERIC_CORRECTION_H_ - -#include -#include -#include -#include -#include -#include -#include -#include - - -/*! - * \brief Struct that represents a Ionospheric Grid Point (IGP) - */ -struct Igp -{ -public: - //bool d_valid; // valid==true indicates that the IGP can be used for corrections. it is set to false when a new IGP mask (MT18) has been received but no corresponding delays (MT26) - double t0; // time of reception, time of correction - int d_latitude; - int d_longitude; - int d_give; - double d_delay; -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & t0; - ar & d_latitude; - ar & d_longitude; - ar & d_give; - ar & d_delay; - } -}; - - -/*! - * \brief Struct that represents the band of a Ionospheric Grid Point (IGP) - */ -struct Igp_Band -{ - //int d_iodi; - //int d_nigp; // number if IGPs in this band (defined by IGP mask from MT18) - std::vector d_igps; -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & d_igps; - } -}; - - - -/*! - * \brief Class that handles valid SBAS ionospheric correction for GPS - */ -class Sbas_Ionosphere_Correction -{ -private: - /* Inner product of vectors - * params : double *a,*b I vector a,b (n x 1) - * int n I size of vector a,b - * return : a'*b - */ - double dot(const double *a, const double *b, int n); - - /* Multiply matrix */ - void matmul(const char *tr, int n, int k, int m, double alpha, - const double *A, const double *B, double beta, double *C); - - /* EFEC to local coordinate transfomartion matrix - * Compute ecef to local coordinate transfomartion matrix - * params : double *pos I geodetic position {lat,lon} (rad) - * double *E O ecef to local coord transformation matrix (3x3) - * return : none - * notes : matrix stored by column-major order (fortran convention) - */ - void xyz2enu(const double *pos, double *E); - - /* Transforms ECEF vector into local tangential coordinates - * params : double *pos I geodetic position {lat,lon} (rad) - * double *r I vector in ecef coordinate {x,y,z} - * double *e O vector in local tangental coordinate {e,n,u} - * return : none - */ - void ecef2enu(const double *pos, const double *r, double *e); - - /* Compute satellite azimuth/elevation angle - * params : double *pos I geodetic position {lat,lon,h} (rad,m) - * double *e I receiver-to-satellilte unit vevtor (ecef) - * double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output) - * (0.0 <= azel[0] < 2*pi, -pi/2 <= azel[1] <= pi/2) - * return : elevation angle (rad) - */ - double satazel(const double *pos, const double *e, double *azel); - - /* Debug trace functions */ - void trace(int level, const char *format, ...); - - /* time difference ------------------------------------------------------------- - * difference between gtime_t structs - * args : gtime_t t1,t2 I gtime_t structs - * return : time difference (t1-t2) (s) - *-----------------------------------------------------------------------------*/ - //double timediff(gtime_t t1, gtime_t t2); - - /* Compute Ionospheric Pierce Point (IPP) position and slant factor - * params : double *pos I receiver position {lat,lon,h} (rad,m) - * double *azel I azimuth/elevation angle {az,el} (rad) - * double re I earth radius (km) - * double hion I altitude of ionosphere (km) - * double *posp O pierce point position {lat,lon,h} (rad,m) - * return : slant factor - * notes : see ref [2], only valid on the earth surface - * fixing bug on ref [2] A.4.4.10.1 A-22,23 - *-----------------------------------------------------------------------------*/ - double ionppp(const double *pos, const double *azel, double re, - double hion, double *posp); - - /* Variance of ionosphere correction (give = GIVEI + 1) */ - double varicorr(int give); - - /* Search igps */ - void searchigp(const double *pos, const Igp **igp, double *x, double *y); - - /* Compute sbas ionospheric delay correction - * params : long sample_stamp I sample stamp of observable on which the correction will be applied - * sbsion_t *ion I ionospheric correction data (implicit) - * double *pos I receiver position {lat,lon,height} (rad/m) - * double *azel I satellite azimuth/elavation angle (rad) - * double *delay O slant ionospheric delay (L1) (m) - * double *var O variance of ionospheric delay (m^2) - * return : status (1:ok, 0:no correction) - * notes : before calling the function, sbas ionosphere correction parameters - * in navigation data (nav->sbsion) must be set by callig - * sbsupdatecorr() - */ - int sbsioncorr(const double sample_stamp, const double *pos, - const double *azel, double *delay, double *var); - - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version){ar & d_bands;} - -public: - std::vector d_bands; - void print(std::ostream &out); - - /*! - * \brief Computes SBAS ionospheric delay correction. - * - * \param[out] delay Slant ionospheric delay (L1) (m) - * \param[out] var Variance of ionospheric delay (m^2) - * \param[in] sample_stamp Sample stamp of observable on which the correction will be applied - * \param[in] longitude_d Receiver's longitude in terms of WGS84 (degree) - * \param[in] latitude_d Receiver's latitude in terms of WGS84 (degree) - * \param[in] azimuth_d Satellite azimuth/elavation angle (rad). Azimuth is the angle of - * the satellite from the user's location measured clockwise from north - * \param[in] elevation_d Elevation is the angle of the satellite from the user's location measured - * with respect to the local-tangent-plane - */ - bool apply(double sample_stamp, double latitude_d, double longitude_d, - double azimut_d, double evaluation_d, double &delay, double &var); - -}; - - -#endif /* SBAS_IONOSPHERIC_CORRECTION_H_ */ diff --git a/src/core/system_parameters/sbas_satellite_correction.cc b/src/core/system_parameters/sbas_satellite_correction.cc deleted file mode 100644 index 106992364..000000000 --- a/src/core/system_parameters/sbas_satellite_correction.cc +++ /dev/null @@ -1,296 +0,0 @@ -/*! - * \file sbas_satellite_correction.cc - * \brief Implementation of the SBAS satellite correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "sbas_satellite_correction.h" -#include -#include -#include -#include -#include - - -#define EVENT 2 // logs important events which don't occur every update() call -#define FLOW 3 // logs the function calls of block processing functions - -#define CLIGHT 299792458.0 /* speed of light (m/s) */ -#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ -#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ - - -void Sbas_Satellite_Correction::print(std::ostream &out) -{ - out << "<> Sbas satellite corrections for PRN" << d_prn << ":" << std::endl; - print_fast_correction(out); - print_long_term_correction(out); -} - - -void Sbas_Satellite_Correction::print_fast_correction(std::ostream &out) -{ - Fast_Correction fcorr = d_fast_correction; - out << "<> Fast PRN" << d_prn << ":"; - if(fcorr.d_tof.is_related()) - { - int gps_week; - double gps_sec; - fcorr.d_tof.get_gps_time(gps_week, gps_sec); - out << " d_t0=(week=" << gps_week << ",sec=" << gps_sec << ")"; - } - else - { - out << " d_t0=" << fcorr.d_tof.get_time_stamp(); - } - out << " d_prc=" << fcorr.d_prc; - out << " d_rrc=" << fcorr.d_rrc; - out << " d_dt=" << fcorr.d_dt; - out << " d_udre=" << fcorr.d_udre; - out << " d_ai=" << fcorr.d_ai; - out << " d_tlat=" << fcorr.d_tlat; -} - - -void Sbas_Satellite_Correction::print_long_term_correction(std::ostream &out) -{ - Long_Term_Correction lcorr = d_long_term_correction; - out << "<> Long PRN" << d_prn << ":"; - out << " d_trx=" << lcorr.d_trx; - out << " i_tapp=" << lcorr.i_tapp; - out << " i_vel=" << lcorr.i_vel; - double *p = lcorr.d_dpos; - out << " d_dpos=(x=" << p[0] << ", y=" << p[1] << ", z=" << p[2] << ")" ; - double *v = lcorr.d_dvel; - out << " d_dvel=(x=" << v[0] << ", y=" << v[1] << ", z=" << v[2] << ")" ; - out << " d_daf0=" << lcorr.d_daf0; - out << " d_daf1=" << lcorr.d_daf1; -} - - -int Sbas_Satellite_Correction::apply_fast(double sample_stamp, double &pr, double &var) -{ - int result; - double prc = 0; // pseudo range correction - result = sbsfastcorr(sample_stamp, &prc, &var); - pr += prc; - VLOG(FLOW) << "<> fast correction applied: sample_stamp=" << sample_stamp << " prc=" << prc << " corr. pr=" << pr; - return result; -} - - - -int Sbas_Satellite_Correction::apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var) -{ - int result; - double drs[3] = {0}; - double ddts = 0; - result = sbslongcorr(sample_stamp, drs, &ddts); - for (int i = 0; i < 3; i++) sv_pos[i] += drs[i]; - VLOG(FLOW) << "<> long term sv pos correction applied: sample_stamp=" << sample_stamp << " drs=(x=" << drs[0] << " y=" << drs[1] << " z=" << drs[2] << ")"; - return result; -} - - - -int Sbas_Satellite_Correction::apply_long_term_sv_clk(double sample_stamp, double &dts, double &var) -{ - int result; - double drs[3] = {0}; - double ddts = 0; - result = sbslongcorr(sample_stamp, drs, &ddts); - dts += ddts; - VLOG(FLOW) << "<> long term sv clock correction correction applied: sample_stamp=" << sample_stamp << " ddts=" << ddts; - return result; -} - - -bool Sbas_Satellite_Correction::alarm() -{ - return this->d_fast_correction.d_udre == 16; -} - - - -/* debug trace function -----------------------------------------------------*/ -void Sbas_Satellite_Correction::trace(int level, const char *format, ...) -{ - va_list ap; - char str[1000]; - va_start(ap,format); - vsprintf(str,format,ap); - va_end(ap); - VLOG(FLOW) << "<> " << std::string(str); -} - - -/* variance of fast correction (udre=UDRE+1) ---------------------------------*/ -double Sbas_Satellite_Correction::varfcorr(int udre) -{ - const double var[14] = { - 0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326, - 5.1968, 20.7870, 230.9661, 2078.695 - }; - return 0 < udre && udre <= 14 ? var[udre - 1] : 0.0; -} - - -/* fast correction degradation -----------------------------------------------*/ -double Sbas_Satellite_Correction::degfcorr(int ai) -{ - const double degf[16] = { - 0.00000, 0.00005, 0.00009, 0.00012, 0.00015, 0.00020, 0.00030, 0.00045, - 0.00060, 0.00090, 0.00150, 0.00210, 0.00270, 0.00330, 0.00460, 0.00580 - }; - return 0 < ai && ai <= 15 ? degf[ai] : 0.0058; -} - - - -/* long term correction ------------------------------------------------------*/ -int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, double *ddts) -{ - double t = 0.0; - int i; - Long_Term_Correction lcorr = d_long_term_correction; - trace(3, "sbslongcorr: prn=%2d", this->d_prn); - // if (p->sat!=sat||p->lcorr.t0.time==0) continue; - // compute time of applicability - if(d_long_term_correction.i_vel == 1) - { - // time of applicability is the one sent, i.e., tapp - // TODO: adapt for vel==1 case - // t = tow-d_long_term_correction.i_tapp; - // vel=1 -> time of applicability is sent in message, needs to be corrected for rollover which can not be done here, since the absolute gps time is unknown. see IS-GPS-200G pdf page 116 for correction - /* t = (int)getbitu(msg->msg, p + 90, 13)*16 - (int)msg->tow%86400; - if (t <= -43200) t += 86400; - else if (t > 43200) t -= 86400; - sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/ - } - else - { - // time of applicability is time of reception - t = time_stamp - lcorr.d_trx; // should not have any impact if vel==0 since d_dvel and d_daf1 are zero, is only used to check outdating - } - //t=time_stamp-lcorr.d_t0; - if (fabs(t) > MAXSBSAGEL) - { - trace(2,"sbas long-term correction expired: sat=%2d time_stamp=%5.0f", d_prn, time_stamp); - return 0; - } - // sv position correction - for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t; - // sv clock correction correction - *ddts = lcorr.d_daf0 + lcorr.d_daf1*t; - trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f", d_prn, drs[0], drs[1], drs[2], *ddts * CLIGHT); - return 1; - /* if sbas satellite without correction, no correction applied */ - //if (satsys(sat,NULL)==SYS_SBS) return 1; - //trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); - //return 0; -} - - - - -/* fast correction -----------------------------------------------------------*/ -int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, double *var) -#define RRCENA -{ - double t; - Fast_Correction fcorr = d_fast_correction; - trace(3, "sbsfastcorr: sat=%2d", this->d_prn); - //if (p->fcorr.t0.time==0) break; // time==0is only true if t0 hasn't been initialised -> it checks if the correction is valid - t = (time_stamp - fcorr.d_tof.get_time_stamp()) + fcorr.d_tlat; // delta t between now and tof - /* expire age of correction? */ - if (fabs(t) > MAXSBSAGEF) - { - trace(2, "no sbas fast correction (expired): time_stamp=%f prn=%2d", time_stamp, d_prn); - return 0; - } - /* UDRE==14 (not monitored)? */ - else if(fcorr.d_udre == 15) - { - trace(2,"no sbas fast correction (not monitored): time_stamp=%f prn=%2d", time_stamp, d_prn); - return 0; - } - else if(fcorr.d_udre == 16) - { - trace(2,"SV is marked as unhealthy: time_stamp=%f prn=%2d", time_stamp, d_prn); - return 0; - } - *prc = fcorr.d_prc; -#ifdef RRCENA - if (fcorr.d_ai > 0 && fabs(t) <= 8.0*fcorr.d_dt) - { - *prc += fcorr.d_rrc*t; - } -#endif - *var = varfcorr(fcorr.d_udre) + degfcorr(fcorr.d_ai) * t * t / 2.0; - trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f", d_prn, *prc, sqrt(*var), t); - return 1; -} - - - -/* sbas satellite ephemeris and clock correction ------------------------------- -* correct satellite position and clock bias with sbas satellite corrections -* args : long time_stamp I reception time stamp -* double *rs IO sat position and corrected {x,y,z} (ecef) (m) -* double *dts IO sat clock bias and corrected (s) -* double *var O sat position and clock variance (m^2) -* return : status (1:ok,0:no correction) -* notes : before calling the function, sbas satellite correction parameters -* in navigation data (nav->sbssat) must be set by callig -* sbsupdatecorr(). -* satellite clock correction include long-term correction and fast -* correction. -* sbas clock correction is usually based on L1C/A code. TGD or DCB has -* to be considered for other codes -*-----------------------------------------------------------------------------*/ -int Sbas_Satellite_Correction::sbssatcorr(double time_stamp, double *rs, double *dts, double *var) -{ - double drs[3] = {0}, dclk = 0.0, prc = 0.0; - int i; - trace(3,"sbssatcorr : sat=%2d",d_prn); - /* sbas long term corrections */ - if (!sbslongcorr(time_stamp, drs, &dclk)) - { - return 0; - } - /* sbas fast corrections */ - if (!sbsfastcorr(time_stamp, &prc, var)) - { - return 0; - } - for (i = 0; i < 3; i++) rs[i] += drs[i]; - dts[0] += dclk + prc/CLIGHT; - trace(5, "sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f", - d_prn, drs[0], drs[1], drs[2], dclk,prc/CLIGHT, *var); - return 1; -} - diff --git a/src/core/system_parameters/sbas_satellite_correction.h b/src/core/system_parameters/sbas_satellite_correction.h deleted file mode 100644 index 72c179594..000000000 --- a/src/core/system_parameters/sbas_satellite_correction.h +++ /dev/null @@ -1,107 +0,0 @@ -/*! - * \file sbas_satellite_correction.h - * \brief Interface of the SBAS satellite correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_ -#define GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_ - -#include "sbas_time.h" - -struct Fast_Correction -{ - Sbas_Time d_tof; // for fast corrections the time of applicability (tof) is defined as the time when the corresponding message was send - double d_prc; - double d_rrc; - double d_dt; - int d_udre; // UDRE - int d_ai; - int d_tlat; -}; - - -struct Long_Term_Correction -{ - double d_trx; //!< Time when message was received - int i_tapp; //!< Time of applicability (only valid if vel=1, equals the sent t0) - int i_vel; //!< Use velocity corrections if vel=1 - int d_iode; - double d_dpos[3]; //!< position correction - double d_dvel[3]; //!< velocity correction - double d_daf0; //!< clock offset correction - double d_daf1; //!< clock drift correction -}; - - -/*! - * \brief Valid long and fast term SBAS corrections for one SV - */ -class Sbas_Satellite_Correction -{ -public: - int d_prn; - Fast_Correction d_fast_correction; - Long_Term_Correction d_long_term_correction; - void print(std::ostream &out); - void print_fast_correction(std::ostream &out); - void print_long_term_correction(std::ostream &out); - int apply_fast(double sample_stamp, double &pr, double &var); - int apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var); - int apply_long_term_sv_clk(double sample_stamp, double &dts, double &var); - bool alarm(); -private: - /* debug trace functions -----------------------------------------------------*/ - void trace(int level, const char *format, ...); - /* variance of fast correction (udre=UDRE+1) ---------------------------------*/ - double varfcorr(int udre); - /* fast correction degradation -----------------------------------------------*/ - double degfcorr(int ai); - /* long term correction ------------------------------------------------------*/ - int sbslongcorr(double time_stamp, double *drs, double *ddts); - /* fast correction -----------------------------------------------------------*/ - int sbsfastcorr(double time_stamp, double *prc, double *var); - /* sbas satellite ephemeris and clock correction ------------------------------- - * correct satellite position and clock bias with sbas satellite corrections - * args : long time_stamp I reception time stamp - * double *rs IO sat position and corrected {x,y,z} (ecef) (m) - * double *dts IO sat clock bias and corrected (s) - * double *var O sat position and clock variance (m^2) - * return : status (1:ok,0:no correction) - * notes : before calling the function, sbas satellite correction parameters - * in navigation data (nav->sbssat) must be set by callig - * sbsupdatecorr(). - * satellite clock correction include long-term correction and fast - * correction. - * sbas clock correction is usually based on L1C/A code. TGD or DCB has - * to be considered for other codes - *-----------------------------------------------------------------------------*/ - int sbssatcorr(double time_stamp, double *rs, double *dts, double *var); -}; - - -#endif /* GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_ */ diff --git a/src/core/system_parameters/sbas_telemetry_data.cc b/src/core/system_parameters/sbas_telemetry_data.cc deleted file mode 100644 index 599f03b55..000000000 --- a/src/core/system_parameters/sbas_telemetry_data.cc +++ /dev/null @@ -1,978 +0,0 @@ -/*! - * \file sbas_telemetry_data.cc - * \brief Implementation of the SBAS telemetry parser based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include -#include -#include -#include -#include -#include -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" -#include "sbas_ephemeris.h" - - -// logging levels -#define EVENT 2 // logs important events which don't occur every update() call -#define FLOW 3 // logs the function calls of block processing functions -#define DETAIL 4 - - - -Sbas_Telemetry_Data::Sbas_Telemetry_Data() -{ - fp_trace = nullptr; // file pointer of trace - level_trace = 0; // level of trace - tick_trace = 0; // tick time at traceopen (ms) - - d_nav.sbssat.iodp = -1; // make sure that in any case iodp is not equal to the received one - prn_mask_changed(); // invalidate all satellite corrections - - for(size_t band = 0; band < sizeof(d_nav.sbsion)/sizeof(sbsion_t); band++) - { - d_nav.sbsion[band].iodi = -1; // make sure that in any case iodi is not equal to the received one - igp_mask_changed(band); - } -} - -Sbas_Telemetry_Data::~Sbas_Telemetry_Data() -{ - -} - - -int Sbas_Telemetry_Data::update(Sbas_Raw_Msg sbas_raw_msg) -{ - VLOG(FLOW) << "<> Sbas_Telemetry_Data.update():"; - int parsing_result; - - // if GPS time from MT12 is known (automatically handled by relate()): - // express the rx time in terms of GPS time - sbas_raw_msg.relate(mt12_time_ref); - - int mt = sbas_raw_msg.get_msg_type(); - // update internal state - if(mt == 12) parsing_result = decode_mt12(sbas_raw_msg); - //else if(mt == 9) parsing_result = parse_mt9(sbas_raw_msg); - else - { - // use RTKLIB to parse the message -> updates d_nav structure - sbsmsg_t sbas_raw_msg_rtklib; - std::vector msg_bytes = sbas_raw_msg.get_msg(); - // cast raw message to RTKLIB raw message struct - sbas_raw_msg_rtklib.prn = sbas_raw_msg.get_prn(); - //sbas_raw_msg_rtklib.tow = sbas_raw_msg.get_tow(); - //sbas_raw_msg_rtklib.week = sbas_raw_msg.get_week(); - sbas_raw_msg_rtklib.sample_stamp = sbas_raw_msg.get_sample_stamp(); - for (std::vector::const_iterator it = msg_bytes.begin(); it != msg_bytes.end() - 3; ++it) - { - int i = it - msg_bytes.begin(); - sbas_raw_msg_rtklib.msg[i] = *it; - } - parsing_result = sbsupdatecorr(&sbas_raw_msg_rtklib, &d_nav); - VLOG(FLOW) << "<> RTKLIB parsing result: " << parsing_result; - } - - // update gnss-sdr correction data sets from RTKLIB d_nav structure, emit SBAS data into queues - switch (parsing_result) - { - case -1: VLOG(FLOW) << "message parsing problem for MT" << sbas_raw_msg.get_msg_type(); break; - case 2: - case 3: - case 4: - case 5: - case 6: - case 7: - case 24: - case 25: updated_satellite_corrections(); break; - case 18: break; // new iono band mask received -> dont update iono corrections because delays are not - case 26: received_iono_correction(); break; - case 9: /*updated_sbas_ephemeris(sbas_raw_msg);*/ break; - - default: break; - } - - // send it to raw message queue - //TODO: update to new GNURadio msg queue - //if(raw_msg_queue != nullptr) raw_msg_queue->push(sbas_raw_msg); - return parsing_result; -} - - -unsigned int getbitu(const unsigned char *buff, int pos, int len); - - - -int getbits(const unsigned char *buff, int pos, int len); - - - -int Sbas_Telemetry_Data::decode_mt12(Sbas_Raw_Msg sbas_raw_msg) -{ - const double rx_delay = 38000.0/300000.0; // estimated sbas signal geosat to ground signal travel time - unsigned char * msg = sbas_raw_msg.get_msg().data(); - uint32_t gps_tow = getbitu(msg, 121, 20); - uint32_t gps_week = getbitu(msg, 141, 10) + 1024; // consider last gps time week overflow - double gps_tow_rx = double(gps_tow) + rx_delay; - mt12_time_ref = Sbas_Time_Relation(sbas_raw_msg.get_sample_stamp(), gps_week, gps_tow_rx); - VLOG(FLOW) << "<> extracted GPS time from MT12: gps_tow=" << gps_tow << " gps_week=" << gps_week; - return 12; -} - - - - -void Sbas_Telemetry_Data::updated_sbas_ephemeris(Sbas_Raw_Msg msg) -{ - VLOG(FLOW) << "<> updated_sbas_ephemeris():" << std::endl; - Sbas_Ephemeris seph; - int satidx = msg.get_prn() - MINPRNSBS; - seph_t seph_rtklib = d_nav.seph[satidx]; - // copy data - seph.i_prn = msg.get_prn(); - seph.i_t0 = seph_rtklib.t0; - seph.d_tof = seph_rtklib.tof; - seph.i_sv_ura = seph_rtklib.sva; - seph.b_sv_do_not_use = seph_rtklib.svh; - memcpy(seph.d_pos, seph_rtklib.pos, sizeof(seph.d_pos)); - memcpy(seph.d_vel, seph_rtklib.vel, sizeof(seph.d_vel)); - memcpy(seph.d_acc, seph_rtklib.acc, sizeof(seph.d_acc)); - seph.d_af0 = seph_rtklib.af0; - seph.d_af1 = seph_rtklib.af1; - // print ephemeris for debugging purposes - std::stringstream ss; - seph.print(ss); - VLOG(FLOW) << ss.str(); - - //todo_Update to new GNURadio msg queue - //if(ephemeris_queue != nullptr) ephemeris_queue->push(seph); -} - - - -void Sbas_Telemetry_Data::received_iono_correction() -{ - VLOG(FLOW) << "<> received_iono_correction():"; - std::stringstream ss; - - Sbas_Ionosphere_Correction iono_corr; - for (size_t i_band = 0; i_band < sizeof(d_nav.sbsion)/sizeof(sbsion_t); i_band++) - { - ss << "<> band=" << i_band - << " nigp=" << d_nav.sbsion[i_band].nigp << std::endl; - ss << "<> -> valid igps:"; - Igp_Band igp_band; - for (int i_igp = 0; i_igp < d_nav.sbsion[i_band].nigp; i_igp++) - { - if(d_nav.sbsion[i_band].igp[i_igp].valid) - { - // consider only valid IGPs, i.e, such ones which got updated at least once since instantiating sbas_telemtry_data - ss << " " << i_igp; - Igp igp; - igp.t0 = d_nav.sbsion[i_band].igp[i_igp].t0; - igp.d_latitude = d_nav.sbsion[i_band].igp[i_igp].lat; - igp.d_longitude = d_nav.sbsion[i_band].igp[i_igp].lon; - igp.d_give = d_nav.sbsion[i_band].igp[i_igp].give; - igp.d_delay = d_nav.sbsion[i_band].igp[i_igp].delay; - igp_band.d_igps.push_back(igp); - } - } - ss << std::endl; - iono_corr.d_bands.push_back(igp_band); - } - VLOG(DETAIL) << ss.str(); - ss.str(""); - iono_corr.print(ss); - VLOG(EVENT) << ss.str(); - - // send to SBAS ionospheric correction queue - //todo_Update to new GNURadio msg queue - //if(iono_queue != nullptr) iono_queue->push(iono_corr); -} - - -// helper for comparing two POD structures with undefined padding between members -// not guaranteed to work always properly -> don't use it for critical tasks -template -inline bool are_equal(const Struct &s1, const Struct &s2) -{ - const size_t struct_size = sizeof(Struct); - bool is_equal = true; - bool is_equal_manual = true; - std::stringstream ss; - - Struct *s1_; - Struct *s2_; - - // reserve zero initialised memory - s1_ = (Struct*) calloc (1, struct_size); - s2_ = (Struct*) calloc (1, struct_size); - - // use assignment constructor which doesn't copy paddings - *s1_ = s1; - *s2_ = s2; - - // compare struct memory byte-wise - is_equal_manual = true; - ss.str(); - ss << "\n<> compare objects of size=" << sizeof(Struct) << " (memcmp says is_equal=" << is_equal << ") :" << std::endl; - for (size_t i = 0; i < sizeof(Struct); ++i) - { - char byte1 = ((char *)s1_)[i]; - char byte2 = ((char *)s2_)[i]; - if(byte1 != byte2) is_equal_manual = false; - ss << "<> s1=" << std::hex << std::setw(4) << std::setfill('0'); - ss << (short)byte1; - ss << " s2=" << std::hex << std::setw(4) << std::setfill('0'); - ss << (short)byte2; - ss << " equal=" << is_equal_manual; - ss << std::endl; - } - - free(s1_); - free(s2_); - - return is_equal_manual; -} - - - -void Sbas_Telemetry_Data::updated_satellite_corrections() -{ - VLOG(FLOW) << "<> updated_satellite_corrections():"; - // for each satellite in the RTKLIB structure - for (int i_sat = 0; i_sat < d_nav.sbssat.nsat; i_sat++) - { - std::stringstream ss; - ss << "<> sat=" << d_nav.sbssat.sat[i_sat].sat - << " fastcorr.valid=" << d_nav.sbssat.sat[i_sat].fcorr.valid - << " lcorr.valid=" << d_nav.sbssat.sat[i_sat].lcorr.valid; - - if(is_rtklib_sat_correction_valid(i_sat)) // check if ever updated by a received message - { - int prn = d_nav.sbssat.sat[i_sat].sat; - - // get fast correction from RTKLIB structure - sbsfcorr_t fcorr_rtklib = d_nav.sbssat.sat[i_sat].fcorr; - Fast_Correction fcorr; - fcorr.d_tof = Sbas_Time(fcorr_rtklib.t0, mt12_time_ref); - fcorr.d_prc = fcorr_rtklib.prc; - fcorr.d_rrc = fcorr_rtklib.rrc; - fcorr.d_dt = fcorr_rtklib.dt; - fcorr.d_udre = fcorr_rtklib.udre; // UDRE - fcorr.d_ai = fcorr_rtklib.ai; - fcorr.d_tlat = d_nav.sbssat.tlat; - - // get long term correction from RTKLIB structure - sbslcorr_t lcorr_rtklib = d_nav.sbssat.sat[i_sat].lcorr; - Long_Term_Correction lcorr; - lcorr.d_trx = lcorr_rtklib.trx; - lcorr.i_tapp = lcorr_rtklib.tapp; - lcorr.i_vel = lcorr_rtklib.vel; - lcorr.d_iode = lcorr_rtklib.iode; - memcpy(lcorr.d_dpos, lcorr_rtklib.dpos, sizeof(lcorr.d_dpos)); - memcpy(lcorr.d_dvel, lcorr_rtklib.dvel, sizeof(lcorr.d_dvel)); - lcorr.d_daf0 = lcorr_rtklib.daf0; - lcorr.d_daf1= lcorr_rtklib.daf1; - - bool fast_correction_updated = false; - bool long_term_correction_updated = false; - - // check if fast corrections got updated - std::map::iterator it_old_fcorr = emitted_fast_corrections.find(prn); - if(it_old_fcorr == emitted_fast_corrections.end() || !are_equal < Fast_Correction>(fcorr, it_old_fcorr->second )) - { - // got updated - ss << " fast_correction_updated=" << true; - //ss << " not_found=" << (it_old_fcorr == emitted_fast_corrections.end()); - //ss << " not_equal=" << (!are_equal(fcorr, it_old_fcorr->second )); - fast_correction_updated = true; - emitted_fast_corrections[prn] = fcorr; - } - - // check if long term corrections got updated - std::map::iterator it_old_lcorr = emitted_long_term_corrections.find(prn); - if(it_old_lcorr == emitted_long_term_corrections.end() || !are_equal < Long_Term_Correction>(lcorr, it_old_lcorr->second )) - { - // got updated - ss << " long_term_correction_updated=" << true; - //ss << " not_found=" << (it_old_lcorr == emitted_long_term_corrections.end()); - //ss << " not_equal=" << (!are_equal(lcorr, it_old_lcorr->second )); - long_term_correction_updated = true; - emitted_long_term_corrections[prn] = lcorr; - } - - Sbas_Satellite_Correction sbas_satelite_correction; - sbas_satelite_correction.d_prn = prn; - sbas_satelite_correction.d_fast_correction = fcorr; - sbas_satelite_correction.d_long_term_correction = lcorr; - - if(fast_correction_updated) - { - ss << std::endl; - sbas_satelite_correction.print_fast_correction(ss << " "); - } - - if(long_term_correction_updated) - { - ss << std::endl; - sbas_satelite_correction.print_long_term_correction(ss << " "); - } - - if(fast_correction_updated || long_term_correction_updated) - { - //todo_Update to new GNURadio msg queue - //if(sat_corr_queue != nullptr) sat_corr_queue->push(sbas_satelite_correction); - } - } - VLOG(FLOW) << ss.str(); ss.str(""); - } -} - - - -const double Sbas_Telemetry_Data::gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ - -/* debug trace function -----------------------------------------------------*/ -void Sbas_Telemetry_Data::trace(int level, const char *format, ...) -{ - va_list ap; - char str[1000]; - va_start(ap, format); - vsprintf(str, format, ap); - va_end(ap); - VLOG(FLOW) << "<> " << std::string(str); -} - - - -/* satellite system+prn/slot number to satellite number ------------------------ - * convert satellite system+prn/slot number to satellite number - * args : int sys I satellite system (SYS_GPS,SYS_GLO,...) - * int prn I satellite prn/slot number - * return : satellite number (0:error) - *-----------------------------------------------------------------------------*/ -int Sbas_Telemetry_Data::satno(int sys, int prn) -{ - if (prn <= 0) return 0; - switch (sys) - { - case SYS_GPS: - if (prn < MINPRNGPS || MAXPRNGPS < prn) return 0; - return prn - MINPRNGPS + 1; - case SYS_GLO: - if (prn < MINPRNGLO || MAXPRNGLO < prn) return 0; - return NSATGPS + prn - MINPRNGLO + 1; - case SYS_GAL: - if (prn < MINPRNGAL || MAXPRNGAL < prn) return 0; - return NSATGPS + NSATGLO + prn - MINPRNGAL + 1; - case SYS_QZS: - if (prn < MINPRNQZS || MAXPRNQZS < prn) return 0; - return NSATGPS + NSATGLO + NSATGAL + prn - MINPRNQZS + 1; - case SYS_CMP: - if (prn < MINPRNCMP || MAXPRNCMP < prn) return 0; - return NSATGPS + NSATGLO + NSATGAL + NSATQZS + prn - MINPRNCMP + 1; - case SYS_SBS: - if (prn < MINPRNSBS || MAXPRNSBS < prn) return 0; - return NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATCMP + prn - MINPRNSBS + 1; - } - return 0; -} - -/*! - * \brief Extracts unsigned/signed bits from byte data - * params : unsigned char *buff I byte data - * int pos I bit position from start of data (bits) - * int len I bit length (bits) (len<=32) - * return : extracted unsigned/signed bits - */ -unsigned int Sbas_Telemetry_Data::getbitu(const unsigned char *buff, int pos, int len) -{ - unsigned int bits = 0; - int i; - for (i = pos; i < pos + len; i++) bits = (bits << 1) + ((buff[i/8] >> (7 - i % 8)) & 1u); - return bits; -} - - - -int Sbas_Telemetry_Data::getbits(const unsigned char *buff, int pos, int len) -{ - unsigned int bits = getbitu(buff,pos,len); - if (len <= 0 || 32 <= len || !(bits & (1u << (len - 1)))) return (int)bits; - return (int)(bits|(~0u << len)); /* extend sign */ -} - - - -/* convert calendar day/time to time ------------------------------------------- - * convert calendar day/time to gtime_t struct - * args : double *ep I day/time {year,month,day,hour,min,sec} - * return : gtime_t struct - * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) - *-----------------------------------------------------------------------------*/ -Sbas_Telemetry_Data::gtime_t Sbas_Telemetry_Data::epoch2time(const double *ep) -{ - const int doy[] = {1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}; - gtime_t time = gtime_t(); - int days, sec, year = (int)ep[0], mon = (int)ep[1], day = (int)ep[2]; - - if (year < 1970 || 2099 < year || mon < 1 || 12 < mon) return time; - - /* leap year if year%4==0 in 1901-2099 */ - days = (year - 1970)*365 + (year - 1969)/4 + doy[mon - 1] + day - 2 + (year % 4 == 0 && mon >= 3 ? 1 : 0); - sec = (int)floor(ep[5]); - time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + sec; - time.sec = ep[5] - sec; - return time; -} - - - - -/* time difference ------------------------------------------------------------- - * difference between gtime_t structs - * args : gtime_t t1,t2 I gtime_t structs - * return : time difference (t1-t2) (s) - *-----------------------------------------------------------------------------*/ -double Sbas_Telemetry_Data::timediff(gtime_t t1, gtime_t t2) -{ - return difftime(t1.time, t2.time) + t1.sec - t2.sec; -} - - - -/* gps time to time ------------------------------------------------------------ - * convert week and tow in gps time to gtime_t struct - * args : int week I week number in gps time - * double sec I time of week in gps time (s) - * return : gtime_t struct - *-----------------------------------------------------------------------------*/ -Sbas_Telemetry_Data::gtime_t Sbas_Telemetry_Data::gpst2time(int week, double sec) -{ - gtime_t t = epoch2time(gpst0); - if (sec < -1E9 || 1E9 < sec) sec = 0.0; - t.time += 86400*7*week + (int)sec; - t.sec = sec - (int)sec; - return t; -} - - - -/* sbas igp definition -------------------------------------------------------*/ -const short -Sbas_Telemetry_Data::x1[] = {-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, - 25, 30, 35, 40, 45, 50, 55, 65, 75, 85}, -Sbas_Telemetry_Data::x2[] = {-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, - 35, 40, 45, 50, 55}, -Sbas_Telemetry_Data::x3[] = {-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, - 25, 30, 35, 40, 45, 50, 55, 65, 75}, -Sbas_Telemetry_Data::x4[] = {-85,-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, - 20, 25, 30, 35, 40, 45, 50, 55, 65, 75}, -Sbas_Telemetry_Data::x5[] = {-180,-175,-170,-165,-160,-155,-150,-145,-140,-135,-130,-125,-120,-115, - -110,-105,-100,- 95,- 90,- 85,- 80,- 75,- 70,- 65,- 60,- 55,- 50,- 45, - - 40,- 35,- 30,- 25,- 20,- 15,- 10,- 5, 0, 5, 10, 15, 20, 25, - 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, - 100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, - 170, 175}, -Sbas_Telemetry_Data::x6[] = {-180,-170,-160,-150,-140,-130,-120,-110,-100,- 90,- 80,- 70,- 60,- 50, - -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, - 100, 110, 120, 130, 140, 150, 160, 170}, -Sbas_Telemetry_Data::x7[] = {-180,-150,-120,- 90,- 60,- 30, 0, 30, 60, 90, 120, 150}, -Sbas_Telemetry_Data::x8[] = {-170,-140,-110,- 80,- 50,- 20, 10, 40, 70, 100, 130, 160}; - -const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband1[9][8] = { /* band 0-8 */ - {{-180,x1, 1, 28},{-175,x2, 29, 51},{-170,x3, 52, 78},{-165,x2, 79,101}, - {-160,x3,102,128},{-155,x2,129,151},{-150,x3,152,178},{-145,x2,179,201}}, - {{-140,x4, 1, 28},{-135,x2, 29, 51},{-130,x3, 52, 78},{-125,x2, 79,101}, - {-120,x3,102,128},{-115,x2,129,151},{-110,x3,152,178},{-105,x2,179,201}}, - {{-100,x3, 1, 27},{- 95,x2, 28, 50},{- 90,x1, 51, 78},{- 85,x2, 79,101}, - {- 80,x3,102,128},{- 75,x2,129,151},{- 70,x3,152,178},{- 65,x2,179,201}}, - {{- 60,x3, 1, 27},{- 55,x2, 28, 50},{- 50,x4, 51, 78},{- 45,x2, 79,101}, - {- 40,x3,102,128},{- 35,x2,129,151},{- 30,x3,152,178},{- 25,x2,179,201}}, - {{- 20,x3, 1, 27},{- 15,x2, 28, 50},{- 10,x3, 51, 77},{- 5,x2, 78,100}, - { 0,x1,101,128},{ 5,x2,129,151},{ 10,x3,152,178},{ 15,x2,179,201}}, - {{ 20,x3, 1, 27},{ 25,x2, 28, 50},{ 30,x3, 51, 77},{ 35,x2, 78,100}, - { 40,x4,101,128},{ 45,x2,129,151},{ 50,x3,152,178},{ 55,x2,179,201}}, - {{ 60,x3, 1, 27},{ 65,x2, 28, 50},{ 70,x3, 51, 77},{ 75,x2, 78,100}, - { 80,x3,101,127},{ 85,x2,128,150},{ 90,x1,151,178},{ 95,x2,179,201}}, - {{ 100,x3, 1, 27},{ 105,x2, 28, 50},{ 110,x3, 51, 77},{ 115,x2, 78,100}, - { 120,x3,101,127},{ 125,x2,128,150},{ 130,x4,151,178},{ 135,x2,179,201}}, - {{ 140,x3, 1, 27},{ 145,x2, 28, 50},{ 150,x3, 51, 77},{ 155,x2, 78,100}, - { 160,x3,101,127},{ 165,x2,128,150},{ 170,x3,151,177},{ 175,x2,178,200}} -}; - - - -const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband2[2][5] = { /* band 9-10 */ - {{ 60,x5, 1, 72},{ 65,x6, 73,108},{ 70,x6,109,144},{ 75,x6,145,180}, - { 85,x7,181,192}}, - {{- 60,x5, 1, 72},{- 65,x6, 73,108},{- 70,x6,109,144},{- 75,x6,145,180}, - {- 85,x8,181,192}} -}; - - -/* decode type 1: prn masks --------------------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, n, sat; - // see figure A-6: i corresponds to bit number (and for the GPS satellites is identically to the PRN), n to the PRN mask number - - trace(4, "decode_sbstype1:"); - - for (i = 1, n = 0; i <= 210 && n < MAXSAT; i++) - { - if (getbitu(msg->msg, 13 + i, 1)) - { - if (i <= 37) sat = satno(SYS_GPS, i); /* 0 - 37: gps */ - else if (i <= 61) sat = satno(SYS_GLO, i - 37); /* 38 - 61: glonass */ - else if (i <= 119) sat = 0; /* 62 - 119: future gnss */ - else if (i <= 138) sat = satno(SYS_SBS, i); /* 120 - 138: geo/waas */ - else if (i <= 182) sat = 0; /* 139 - 182: reserved */ - else if (i <= 192) sat = satno(SYS_SBS, i + 10); /* 183 - 192: qzss ref [2] */ - else if (i <= 202) sat = satno(SYS_QZS, i); /* 193 - 202: qzss ref [2] */ - else sat = 0; /* 203 - : reserved */ - sbssat->sat[n++].sat = sat; - } - } - // TODO consider the use of the old prn mask in the transition phase such that old data sets still can be used - int new_iodp = getbitu(msg->msg, 224, 2); - if (sbssat->iodp != new_iodp) prn_mask_changed(); // invalidate all satellite corrections - sbssat->iodp = new_iodp; - sbssat->nsat = n; - - trace(5, "decode_sbstype1: nprn=%d iodp=%d", n, sbssat->iodp); - return 1; -} - - -/* decode type 2-5,0: fast corrections ---------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, j, iodf, type, udrei; - double prc, dt; - double t0_past; - - trace(4,"decode_sbstype2:"); - - if (sbssat->iodp != (int)getbitu(msg->msg, 16, 2)) return 0; - - type = getbitu(msg->msg, 8, 6); - iodf = getbitu(msg->msg, 14, 2); - - for (i=0; i<13; i++) - { - if ((j = 13*((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat) break; - udrei = getbitu(msg->msg, 174 + 4*i, 4); - t0_past = sbssat->sat[j].fcorr.t0; - prc = sbssat->sat[j].fcorr.prc; - sbssat->sat[j].fcorr.t0 = msg->sample_stamp; - sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i*12, 12)*0.125f; - sbssat->sat[j].fcorr.udre = udrei + 1; - dt = sbssat->sat[j].fcorr.t0 - t0_past; - if (!sbssat->sat[j].fcorr.valid || dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0) - { - sbssat->sat[j].fcorr.rrc = 0.0; - sbssat->sat[j].fcorr.dt = 0.0; - } - else - { - sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc - prc)/dt; - sbssat->sat[j].fcorr.dt = dt; - } - sbssat->sat[j].fcorr.valid = true; - sbssat->sat[j].fcorr.iodf = iodf; - } - trace(5, "decode_sbstype2: type=%d iodf=%d", type, iodf); - return 1; -} - - - - -/* decode type 6: integrity info ---------------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, iodf[4], udrei; - - trace(4, "decode_sbstype6:"); - - if(sbssat->iodp < 0) return 0; - - for (i=0; i<4; i++) - { - iodf[i] = getbitu(msg->msg, 14 + i*2, 2); - } - for (i=0; i < sbssat->nsat && i < MAXSAT; i++) - { - if (!sbssat->sat[i].fcorr.valid || sbssat->sat[i].fcorr.iodf != iodf[i/13]) continue; - udrei = getbitu(msg->msg, 22 + i*4, 4); - sbssat->sat[i].fcorr.udre = udrei + 1; - } - trace(5, "decode_sbstype6: iodf=%d %d %d %d", iodf[0], iodf[1], iodf[2], iodf[3]); - return 1; -} - - - -/* decode type 7: fast correction degradation factor -------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i; - trace(4,"decode_sbstype7"); - if (sbssat->iodp != (int)getbitu(msg->msg, 18, 2)) return 0; - sbssat->tlat = getbitu(msg->msg, 14, 4); - for (i=0; i < sbssat->nsat && i < MAXSAT; i++) - { - sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22 + i*4, 4); - } - return 1; -} - - - -/* decode type 9: geo navigation message -------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) -{ - seph_t seph; - int i; - int sat; - - trace(4,"decode_sbstype9:"); - - if (!(sat = satno(SYS_SBS, msg->prn))) - { - trace(2, "invalid prn in sbas type 9: prn=%3d", msg->prn); - return 0; - } - /*t=(int)getbitu(msg->msg,22,13)*16-(int)msg->tow%86400; - if (t<=-43200) t+=86400; - else if (t> 43200) t-=86400;*/ - //seph.t0 =gpst2time(msg->week,msg->tow+t); - seph.sat = sat; - seph.t0 = getbitu(msg->msg, 22, 13)*16; - seph.tof = msg->sample_stamp; - seph.sva = getbitu(msg->msg, 35, 4); - seph.svh = seph.sva == 15 ? 1 : 0; /* unhealthy if ura==15 */ - - seph.pos[0] = getbits(msg->msg, 39, 30)*0.08; - seph.pos[1] = getbits(msg->msg, 69, 30)*0.08; - seph.pos[2] = getbits(msg->msg, 99, 25)*0.4; - seph.vel[0] = getbits(msg->msg, 124, 17)*0.000625; - seph.vel[1] = getbits(msg->msg, 141, 17)*0.000625; - seph.vel[2] = getbits(msg->msg, 158, 18)*0.004; - seph.acc[0] = getbits(msg->msg, 176, 10)*0.0000125; - seph.acc[1] = getbits(msg->msg, 186, 10)*0.0000125; - seph.acc[2] = getbits(msg->msg, 196, 10)*0.0000625; - - seph.af0 = getbits(msg->msg, 206, 12)*P2_31; - seph.af1 = getbits(msg->msg, 218, 8)*P2_39/2.0; - - i = msg->prn-MINPRNSBS; - if (std::abs(nav->seph[i].t0 - seph.t0) < 1E-3) - { /* not change */ - VLOG(FLOW) << "<> no change in ephemeris -> won't parse"; - return 0; - } - nav->seph[NSATSBS + i] = nav->seph[i]; /* previous */ - nav->seph[i] = seph; /* current */ - - trace(5, "decode_sbstype9: prn=%d", msg->prn); - return 1; -} - - - -/* decode type 18: ionospheric grid point masks ------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion) -{ - const sbsigpband_t *p; - int i, j, n, m, band = getbitu(msg->msg, 18, 4); - - trace(4, "decode_sbstype18:"); - - if (0 <= band && band <= 8) {p = igpband1[band]; m = 8;} - else if (9 <= band && band <= 10) {p = igpband2[band - 9]; m = 5;} - else return 0; - - short iodi_new = (short)getbitu(msg->msg, 22, 2); - if(sbsion[band].iodi != iodi_new) - { - // IGP mask changed -> invalidate all IGPs in this band - igp_mask_changed(band); - } - sbsion[band].iodi = iodi_new; - - for (i=1, n=0; i <= 201; i++) - { - if (!getbitu(msg->msg, 23 + i, 1)) continue; - for (j = 0; j < m; j++) - { - if (i < p[j].bits || p[j].bite < i) continue; - sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x; - sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits]; - break; - } - } - sbsion[band].nigp = n; - - trace(5, "decode_sbstype18: band=%d nigp=%d", band, n); - return 1; -} - - - - -/* decode half long term correction (vel code=0) -----------------------------*/ -int Sbas_Telemetry_Data::decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat) -{ - int i, n = getbitu(msg->msg, p, 6); - - trace(4, "decode_longcorr0:"); - - if (n == 0 || n > MAXSAT) return 0; - - sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8); - - for (i = 0; i < 3; i++) - { - sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + 9*i, 9)*0.125; - sbssat->sat[n - 1].lcorr.dvel[i] = 0.0; - } - sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 41, 10)*P2_31; - sbssat->sat[n - 1].lcorr.daf1 = 0.0; - //sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow); - sbssat->sat[n - 1].lcorr.trx = msg->sample_stamp; // vel=0 -> time of applicability is reception time - sbssat->sat[n - 1].lcorr.tapp = 0; - sbssat->sat[n - 1].lcorr.valid = true; - - trace(5, "decode_longcorr0:sat=%2d", sbssat->sat[n - 1].sat); - return 1; -} - - - -/* decode half long term correction (vel code=1) -----------------------------*/ -int Sbas_Telemetry_Data::decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat) -{ - int i; - int n = getbitu(msg->msg, p, 6); - - trace(4,"decode_longcorr1:"); - - if (n == 0 || n > MAXSAT) return 0; - - sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8); - - for (i=0; i<3; i++) - { - sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + i*11, 11)*0.125; - sbssat->sat[n - 1].lcorr.dvel[i] = getbits(msg->msg, p + 58 + i*8, 8)*P2_11; - } - sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 47, 11)*P2_31; - sbssat->sat[n - 1].lcorr.daf1 = getbits(msg->msg, p + 82, 8)*P2_39; - // vel=1 -> time of applicability is sent in message, needs to be corrected for rollover which can not be done here, since the absolute gps time is unknown. see IS-GPS-200G pdf page 116 for correction - /*t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400; - if (t<=-43200) t+=86400; - else if (t> 43200) t-=86400; - sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t);*/ - sbssat->sat[n - 1].lcorr.trx = msg->sample_stamp; - sbssat->sat[n - 1].lcorr.tapp = (int)getbitu(msg->msg, p + 90, 13)*16; - sbssat->sat[n - 1].lcorr.vel = 1; - sbssat->sat[n - 1].lcorr.valid = true; - trace(5, "decode_longcorr1: sat=%2d", sbssat->sat[n - 1].sat); - return 1; -} - - - - -/* decode half long term correction ------------------------------------------*/ -int Sbas_Telemetry_Data::decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat) -{ - trace(4,"decode_longcorrh:"); - - if (getbitu(msg->msg, p, 1) == 0 ) - { - /* vel code=0 */ - if (sbssat->iodp == (int)getbitu(msg->msg, p + 103, 2)) - { - return decode_longcorr0(msg, p + 1, sbssat) && decode_longcorr0(msg, p + 52, sbssat); - } - } - else if (sbssat->iodp == (int)getbitu(msg->msg, p + 104, 2)) - { - return decode_longcorr1(msg, p + 1, sbssat); - } - return 0; -} - - - - -/* decode type 24: mixed fast/long term correction ---------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, j, iodf, blk, udre; - - trace(4, "decode_sbstype24:"); - - if (sbssat->iodp != (int)getbitu(msg->msg, 110, 2)) return 0; /* check IODP */ - - blk = getbitu(msg->msg, 112, 2); - iodf =getbitu(msg->msg, 114, 2); - - for (i=0; i<6; i++) - { - if ((j = 13*blk+i) >= sbssat->nsat) break; - udre = getbitu(msg->msg, 86 + 4*i, 4); - - //sbssat->sat[j].fcorr.t0 =gpst2time(msg->week,msg->tow); - sbssat->sat[j].fcorr.t0 = msg->sample_stamp; - sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14 + i*12, 12)*0.125f; - sbssat->sat[j].fcorr.udre = udre + 1; - sbssat->sat[j].fcorr.iodf = iodf; - } - return decode_longcorrh(msg, 120, sbssat); -} - - - - -/* decode type 25: long term satellite error correction ----------------------*/ -int Sbas_Telemetry_Data::decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - trace(4,"decode_sbstype25:"); - return decode_longcorrh(msg, 14, sbssat) && decode_longcorrh(msg, 120, sbssat); -} - - - - -/* decode type 26: ionospheric delay corrections -----------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion) -{ - int i, j, block, delay, give, band = getbitu(msg->msg, 14, 4); - - trace(4, "decode_sbstype26:"); - - if (band > MAXBAND || sbsion[band].iodi != (int)getbitu(msg->msg, 217, 2)) return 0; - - block = getbitu(msg->msg, 18, 4); - - for (i = 0; i < 15; i++) - { - if ((j = block*15 + i) >= sbsion[band].nigp) continue; - give = getbitu(msg->msg, 2 + i*13 + 9, 4); - delay = getbitu(msg->msg, 22 + i*13, 9); - //sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow); - sbsion[band].igp[j].t0 = msg->sample_stamp; - sbsion[band].igp[j].valid = true; - sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0f : delay*0.125f; - sbsion[band].igp[j].give = give; - - if(sbsion[band].igp[j].give > 15) sbsion[band].igp[j].give = 15; // give is not higher than 15, but to be sure - } - trace(5, "decode_sbstype26: band=%d block=%d", band, block); - return 1; -} - - - - -/* update sbas corrections ----------------------------------------------------- - * update sbas correction parameters in navigation data with a sbas message - * args : sbsmg_t *msg I sbas message - * nav_t *nav IO navigation data - * return : message type (-1: error or not supported type) - * notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) - * seph[prn-MINPRNSBS+1] : sat prn current epehmeris - * seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris - *-----------------------------------------------------------------------------*/ -int Sbas_Telemetry_Data::sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav) -{ - int type = getbitu(msg->msg, 8, 6), stat = -1; - - trace(3,"sbsupdatecorr: type=%d",type); - - //if (msg->week==0) return -1; - - switch (type) - { - case 0: stat = decode_sbstype2(msg, &nav->sbssat); break; - case 1: stat = decode_sbstype1(msg, &nav->sbssat); break; - case 2: - case 3: - case 4: - case 5: stat = decode_sbstype2(msg, &nav->sbssat); break; - case 6: stat = decode_sbstype6(msg, &nav->sbssat); break; - case 7: stat = decode_sbstype7(msg, &nav->sbssat); break; - case 9: stat = decode_sbstype9(msg, nav); break; - case 10: trace(2, "unsupported sbas message: type=%d", type); break; - case 12: trace(2, "unsupported sbas message: type=%d", type); break; - case 17: trace(2, "unsupported sbas message: type=%d", type); break; - case 18: stat = decode_sbstype18(msg, nav ->sbsion); break; - case 24: stat = decode_sbstype24(msg, &nav->sbssat); break; - case 25: stat = decode_sbstype25(msg, &nav->sbssat); break; - case 26: stat = decode_sbstype26(msg, nav ->sbsion); break; - case 63: break; /* null message */ - - default: trace(2, "Unsupported SBAS message: type=%d", type); break; - } - return stat ? type : -1; -} - - -void Sbas_Telemetry_Data::prn_mask_changed() -{ - d_nav.sbssat.tlat = -1; - // for each satellite in the RTKLIB structure - for (int i_sat = 0; i_sat < d_nav.sbssat.nsat; i_sat++) - { - d_nav.sbssat.sat[i_sat].fcorr.valid = false; - d_nav.sbssat.sat[i_sat].lcorr.valid = false; - } -} - - -bool Sbas_Telemetry_Data::is_rtklib_sat_correction_valid(int sat) -{ - return d_nav.sbssat.tlat > -1 - && d_nav.sbssat.sat[sat].fcorr.valid - && d_nav.sbssat.sat[sat].lcorr.valid; -} - - -void Sbas_Telemetry_Data::igp_mask_changed(int band) -{ - for(int i_igp = 0; i_igp < d_nav.sbsion[band].nigp; i_igp++) - d_nav.sbsion[band].igp[i_igp].valid = false; -} diff --git a/src/core/system_parameters/sbas_telemetry_data.h b/src/core/system_parameters/sbas_telemetry_data.h deleted file mode 100644 index 9f75dee07..000000000 --- a/src/core/system_parameters/sbas_telemetry_data.h +++ /dev/null @@ -1,492 +0,0 @@ -/*! - * \file sbas_telemetry_data.h - * \brief Interface of the SBAS telemetry parser based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_SBAS_TELEMETRY_DATA_H_ -#define GNSS_SDR_SBAS_TELEMETRY_DATA_H_ - -#include -#include -#include -#include -#include -#include -#include -#include "boost/assign.hpp" -#include "concurrent_queue.h" -#include "sbas_time.h" - - -class Sbas_Ionosphere_Correction; -class Sbas_Satellite_Correction; -struct Fast_Correction; -struct Long_Term_Correction; -class Sbas_Ephemeris; - - -/*! - * \brief Represents a raw SBAS message of 250cbits + 6 bits padding - * (8b preamble + 6b message type + 212b data + 24b CRC + 6b zero padding) - */ -class Sbas_Raw_Msg -{ -public: - Sbas_Raw_Msg(){ rx_time = Sbas_Time(0); i_prn = -1; }; - Sbas_Raw_Msg(double sample_stamp, int prn, const std::vector msg) : rx_time(sample_stamp), i_prn(prn), d_msg(msg) {} - double get_sample_stamp() { return rx_time.get_time_stamp(); } //!< Time of reception sample stamp (first sample of preample) - void relate(Sbas_Time_Relation time_relation) - { - rx_time.relate(time_relation); - } - Sbas_Time get_rx_time_obj() const { return rx_time; } - int get_prn() const { return i_prn; } - std::vector get_msg() const { return d_msg; } - int get_preamble() - { - return d_msg[0]; - } - int get_msg_type() const - { - return d_msg[1] >> 2; - } - int get_crc() - { - unsigned char crc_last_byte = (d_msg[30] << 2) && (d_msg[31] >> 6); - unsigned char crc_middle_byte = (d_msg[29] << 2) && (d_msg[30] >> 6); - unsigned char crc_first_byte = (d_msg[28] << 2) && (d_msg[29] >> 6); - return ((unsigned int)(crc_first_byte) << 16) && ((unsigned int)(crc_middle_byte) << 8) && crc_last_byte; - } -private: - Sbas_Time rx_time; - int i_prn; /* SBAS satellite PRN number */ - std::vector d_msg; /* SBAS message (226 bit) padded by 0 */ -}; - - - - -/* - * \brief Holds an updated set of the telemetry data received from SBAS - */ -class Sbas_Telemetry_Data -{ -public: - int update(Sbas_Raw_Msg sbas_raw_msg); - - /*! - * Default constructor - */ - Sbas_Telemetry_Data(); - /*! - * Default deconstructor - */ - ~Sbas_Telemetry_Data(); - -private: - std::map emitted_fast_corrections; - std::map emitted_long_term_corrections; - - Sbas_Time_Relation mt12_time_ref; - - int decode_mt12(Sbas_Raw_Msg sbas_raw_msg); - - void updated_sbas_ephemeris(Sbas_Raw_Msg msg); - void received_iono_correction(); - void updated_satellite_corrections(); - - /////// rtklib.h - -#define SYS_NONE 0x00 /* navigation system: none */ -#define SYS_GPS 0x01 /* navigation system: GPS */ -#define SYS_SBS 0x02 /* navigation system: SBAS */ -#define SYS_GLO 0x04 /* navigation system: GLONASS */ -#define SYS_GAL 0x08 /* navigation system: Galileo */ -#define SYS_QZS 0x10 /* navigation system: QZSS */ -#define SYS_CMP 0x20 /* navigation system: BeiDou */ -#define SYS_ALL 0xFF /* navigation system: all */ - -#define TSYS_GPS 0 /* time system: GPS time */ -#define TSYS_UTC 1 /* time system: UTC */ -#define TSYS_GLO 2 /* time system: GLONASS time */ -#define TSYS_GAL 3 /* time system: Galileo time */ -#define TSYS_QZS 4 /* time system: QZSS time */ -#define TSYS_CMP 5 /* time system: BeiDou time */ - -#ifndef NFREQ -#define NFREQ 3 /* number of carrier frequencies */ -#endif -#define NFREQGLO 2 /* number of carrier frequencies of GLONASS */ - -#ifndef NEXOBS -#define NEXOBS 0 /* number of extended obs codes */ -#endif - -#define MINPRNGPS 1 /* min satellite PRN number of GPS */ -#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ -#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */ -#define NSYSGPS 1 - -#ifdef ENAGLO -#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ -#define MAXPRNGLO 24 /* max satellite slot number of GLONASS */ -#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */ -#define NSYSGLO 1 -#else -#define MINPRNGLO 0 -#define MAXPRNGLO 0 -#define NSATGLO 0 -#define NSYSGLO 0 -#endif -#ifdef ENAGAL -#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ -#define MAXPRNGAL 27 /* max satellite PRN number of Galileo */ -#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */ -#define NSYSGAL 1 -#else -#define MINPRNGAL 0 -#define MAXPRNGAL 0 -#define NSATGAL 0 -#define NSYSGAL 0 -#endif -#ifdef ENAQZS -#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ -#define MAXPRNQZS 195 /* max satellite PRN number of QZSS */ -#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */ -#define MAXPRNQZS_S 185 /* max satellite PRN number of QZSS SAIF */ -#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */ -#define NSYSQZS 1 -#else -#define MINPRNQZS 0 -#define MAXPRNQZS 0 -#define NSATQZS 0 -#define NSYSQZS 0 -#endif -#ifdef ENACMP -#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ -#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */ -#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */ -#define NSYSCMP 1 -#else -#define MINPRNCMP 0 -#define MAXPRNCMP 0 -#define NSATCMP 0 -#define NSYSCMP 0 -#endif -#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP) /* number of systems */ - -#define MINPRNSBS 120 /* min satellite PRN number of SBAS */ -#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */ -#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */ - -#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATSBS) - /* max satellite number (1 to MAXSAT) */ -#ifndef MAXOBS -#define MAXOBS 64 /* max number of obs in an epoch */ -#endif -#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */ -#define MAXOBSTYPE 64 /* max number of obs type in RINEX */ -#define DTTOL 0.005 /* tolerance of time difference (s) */ -#if 0 -#define MAXDTOE 10800.0 /* max time difference to ephem Toe (s) for GPS */ -#else -#define MAXDTOE 7200.0 /* max time difference to ephem Toe (s) for GPS */ -#endif -#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */ -#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */ -#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */ -#define MAXGDOP 300.0 /* max GDOP */ - - //#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ - //#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ - //#define MAXSBSURA 8 /* max URA of SBAS satellite */ -#define MAXBAND 10 /* max SBAS band of IGP */ -#define MAXNIGP 201 /* max number of IGP in SBAS band */ - //#define MAXNGEO 4 /* max number of GEO satellites */ - - -#define P2_11 4.882812500000000E-04 /* 2^-11 */ -#define P2_31 4.656612873077393E-10 /* 2^-31 */ -#define P2_39 1.818989403545856E-12 /* 2^-39 */ - - /* type definitions ----------------------------------------------------------*/ - - typedef struct { /* time struct */ - time_t time; /* time (s) expressed by standard time_t */ - double sec; /* fraction of second under 1 s */ - } gtime_t; - - typedef struct { /* SBAS message type */ - //int week,tow; /* receiption time */ - double sample_stamp; - int prn; /* SBAS satellite PRN number */ - unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ - } sbsmsg_t; - - typedef struct { /* SBAS messages type */ - int n,nmax; /* number of SBAS messages/allocated */ - sbsmsg_t *msgs; /* SBAS messages */ - } sbs_t; - - typedef struct { /* SBAS fast correction type */ - //gtime_t t0; /* time of applicability (TOF) */ - double t0; - bool valid; - double prc; /* pseudorange correction (PRC) (m) */ - double rrc; /* range-rate correction (RRC) (m/s) */ - double dt; /* range-rate correction delta-time (s) */ - int iodf; /* IODF (issue of date fast corr) */ - short udre; /* UDRE+1 */ - short ai; /* degradation factor indicator */ - } sbsfcorr_t; - - typedef struct { /* SBAS long term satellite error correction type */ - //gtime_t t0; /* correction time */ - double trx; /* time when message was received */ - int tapp; /* time of applicability (when vel=1 sent as t0) */ - int vel; /* use velocity if vel=1 */ - bool valid; - int iode; /* IODE (issue of date ephemeris) */ - double dpos[3]; /* delta position (m) (ecef) */ - double dvel[3]; /* delta velocity (m/s) (ecef) */ - double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ - } sbslcorr_t; - - typedef struct { /* SBAS satellite correction type */ - int sat; /* satellite number */ - sbsfcorr_t fcorr; /* fast correction */ - sbslcorr_t lcorr; /* long term correction */ - } sbssatp_t; - - typedef struct { /* SBAS satellite corrections type */ - int iodp; /* IODP (issue of date mask) */ - int nsat; /* number of satellites */ - int tlat; /* system latency (s) */ - sbssatp_t sat[MAXSAT]; /* satellite correction */ - } sbssat_t; - - typedef struct { /* SBAS ionospheric correction type */ - //gtime_t t0; /* correction time */ - double t0; - bool valid; - short lat,lon; /* latitude/longitude (deg) */ - short give; /* GIVI+1 */ - float delay; /* vertical delay estimate (m) */ - } sbsigp_t; - - typedef struct { /* IGP band type */ - short x; /* longitude/latitude (deg) */ - const short *y; /* latitudes/longitudes (deg) */ - unsigned char bits; /* IGP mask start bit */ - unsigned char bite; /* IGP mask end bit */ - } sbsigpband_t; - - typedef struct { /* SBAS ionospheric corrections type */ - int iodi; /* IODI (issue of date ionos corr) */ - int nigp; /* number of igps */ - sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ - } sbsion_t; - - /* - * indicators - */ - - typedef struct { /* SBAS ephemeris type */ - int sat = 0; /* satellite number */ - //gtime_t t0; /* reference epoch time (GPST) */ - int t0 = 0; - //gtime_t tof; /* time of message frame (GPST) */ - double tof = 0; - int sva = 0; /* SV accuracy (URA index) */ - int svh = 0; /* SV health (0:ok) */ - double pos[3] = {0, 0, 0}; /* satellite position (m) (ecef) */ - double vel[3] = {0, 0, 0}; /* satellite velocity (m/s) (ecef) */ - double acc[3] = {0, 0, 0}; /* satellite acceleration (m/s^2) (ecef) */ - double af0 = 0; - double af1 = 0; /* satellite clock-offset/drift (s,s/s) */ - } seph_t; - - typedef struct { /* navigation data type */ - //int n,nmax; /* number of broadcast ephemeris */ - //int ng,ngmax; /* number of glonass ephemeris */ - //int ns,nsmax; /* number of sbas ephemeris */ - //int ne,nemax; /* number of precise ephemeris */ - //int nc,ncmax; /* number of precise clock */ - //int na,namax; /* number of almanac data */ - //int nt,ntmax; /* number of tec grid data */ - //int nn,nnmax; /* number of stec grid data */ - //eph_t *eph; /* GPS/QZS/GAL ephemeris */ - //geph_t *geph; /* GLONASS ephemeris */ - seph_t seph[2*NSATSBS]; /* SBAS ephemeris */ - // peph_t *peph; /* precise ephemeris */ - // pclk_t *pclk; /* precise clock */ - // alm_t *alm; /* almanac data */ - // tec_t *tec; /* tec grid data */ - // stec_t *stec; /* stec grid data */ - // erp_t erp; /* earth rotation parameters */ - - //double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */ - //double utc_glo[4]; /* GLONASS UTC GPS time parameters */ - //double utc_gal[4]; /* Galileo UTC GPS time parameters */ - //double utc_qzs[4]; /* QZS UTC GPS time parameters */ - //double utc_cmp[4]; /* BeiDou UTC parameters */ - //double utc_sbs[4]; /* SBAS UTC parameters */ - //double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ - //double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */ - //double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ - //double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ - //int leaps; /* leap seconds (s) */ - //double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */ - //double cbias[MAXSAT][3]; /* code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ - //double wlbias[MAXSAT]; /* wide-lane bias (cycle) */ - //double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */ - //char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */ - // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ - sbssat_t sbssat; /* SBAS satellite corrections */ - sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */ - // dgps_t dgps[MAXSAT]; /* DGPS corrections */ - // ssr_t ssr[MAXSAT]; /* SSR corrections */ - // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */ - // lexion_t lexion; /* LEX ionosphere correction */ - } nav_t; - - //// common - - static const double gpst0[]; /* gps time reference */ - - /* debug trace functions -----------------------------------------------------*/ - - FILE *fp_trace; /* file pointer of trace */ - int level_trace; /* level of trace */ - unsigned int tick_trace; /* tick time at traceopen (ms) */ - - void trace(int level, const char *format, ...); - - /* satellite system+prn/slot number to satellite number ------------------------ - * convert satellite system+prn/slot number to satellite number - * args : int sys I satellite system (SYS_GPS,SYS_GLO,...) - * int prn I satellite prn/slot number - * return : satellite number (0:error) - *-----------------------------------------------------------------------------*/ - int satno(int sys, int prn); - - /* extract unsigned/signed bits ------------------------------------------------ - * extract unsigned/signed bits from byte data - * args : unsigned char *buff I byte data - * int pos I bit position from start of data (bits) - * int len I bit length (bits) (len<=32) - * return : extracted unsigned/signed bits - *-----------------------------------------------------------------------------*/ - unsigned int getbitu(const unsigned char *buff, int pos, int len); - - int getbits(const unsigned char *buff, int pos, int len); - - /* convert calendar day/time to time ------------------------------------------- - * convert calendar day/time to gtime_t struct - * args : double *ep I day/time {year,month,day,hour,min,sec} - * return : gtime_t struct - * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) - *-----------------------------------------------------------------------------*/ - gtime_t epoch2time(const double *ep); - - /* time difference ------------------------------------------------------------- - * difference between gtime_t structs - * args : gtime_t t1,t2 I gtime_t structs - * return : time difference (t1-t2) (s) - *-----------------------------------------------------------------------------*/ - double timediff(gtime_t t1, gtime_t t2); - - /* gps time to time ------------------------------------------------------------ - * convert week and tow in gps time to gtime_t struct - * args : int week I week number in gps time - * double sec I time of week in gps time (s) - * return : gtime_t struct - *-----------------------------------------------------------------------------*/ - gtime_t gpst2time(int week, double sec); - - - ////// sbas.c - - /* sbas igp definition -------------------------------------------------------*/ - static const short - x1[], - x2[], - x3[], - x4[], - x5[], - x6[], - x7[], - x8[]; - - static const sbsigpband_t igpband1[9][8]; /* band 0-8 */ - static const sbsigpband_t igpband2[2][5]; /* band 9-10 */ - - /* decode type 1: prn masks --------------------------------------------------*/ - int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 2-5,0: fast corrections ---------------------------------------*/ - int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 6: integrity info ---------------------------------------------*/ - int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 7: fast correction degradation factor -------------------------*/ - int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 9: geo navigation message -------------------------------------*/ - int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav); - /* decode type 18: ionospheric grid point masks ------------------------------*/ - int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion); - /* decode half long term correction (vel code=0) -----------------------------*/ - int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat); - /* decode half long term correction (vel code=1) -----------------------------*/ - int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat); - /* decode half long term correction ------------------------------------------*/ - int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat); - /* decode type 24: mixed fast/long term correction ---------------------------*/ - int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 25: long term satellite error correction ----------------------*/ - int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 26: ionospheric deley corrections -----------------------------*/ - int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion); - /* update sbas corrections ----------------------------------------------------- - * update sbas correction parameters in navigation data with a sbas message - * args : sbsmg_t *msg I sbas message - * nav_t *nav IO navigation data - * return : message type (-1: error or not supported type) - * notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) - * seph[prn-MINPRNSBS+1] : sat prn current epehmeris - * seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris - *-----------------------------------------------------------------------------*/ - int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); - - void prn_mask_changed(); - bool is_rtklib_sat_correction_valid(int sat); - void igp_mask_changed(int band); - - // RTKLIB SBAS telemetry data representation - nav_t d_nav; -}; - -#endif diff --git a/src/core/system_parameters/sbas_time.h b/src/core/system_parameters/sbas_time.h deleted file mode 100644 index 3c86efa11..000000000 --- a/src/core/system_parameters/sbas_time.h +++ /dev/null @@ -1,193 +0,0 @@ -/*! - * \file sbas_time.h - * \brief Interface and implementation of classes to handle and relate sample stamp and GPS time based time scales - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_SBAS_TIME_H_ -#define GNSS_SDR_SBAS_TIME_H_ - -#include -#include -#include - -#define EVENT 2 // logs important events which don't occur every update() call -#define FLOW 3 // logs the function calls of block processing functions - -class Sbas_Time_Relation -{ -public: - Sbas_Time_Relation() - { - i_gps_week = 0; - d_delta_sec = 0; - b_valid = false; - VLOG(FLOW) << "<> new invalid time relation: i_gps_week=" << i_gps_week << " d_delta_sec=" << d_delta_sec; - } - - Sbas_Time_Relation(double time_stamp_sec, int gps_week, double gps_sec) - { - i_gps_week = gps_week; - d_delta_sec = gps_sec - time_stamp_sec; - b_valid = true; - VLOG(FLOW) << "<> new time relation: i_gps_week=" << i_gps_week << " d_delta_sec=" << d_delta_sec; - } - - bool to_gps_time(double time_stamp_sec, int &gps_week, double &gps_sec) - { - int delta_weeks = int(trunc(time_stamp_sec + d_delta_sec))/604800; - gps_sec = time_stamp_sec + d_delta_sec - delta_weeks*604800; - gps_week = i_gps_week + delta_weeks; - VLOG(FLOW) << "<> to gps time: time_stamp_sec=" << time_stamp_sec << " gps_week=" << gps_week << " gps_sec=" << gps_sec; - return b_valid; - } - - bool to_sample_stamp(int gps_week, double gps_sec, double &time_stamp_sec) - { - time_stamp_sec = (gps_sec - d_delta_sec) + (gps_week - i_gps_week)*604800; - VLOG(FLOW) << "<> to gps time: gps_week=" << gps_week << " gps_sec=" << gps_sec << " time_stamp_sec=" << time_stamp_sec; - return b_valid; - } - - bool is_valid() - { - return b_valid; - } - -private: - int i_gps_week; - double d_delta_sec; - bool b_valid; -}; - - -/*! - * \brief Sbas_Time relates the relative sample stamp time scale with the absolute GPS time scale. - * There are three different states for a Sbas_Time object: - * - only relative time (sample stamp) is known - * - only absolute time (gps time) is known - * - absolute and relative time and their relation is known - */ -class Sbas_Time -{ -public: - enum Sbas_Time_State {RELATIVE, /*ABSOLUTE,*/ RELATED, UNDEFINED}; - - Sbas_Time() - { - e_state = UNDEFINED; - i_gps_week = 0; - d_gps_sec = 0; - d_time_stamp_sec = 0; - } - - Sbas_Time(double time_stamp_sec) - { - d_time_stamp_sec = time_stamp_sec; - i_gps_week = 0; - d_gps_sec = 0; - e_state = RELATIVE; - } - /* - Sbas_Time(int gps_week, double gps_sec) - { - i_gps_week = gps_week; - d_gps_sec = gps_sec; - d_time_stamp_sec = 0; - e_state = ABSOLUTE; - }*/ - Sbas_Time(double time_stamp_sec, Sbas_Time_Relation relation) - { - if(relation.is_valid()) - { // construct a RELATED object - d_time_stamp_sec = time_stamp_sec; - relation.to_gps_time(d_time_stamp_sec, i_gps_week, d_gps_sec); - e_state = RELATED; - } - else - { // construct a RELATIVE object - *this = Sbas_Time(time_stamp_sec); - VLOG(FLOW) << "<> create RELATIVE time (invalid relation): time_stamp_sec=" << time_stamp_sec; - } - } - /*Sbas_Time(int gps_week, double gps_sec, Sbas_Time_Relation relation) - { - i_gps_week = gps_week; - d_gps_sec = gps_sec; - relation.to_sample_stamp(gps_week, gps_sec, d_time_stamp_sec); - e_state = RELATED; - }*/ - - void relate(Sbas_Time_Relation sbas_time_realtion) - { - switch (e_state) - { - case RELATIVE: *this = Sbas_Time(d_time_stamp_sec, sbas_time_realtion); - break; - - //case ABSOLUTE: return Sbas_Time(i_gps_week, d_gps_sec, sbas_time_realtion); - break; - - case RELATED: LOG(INFO) << "Relating an already related Sbas_Time object is not possible"; - break; - - case UNDEFINED: std::cerr << "Sbas_Time object state undefined" << std::endl; - break; - - default: std::cerr << "Sbas_Time object state not known" << std::endl; - break; - } - return; - } - - double get_time_stamp() - { - return d_time_stamp_sec; - //return (e_state == RELATIVE || e_state == RELATED); - } - - bool get_gps_time(int &gps_week, double &gps_sec) - { - gps_week = i_gps_week; - gps_sec = d_gps_sec; - return (/*e_state == ABSOLUTE ||*/ e_state == RELATED); - } - - bool is_only_relativ() { return e_state == RELATIVE; } - //bool is_only_absolute() {return e_state == ABSOLUTE;} - bool is_related() { return e_state == RELATED; } - Sbas_Time_State get_state() { return e_state; } - -private: - Sbas_Time_State e_state; - double d_time_stamp_sec; - int i_gps_week; - double d_gps_sec; -}; - - -#endif /* GNSS_SDR_SBAS_TIME_H_ */ diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 38a53bdd1..b9214aae7 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -58,12 +58,6 @@ #include "galileo_utc_model.h" #include "sbas_ephemeris.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" -#include "sbas_time.h" - - using google::LogMessage; diff --git a/src/utils/front-end-cal/main.cc b/src/utils/front-end-cal/main.cc index 9a445ff2b..ce249aa20 100644 --- a/src/utils/front-end-cal/main.cc +++ b/src/utils/front-end-cal/main.cc @@ -51,6 +51,7 @@ #include #include #include "concurrent_map.h" +#include "concurrent_queue.h" #include "file_configuration.h" #include "gps_l1_ca_pcps_acquisition_fine_doppler.h" #include "gnss_signal.h" @@ -67,11 +68,7 @@ #include "galileo_almanac.h" #include "galileo_iono.h" #include "galileo_utc_model.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" #include "sbas_ephemeris.h" -#include "sbas_time.h" #include "gnss_sdr_supl_client.h"