mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-07 18:54:06 +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
Download Patch File Download Diff File Expand all files Collapse all files

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_ */