mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-26 00:46:59 +00:00
Code Issues Releases Wiki Activity

Migration of the internal LS PVT solver to RTKLIB solver in progress. First working version for GPS L1. Removing SBAS duplicated code.

Browse Source
This commit is contained in:
Javier Arribas 2017-04-20 16:10:12 +02:00
parent 807ca24fc2
commit e90a9aa2bf
50 changed files with 12029 additions and 3104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
conf/gnss-sdr_Hybrid_byte_sim.conf
View File

@ -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

2
src/algorithms/PVT/adapters/CMakeLists.txt
View File

@ -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}

2
src/algorithms/PVT/adapters/hybrid_pvt.cc
View File

@ -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
*

2
src/algorithms/PVT/adapters/hybrid_pvt.h
View File

@ -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
*

235
src/algorithms/PVT/adapters/rtklib_pvt.cc Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "rtklib_pvt.h"
#include <glog/logging.h>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/math/common_factor_rt.hpp>
#include <boost/serialization/map.hpp>
#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<int,int> 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<int,Gps_Ephemeris> 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
}

95
src/algorithms/PVT/adapters/rtklib_pvt.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_RTKLIB_PVT_H_
#define GNSS_SDR_RTKLIB_PVT_H_
#include <string>
#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

2
src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt
View File

@ -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}

1
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
View File

@ -40,7 +40,6 @@
#include <gnuradio/gr_complex.h>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include "concurrent_map.h"
using google::LogMessage;

1104
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc Normal file
View File

File diff suppressed because it is too large Load Diff

172
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_RTKLIB_PVT_CC_H
#define GNSS_SDR_RTKLIB_PVT_CC_H
#include <fstream>
#include <utility>
#include <string>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <gnuradio/block.h>
#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> 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<int,int> 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<int,int> 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<int,int> 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<Rinex_Printer> rp;
std::shared_ptr<Kml_Printer> d_kml_dump;
std::shared_ptr<Nmea_Printer> d_nmea_printer;
std::shared_ptr<GeoJSON_Printer> d_geojson_printer;
std::shared_ptr<Rtcm_Printer> d_rtcm_printer;
double d_rx_time;
std::shared_ptr<rtklib_solver> d_ls_pvt;
std::map<int,Gnss_Synchro> gnss_observables_map;
bool observables_pairCompare_min(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b);
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<int,Gps_Ephemeris> 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

18
src/algorithms/PVT/libs/CMakeLists.txt
View File

@ -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}
)

155
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -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<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;
}
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||1E9<sec) sec=0.0;
t.time+=86400*7*week+(int)sec;
t.sec=sec-(int)sec;
return t;
}
void hybrid_ls_pvt::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]);
}
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<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging)
{
std::map<int,Gnss_Synchro>::iterator gnss_observables_iter;

95
src/algorithms/PVT/libs/hybrid_ls_pvt.h
View File

@ -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);

162
src/algorithms/PVT/libs/rinex_printer.cc
View File

@ -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<unsigned char> 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<unsigned char> 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)

3
src/algorithms/PVT/libs/rinex_printer.h
View File

@ -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);

291
src/algorithms/PVT/libs/rtklib_solver.cc Normal file
View File

@ -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 <ul>
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* <li> 2007-2013, T. Takasu
* </ul>
*
* 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 <glog/logging.h>
#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<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging)
{
std::map<int,Gnss_Synchro>::iterator gnss_observables_iter;
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
std::map<int,Gps_CNAV_Ephemeris>::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: "<<rtklib_msg;
d_rx_dt_s = 0; //reset rx time estimation
}else{
b_valid_position=true;
arma::vec rx_position_and_time(4);
rx_position_and_time(0)=old_pvt_sol.rr[0];
rx_position_and_time(1)=old_pvt_sol.rr[1];
rx_position_and_time(2)=old_pvt_sol.rr[2];
rx_position_and_time(3)=old_pvt_sol.dtr[0];
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[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<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(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;
}

104
src/algorithms/PVT/libs/rtklib_solver.h Normal file
View File

@ -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 <ul>
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* <li> 2007-2013, T. Takasu
* </ul>
*
* 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 <fstream>
#include <iostream>
#include <map>
#include <string>
#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<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging);
int d_nchannels; //!< Number of available channels for positioning
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
std::map<int,Gps_CNAV_Ephemeris> 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

1
src/algorithms/libs/CMakeLists.txt
View File

@ -16,6 +16,7 @@
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
add_subdirectory(rtklib)
set(GNSS_SPLIBS_SOURCES
gps_l2c_signal.cc

60
src/algorithms/libs/rtklib/CMakeLists.txt Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
#
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} )

1044
src/algorithms/libs/rtklib/rtklib.h Normal file
View File

File diff suppressed because it is too large Load Diff

210
src/algorithms/libs/rtklib/rtklib_conversions.cc Normal file
View File

@ -0,0 +1,210 @@
/*!
* \file rtklib_conversions.cc
* \brief GNSS-SDR to RTKLIB data structures conversion functions
* \authors <ul>
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* <li> 2007-2013, T. Takasu
* </ul>
*
* 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.toes<tow-302400.0) {rtklib_sat.week++; tow-=604800.0;}
else if (rtklib_sat.toes>tow+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.toes<tow-302400.0) {rtklib_sat.week++; tow-=604800.0;}
else if (rtklib_sat.toes>tow+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.toes<tow-302400.0) {rtklib_sat.week++; tow-=604800.0;}
else if (rtklib_sat.toes>tow+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;
}

65
src/algorithms/libs/rtklib/rtklib_conversions.h Normal file
View File

@ -0,0 +1,65 @@
/*!
* \file rtklib_conversions.h
* \brief GNSS-SDR to RTKLIB data structures conversion functions
* \authors <ul>
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* <li> 2007-2013, T. Takasu
* </ul>
*
* 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_ */

797
src/algorithms/libs/rtklib/rtklib_ephemeris.cc Normal file
View File

@ -0,0 +1,797 @@
/*!
* \file rtklib_ephemeris.cc
* \brief satellite ephemeris and clock functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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<ura
* 2014/12/07 1.10 modify MAXDTOE for qzss,gal and bds
* test max number of iteration for Kepler
* 2015/08/26 1.11 update RTOL_ELPLER 1E-14 -> 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<ura?SQR(6144.0):SQR(ura_value[ura]);
}
/* variance by ura ssr (ref [4]) ---------------------------------------------*/
double var_urassr(int ura)
{
double std;
if (ura<= 0) return SQR(DEFURASSR);
if (ura>=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&&n<MAX_ITER_KEPLER;n++) {
Ek=E; E-=(E-alm->e*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&&n<MAX_ITER_KEPLER;n++) {
Ek=E; E-=(E-eph->e*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)<TSTEP) tt=t;
glorbit(tt,x,geph->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;i<nav->n;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;i<nav->ng;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;i<nav->ns;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;i<nav->sbssat.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)<MAXAGESSR_HRCLK) {
dclk+=ssr->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<n&&i<MAXOBS;i++) {
for (j=0;j<6;j++) rs [j+i*6]=0.0;
for (j=0;j<2;j++) dts[j+i*2]=0.0;
var[i]=0.0; svh[i]=0;
/* search any psuedorange */
for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break;
if (j>=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<n&&i<MAXOBS;i++) {
trace(4,"%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n",
time_str(time[i],6),obs[i].sat,rs[i*6],rs[1+i*6],rs[2+i*6],
dts[i*2]*1E9,var[i],svh[i]);
}
}

125
src/algorithms/libs/rtklib/rtklib_ephemeris.h Normal file
View File

@ -0,0 +1,125 @@
/*!
* \file rtklib_ephemeris.h
* \brief satellite ephemeris and clock functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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<ura
* 2014/12/07 1.10 modify MAXDTOE for qzss,gal and bds
* test max number of iteration for Kepler
* 2015/08/26 1.11 update RTOL_ELPLER 1E-14 -> 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_ */

516
src/algorithms/libs/rtklib/rtklib_ionex.cc Normal file
View File

@ -0,0 +1,516 @@
/*!
* \file rtklib_ionex.h
* \brief ionex functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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&&(value<range[0]||range[1]<value)) return -1;
if (range[1]<0.0&&(value<range[1]||range[0]<value)) return -1;
return (int)floor((value-range[0])/range[2]+0.5);
}
/* get number of items -------------------------------------------------------*/
int nitem(const double *range)
{
return getindex(range[1],range)+1;
}
/* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/
int dataindex(int i, int j, int k, const int *ndata)
{
if (i<0||ndata[0]<=i||j<0||ndata[1]<=j||k<0||ndata[2]<=k) return -1;
return i+ndata[0]*(j+ndata[1]*k);
}
/* add tec data to navigation data -------------------------------------------*/
tec_t *addtec(const double *lats, const double *lons, const double *hgts,
double rb, nav_t *nav)
{
tec_t *p,*nav_tec;
gtime_t time0={};
int i,n,ndata[3];
trace(3,"addtec :\n");
ndata[0]=nitem(lats);
ndata[1]=nitem(lons);
ndata[2]=nitem(hgts);
if (ndata[0]<=1||ndata[1]<=1||ndata[2]<=0) return NULL;
if (nav->nt>=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;i<n;i++) {
p->data[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;i<MAXSAT;i++) dcb[i]=rms[i]=0.0;
while (fgets(buff,sizeof(buff),fp)) {
if (strlen(buff)<60) continue;
label=buff+60;
if (strstr(label,"PRN / BIAS / RMS")==label) {
strncpy(id,buff+3,3); id[3]='\0';
if (!(sat=satid2no(id))) {
trace(2,"ionex invalid satellite: %s\n",id);
continue;
}
dcb[sat-1]=str2num(buff, 6,10);
rms[sat-1]=str2num(buff,16,10);
}
else if (strstr(label,"END OF AUX DATA")==label) break;
}
}
/* read ionex header ---------------------------------------------------------*/
double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
double *rb, double *nexp, double *dcb, double *rms)
{
double ver=0.0;
char buff[1024],*label;
trace(3,"readionexh:\n");
while (fgets(buff,sizeof(buff),fp)) {
if (strlen(buff)<60) continue;
label=buff+60;
if (strstr(label,"IONEX VERSION / TYPE")==label) {
if (buff[20]=='I') ver=str2num(buff,0,8);
}
else if (strstr(label,"BASE RADIUS")==label) {
*rb=str2num(buff,0,8);
}
else if (strstr(label,"HGT1 / HGT2 / DHGT")==label) {
hgts[0]=str2num(buff, 2,6);
hgts[1]=str2num(buff, 8,6);
hgts[2]=str2num(buff,14,6);
}
else if (strstr(label,"LAT1 / LAT2 / DLAT")==label) {
lats[0]=str2num(buff, 2,6);
lats[1]=str2num(buff, 8,6);
lats[2]=str2num(buff,14,6);
}
else if (strstr(label,"LON1 / LON2 / DLON")==label) {
lons[0]=str2num(buff, 2,6);
lons[1]=str2num(buff, 8,6);
lons[2]=str2num(buff,14,6);
}
else if (strstr(label,"EXPONENT")==label) {
*nexp=str2num(buff,0,6);
}
else if (strstr(label,"START OF AUX DATA")==label&&
strstr(buff,"DIFFERENTIAL CODE BIASES")) {
readionexdcb(fp,dcb,rms);
}
else if (strstr(label,"END OF HEADER")==label) {
return ver;
}
}
return 0.0;
}
/* read ionex body -----------------------------------------------------------*/
int readionexb(FILE *fp, const double *lats, const double *lons,
const double *hgts, double rb, double nexp, nav_t *nav)
{
tec_t *p=NULL;
gtime_t time={};
double lat,lon[3],hgt,x;
int i,j,k,n,m,index,type=0;
char buff[1024],*label=buff+60;
trace(3,"readionexb:\n");
while (fgets(buff,sizeof(buff),fp)) {
if (strlen(buff)<60) continue;
if (strstr(label,"START OF TEC MAP")==label) {
if ((p=addtec(lats,lons,hgts,rb,nav))) type=1;
}
else if (strstr(label,"END OF TEC MAP")==label) {
type=0;
p=NULL;
}
else if (strstr(label,"START OF RMS MAP")==label) {
type=2;
p=NULL;
}
else if (strstr(label,"END OF RMS MAP")==label) {
type=0;
p=NULL;
}
else if (strstr(label,"EPOCH OF CURRENT MAP")==label) {
if (str2time(buff,0,36,&time)) {
trace(2,"ionex epoch invalid: %-36.36s\n",buff);
continue;
}
if (type==2) {
for (i=nav->nt-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;m<n;m++) {
if (m%16==0&&!fgets(buff,sizeof(buff),fp)) break;
j=getindex(lon[0]+lon[2]*m,p->lons);
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;i<nav->nt-1;i++) {
for (j=i+1;j<nav->nt;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;i<nav->nt;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<MAXEXFILE;i++) {
if (!(efiles[i]=(char *)malloc(1024))) {
for (i--;i>=0;i--) free(efiles[i]);
return;
}
}
/* expand wild card in file path */
n=expath(file,efiles,MAXEXFILE);
for (i=0;i<n;i++) {
if (!(fp=fopen(efiles[i],"r"))) {
trace(2,"ionex file open error %s\n",efiles[i]);
continue;
}
/* read ionex header */
if (readionexh(fp,lats,lons,hgts,&rb,&nexp,dcb,rms)<=0.0) {
trace(2,"ionex file format error %s\n",efiles[i]);
continue;
}
/* read ionex body */
readionexb(fp,lats,lons,hgts,rb,nexp,nav);
fclose(fp);
}
for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
/* combine tec grid data */
if (nav->nt>0) combtec(nav);
/* P1-P2 dcb */
for (i=0;i<MAXSAT;i++) {
nav->cbias[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; /* -360<dlon<=0 */
a=dlat/tec->lats[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;i<tec->ndata[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 el<MIN_EL or h<MIN_HGT
*-----------------------------------------------------------------------------*/
int iontec(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, int opt, double *delay, double *var)
{
double dels[2],vars[2],a,tt;
int i,stat[2];
trace(3,"iontec : 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);
if (azel[1]<MIN_EL||pos[2]<MIN_HGT) {
*delay=0.0;
*var=VAR_NOTEC;
return 1;
}
for (i=0;i<nav->nt;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;
}

95
src/algorithms/libs/rtklib/rtklib_ionex.h Normal file
View File

@ -0,0 +1,95 @@
/*!
* \file rtklib_ionex.cc
* \brief ionex functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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_ */

704
src/algorithms/libs/rtklib/rtklib_pntpos.cc Normal file
View File

@ -0,0 +1,704 @@
/*!
* \file rtklib_pntpos.cc
* \brief standard code-based positioning
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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;i<nav->n;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;i<n&&i<MAXOBS;i++) {
vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;
if (!(sys=satsys(obs[i].sat,NULL))) continue;
/* reject duplicated observation data */
if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
trace(2,"duplicated observation data %s sat=%2d\n",
time_str(obs[i].time,3),obs[i].sat);
i++;
continue;
}
/* geometric distance/azimuth/elevation angle */
if ((r=geodist(rs+i*6,rr,e))<=0.0||
satazel(pos,e,azel+i*2)<opt->elmin)
{
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;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);
/* time system and receiver bias offset correction */
if (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}
else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}
else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
else mask[0]=1;
vsat[i]=1; resp[i]=v[nv]; (*ns)++;
/* error variance */
var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n",obs[i].sat,
azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));
}
/* constraint to avoid rank-deficient */
for (i=0;i<4;i++) {
if (mask[i]) continue;
v[nv]=0.0;
for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
var[nv++]=0.01;
}
return nv;
}
/* 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)
{
double azels[MAXOBS*2],dop[4],vv;
int i,ns;
trace(3,"valsol : n=%d nv=%d\n",n,nv);
/* chi-square validation of residuals */
vv=dot(v,v,nv);
if (nv>nx&&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;i<n;i++) {
if (!vsat[i]) continue;
azels[ ns*2]=azel[ i*2];
azels[1+ns*2]=azel[1+i*2];
ns++;
}
dops(ns,azels,opt->elmin,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;i<MAXITR;i++) {
/* pseudorange residuals */
nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
&ns);
if (nv<NX) {
sprintf(msg,"lack of valid sats ns=%d",nv);
break;
}
/* weight by variance */
for (j=0;j<nv;j++) {
sig=sqrt(var[j]);
v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig;
}
/* least square estimation */
if ((info=lsq(H,v,NX,nv,dx,Q))) {
sprintf(msg,"lsq error info=%d",info);
break;
}
for (j=0;j<NX;j++) x[j]+=dx[j];
if (norm(dx,NX)<1E-4) {
sol->type=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;i<n;i++) {
/* satellite exclution */
for (j=k=0;j<n;j++) {
if (j==i) continue;
obs_e[k]=obs[j];
matcpy(rs_e +6*k,rs +6*j,6,1);
matcpy(dts_e+2*k,dts+2*j,2,1);
vare_e[k]=vare[j];
svh_e[k++]=svh[j];
}
/* estimate receiver position without a satellite */
if (!estpos(obs_e,n-1,rs_e,dts_e,vare_e,svh_e,nav,opt,&sol_e,azel_e,
vsat_e,resp_e,msg_e)) {
trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
continue;
}
for (j=nvsat=0,rms_e=0.0;j<n-1;j++) {
if (!vsat_e[j]) continue;
rms_e+=SQR(resp_e[j]);
nvsat++;
}
if (nvsat<5) {
trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
obs[i].sat,nvsat);
continue;
}
rms_e=sqrt(rms_e/nvsat);
trace(3,"raim_fde: exsat=%2d rms=%8.3f\n",obs[i].sat,rms_e);
if (rms_e>rms) continue;
/* save result */
for (j=k=0;j<n;j++) {
if (j==i) continue;
matcpy(azel+2*j,azel_e+2*k,2,1);
vsat[j]=vsat_e[k];
resp[j]=resp_e[k++];
}
stat=1;
*sol=sol_e;
sat=obs[i].sat;
rms=rms_e;
vsat[i]=0;
strcpy(msg,msg_e);
}
if (stat) {
time2str(obs[0].time,tstr,2); satno2id(sat,name);
trace(2,"%s: %s excluded by raim\n",tstr+11,name);
}
free(obs_e);
free(rs_e ); free(dts_e ); free(vare_e); free(azel_e);
free(svh_e); free(vsat_e); free(resp_e);
return stat;
}
/* 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)
{
double lam,rate,pos[3],E[9],a[3],e[3],vs[3],cosel;
int i,j,nv=0;
trace(3,"resdop : n=%d\n",n);
ecef2pos(rr,pos); xyz2enu(pos,E);
for (i=0;i<n&&i<MAXOBS;i++) {
lam=nav->lam[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;i<MAXITR;i++) {
/* doppler residuals */
if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,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;k<n;k++)
// {
// printf("OBS[%i]: sat %i, P:%f ,LLI:%s \r\n",k,obs[k].sat,obs[k].P[0], obs[k].LLI);
// }
//
// for (k=0;k<nav->n;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<n*2;i++) azel[i]=azel_[i];
}
if (ssat) {
for (i=0;i<MAXSAT;i++) {
ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0;
ssat[i].snr[0]=0;
}
for (i=0;i<n;i++) {
ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
if (!vsat[i]) continue;
ssat[obs[i].sat-1].vs=1;
ssat[obs[i].sat-1].resp[0]=resp[i];
}
}
free(rs); free(dts); free(var); free(azel_); free(resp);
return stat;
}

180
src/algorithms/libs/rtklib/rtklib_pntpos.h Normal file
View File

@ -0,0 +1,180 @@
/*!
* \file rtklib_pntpos.h
* \brief standard code-based positioning
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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_ */

788
src/algorithms/libs/rtklib/rtklib_preceph.cc Normal file
View File

@ -0,0 +1,788 @@
/*!
* \file rtklib_preceph.cc
* \brief precise ephemeris and clock functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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&&k<ns;j++) {
sys=code2sys(buff[9+3*j]);
prn=(int)str2num(buff,10+3*j,2);
if (k<MAXSAT) sats[k++]=satno(sys,prn);
}
}
else if (i==12) {
strncpy(tsys,buff+9,3); tsys[3]='\0';
}
else if (i==14) {
bfact[0]=str2num(buff, 3,10);
bfact[1]=str2num(buff,14,12);
}
}
return ns;
}
/* add precise ephemeris -----------------------------------------------------*/
int addpeph(nav_t *nav, peph_t *peph)
{
peph_t *nav_peph;
if (nav->ne>=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<MAXSAT;i++) {
for (j=0;j<4;j++) {
peph.pos[i][j]=0.0;
peph.std[i][j]=0.0f;
peph.vel[i][j]=0.0;
peph.vst[i][j]=0.0f;
}
for (j=0;j<3;j++) {
peph.cov[i][j]=0.0f;
peph.vco[i][j]=0.0f;
}
}
for (i=pred_o=pred_c=v=0;i<n&&fgets(buff,sizeof(buff),fp);i++) {
if (strlen(buff)<4||(buff[0]!='P'&&buff[0]!='V')) continue;
sys=buff[1]==' '?SYS_GPS:code2sys(buff[1]);
prn=(int)str2num(buff,2,2);
if (sys==SYS_SBS) prn+=100;
else if (sys==SYS_QZS) prn+=192; /* extension to sp3-c */
if (!(sat=satno(sys,prn))) continue;
if (buff[0]=='P') {
pred_c=strlen(buff)>=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;j<nav->ne;j++) {
if (fabs(timediff(nav->peph[i].time,nav->peph[j].time))<1E-9) {
for (k=0;k<MAXSAT;k++) {
if (norm(nav->peph[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 (++i<j) nav->peph[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<MAXEXFILE;i++) {
if (!(efiles[i]=(char *)malloc(1024))) {
for (i--;i>=0;i--) free(efiles[i]);
return;
}
}
/* expand wild card in file path */
n=expath(file,efiles,MAXEXFILE);
for (i=j=0;i<n;i++) {
if (!(ext=strrchr(efiles[i],'.'))) continue;
if (!strstr(ext+1,"sp3")&&!strstr(ext+1,".SP3")&&
!strstr(ext+1,"eph")&&!strstr(ext+1,".EPH")) continue;
if (!(fp=fopen(efiles[i],"r"))) {
trace(2,"sp3 file open error %s\n",efiles[i]);
continue;
}
/* read sp3 header */
ns=readsp3h(fp,&time,&type,sats,bfact,tsys);
/* read sp3 body */
readsp3b(fp,type,sats,ns,bfact,tsys,j++,opt,nav);
fclose(fp);
}
for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
/* combine precise ephemeris */
if (nav->ne>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;i<MAXSAT;i++) {
pcv=searchpcv(i+1,"",time,&pcvs);
nav->pcvs[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;i<MAXRCV;i++) {
if (!strcmp(sta[i].name,str2)) break;
}
if (i<MAXRCV) {
j=!strcmp(str1,"G")?0:1;
nav->rbias[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;i<MAXSAT;i++) for (j=0;j<3;j++) {
nav->cbias[i][j]=0.0;
}
for (i=0;i<MAXEXFILE;i++) {
if (!(efiles[i]=(char *)malloc(1024))) {
for (i--;i>=0;i--) free(efiles[i]);
return 0;
}
}
n=expath(file,efiles,MAXEXFILE);
for (i=0;i<n;i++) {
readdcbf(efiles[i],nav,sta);
}
for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
return 1;
}
/* add satellite fcb ---------------------------------------------------------*/
int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
const double *bias, const double *std)
{
fcbd_t *nav_fcb;
int i,j;
if (nav->nf>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;i<MAXSAT;i++) for (j=0;j<3;j++) {
nav->fcb[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<MAXEXFILE;i++) {
if (!(efiles[i]=(char *)malloc(1024))) {
for (i--;i>=0;i--) free(efiles[i]);
return 0;
}
}
n=expath(file,efiles,MAXEXFILE);
for (i=0;i<n;i++) {
readfcbf(efiles[i],nav);
}
for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
if (nav->nf>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;j<n;j++) {
for (i=0;i<n-j;i++) {
y[i]=(x[i+j]*y[i]-x[i]*y[i+1])/(x[i+j]-x[i]);
}
}
return y[0];
}
/* satellite position by precise ephemeris -----------------------------------*/
int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *vare, double *varc)
{
double t[NMAX+1],p[3][NMAX+1],c[2],*pos,std=0.0,s[3],sinl,cosl;
int i,j,k,index;
trace(4,"pephpos : time=%s sat=%2d\n",time_str(time,3),sat);
rs[0]=rs[1]=rs[2]=dts[0]=0.0;
if (nav->ne<NMAX+1||
timediff(time,nav->peph[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;i<j;) {
k=(i+j)/2;
if (timediff(nav->peph[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;i<j;) {
k=(i+j)/2;
if (timediff(nav->pclk[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<sat) return 0;
/* satellite position and clock bias */
if (!pephpos(time,sat,nav,rss,dtss,&vare,&varc)||
!pephclk(time,sat,nav,dtss,&varc)) return 0;
time=timeadd(time,tt);
if (!pephpos(time,sat,nav,rst,dtst,NULL,NULL)||
!pephclk(time,sat,nav,dtst,NULL)) return 0;
/* satellite antenna offset correction */
if (opt) {
satantoff(time,rss,sat,nav,dant);
}
for (i=0;i<3;i++) {
rs[i ]=rss[i]+dant[i];
rs[i+3]=(rst[i]-rss[i])/tt;
}
/* relativistic effect correction */
if (dtss[0]!=0.0) {
dts[0]=dtss[0]-2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT;
dts[1]=(dtst[0]-dtss[0])/tt;
}
else { /* no precise clock */
dts[0]=dts[1]=0.0;
}
if (var) *var=vare+varc;
return 1;
}

136
src/algorithms/libs/rtklib/rtklib_preceph.h Normal file
View File

@ -0,0 +1,136 @@
/*!
* \file rtklib_preceph.h
* \brief precise ephemeris and clock functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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_

3718
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc Normal file
View File

File diff suppressed because it is too large Load Diff

363
src/algorithms/libs/rtklib/rtklib_rtkcmn.h Normal file
View File

@ -0,0 +1,363 @@
/*!
* \file rtklib_rtkcmn.h
* \brief rtklib common functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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_ */

910
src/algorithms/libs/rtklib/rtklib_sbas.cc Normal file
View File

@ -0,0 +1,910 @@
/*!
* \file rtklib_sbas.cc
* \brief sbas functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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 0<udre&&udre<=14?var[udre-1]:0.0;
}
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
double 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-1]:0.0;
}
/* fast correction degradation -----------------------------------------------*/
double 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;
}
/* decode type 1: prn masks --------------------------------------------------*/
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,n,sat;
trace(4,"decode_sbstype1:\n");
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;
}
}
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.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.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;i<sbssat->nsat&&i<MAXSAT;i++) {
if (sbssat->sat[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;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 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;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\n",band,n);
return 1;
}
/* decode half long term correction (vel code=0) -----------------------------*/
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{
int i,n=getbitu(msg->msg,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 (week<WEEKOFFSET) week+=WEEKOFFSET;
}
else continue;
if (sel!=0&&sel!=prn) continue;
time=gpst2time(week,tow);
if (!screent(time,ts,te,0.0)) continue;
if (sbs->n>=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->tow<q2->tow?-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<MAXEXFILE;i++) {
if (!(efiles[i]=(char *)malloc(1024))) {
for (i--;i>=0;i--) free(efiles[i]);
return 0;
}
}
/* expand wild card in file path */
n=expath(file,efiles,MAXEXFILE);
for (i=0;i<n;i++) {
if (!(ext=strrchr(efiles[i],'.'))) continue;
if (strcmp(ext,".sbs")&&strcmp(ext,".SBS")&&
strcmp(ext,".ems")&&strcmp(ext,".EMS")) continue;
readmsgs(efiles[i],sel,ts,te,sbs);
}
for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
/* sort messages */
if (sbs->n>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;p<ion[i].igp+ion[i].nigp;p++) {
if (p->t0.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.0<pos[2]||azel[1]<=0) {
*var=0.0;
return 0.0;
}
if (zh==0.0||fabs(pos[0]-pos_[0])>1E-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;p<sbssat->sat+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;p<sbssat->sat+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 */
}

187
src/algorithms/libs/rtklib/rtklib_sbas.h Normal file
View File

@ -0,0 +1,187 @@
/*!
* \file rtklib_sbas.h
* \brief sbas functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* 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_ */

3
src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc
View File

@ -34,9 +34,6 @@
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#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"

16
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc
View File

@ -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_Msg> sbas_raw_msgs;
//std::vector<Sbas_Raw_Msg> sbas_raw_msgs;
for(std::vector<msg_candiate_char_t>::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<Sbas_Raw_Msg>::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<Sbas_Raw_Msg>::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();

3
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h
View File

@ -41,7 +41,6 @@
#include <gnuradio/block.h>
#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<int> msg_candidate, std::vector<unsigned char> &bytes);
} d_crc_verifier;
Sbas_Telemetry_Data sbas_telemetry_data;
};
#endif

1
src/core/receiver/CMakeLists.txt
View File

@ -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}

7
src/core/receiver/gnss_block_factory.cc
View File

@ -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<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
out_streams));
block = std::move(block_);
}
else if (implementation.compare("RTKLIB_PVT") == 0)
{
std::unique_ptr<GNSSBlockInterface> block_(new RtklibPvt(configuration.get(), role, in_streams,
out_streams));
block = std::move(block_);
}
else
{
// Log fatal. This causes execution to stop.

9
src/core/system_parameters/CMakeLists.txt
View File

@ -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})

1
src/core/system_parameters/gps_cnav_ephemeris.h
View File

@ -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 <boost/serialization/nvp.hpp>

467
src/core/system_parameters/sbas_ionospheric_correction.cc
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "sbas_ionospheric_correction.h"
#include <cmath>
#include <iostream>
#include <glog/logging.h>
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<Igp_Band>::const_iterator it_band = d_bands.begin(); it_band != d_bands.end(); ++it_band)
{
int band = it_band - d_bands.begin();
out << "<<I>> Band" << band << ":" << std::endl;
for(std::vector<Igp>::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 << "<<I>> -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<EFBFBD>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)<RE_WGS84) return -1.0;
// for (i=0;i<3;i++) e[i]=rs[i]-rr[i];
// r=norm(e,3);
// for (i=0;i<3;i++) e[i]/=r;
// return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;
//}
/* inner product ---------------------------------------------------------------
* inner product of vectors
* args : double *a,*b I vector a,b (n x 1)
* int n I size of vector a,b
* return : a'*b
*-----------------------------------------------------------------------------*/
double Sbas_Ionosphere_Correction::dot(const double *a, const double *b, int n)
{
double c = 0.0;
while (--n >= 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) << "<<I>> " << 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<Igp_Band>::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<Igp>::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) << "<<I>> 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;
}

203
src/core/system_parameters/sbas_ionospheric_correction.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef SBAS_IONOSPHERIC_CORRECTION_H_
#define SBAS_IONOSPHERIC_CORRECTION_H_
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <boost/serialization/serialization.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
/*!
* \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<class Archive>
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<Igp> d_igps;
private:
friend class boost::serialization::access;
template<class Archive>
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<class Archive>
void serialize(Archive& ar, const unsigned int version){ar & d_bands;}
public:
std::vector<Igp_Band> 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_ */

296
src/core/system_parameters/sbas_satellite_correction.cc
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "sbas_satellite_correction.h"
#include <stdarg.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <glog/logging.h>
#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 << "<<S>> 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 << "<<S>> 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 << "<<S>> 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) << "<<S>> 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) << "<<S>> 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) << "<<S>> 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) << "<<S>> " << 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;
}

107
src/core/system_parameters/sbas_satellite_correction.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#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_ */

978
src/core/system_parameters/sbas_telemetry_data.cc
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <cmath>
#include <cstring>
#include <iostream>
#include <stdarg.h>
#include <stdio.h>
#include <glog/logging.h>
#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) << "<<T>> 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<unsigned char> 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<unsigned char>::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) << "<<T>> 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) << "<<T>> 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) << "<<T>> 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) << "<<T>> 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 << "<<T>> band=" << i_band
<< " nigp=" << d_nav.sbsion[i_band].nigp << std::endl;
ss << "<<T>> -> 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 <class Struct>
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<<cmp>> 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 << "<<cmp>> 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) << "<<T>> 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 << "<<T>> 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<int, Fast_Correction>::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<Fast_Correction>(fcorr, it_old_fcorr->second ));
fast_correction_updated = true;
emitted_fast_corrections[prn] = fcorr;
}
// check if long term corrections got updated
std::map<int, Long_Term_Correction>::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<Long_Term_Correction>(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) << "<<T>> " << 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) << "<<T>> 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;
}

492
src/core/system_parameters/sbas_telemetry_data.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_SBAS_TELEMETRY_DATA_H_
#define GNSS_SDR_SBAS_TELEMETRY_DATA_H_
#include <algorithm>
#include <bitset>
#include <cmath>
#include <iostream>
#include <map>
#include <string>
#include <vector>
#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<unsigned char> 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<unsigned char> 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<unsigned char> 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<int, Fast_Correction> emitted_fast_corrections;
std::map<int, Long_Term_Correction> 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

193
src/core/system_parameters/sbas_time.h
View File

@ -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 <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_SBAS_TIME_H_
#define GNSS_SDR_SBAS_TIME_H_
#include <cmath>
#include <iostream>
#include <glog/logging.h>
#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) << "<<R>> 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) << "<<R>> 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) << "<<R>> 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) << "<<R>> 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) << "<<R>> 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_ */

6
src/tests/test_main.cc
View File

@ -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;

5
src/utils/front-end-cal/main.cc
View File

@ -51,6 +51,7 @@
#include <gnuradio/blocks/file_source.h>
#include <gnuradio/blocks/file_sink.h>
#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"