From 27ab390944d31dc268350f4303a401a544d3d8b8 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 21 Apr 2017 11:34:23 +0200 Subject: [PATCH] Refactor of constants, remove defines and start adapting rtklib code to our coding style --- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +- src/algorithms/libs/rtklib/CMakeLists.txt | 9 +- src/algorithms/libs/rtklib/rtklib.h | 601 ++- .../libs/rtklib/rtklib_conversions.cc | 216 +- .../libs/rtklib/rtklib_conversions.h | 8 +- .../libs/rtklib/rtklib_ephemeris.cc | 1271 ++--- src/algorithms/libs/rtklib/rtklib_ephemeris.h | 36 +- src/algorithms/libs/rtklib/rtklib_ionex.cc | 832 ++-- src/algorithms/libs/rtklib/rtklib_ionex.h | 39 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 1202 ++--- src/algorithms/libs/rtklib/rtklib_pntpos.h | 20 +- src/algorithms/libs/rtklib/rtklib_preceph.cc | 1279 ++--- src/algorithms/libs/rtklib/rtklib_preceph.h | 66 +- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 4319 +++++++++-------- src/algorithms/libs/rtklib/rtklib_rtkcmn.h | 166 +- src/algorithms/libs/rtklib/rtklib_sbas.cc | 1385 +++--- src/algorithms/libs/rtklib/rtklib_sbas.h | 124 +- src/core/system_parameters/GPS_L1_CA.h | 7 +- src/core/system_parameters/GPS_L2C.h | 3 +- src/core/system_parameters/Galileo_E1.h | 3 +- src/core/system_parameters/Galileo_E5a.h | 4 +- src/core/system_parameters/MATH_CONSTANTS.h | 18 +- src/core/system_parameters/gnss_frequencies.h | 53 + 23 files changed, 6182 insertions(+), 5483 deletions(-) create mode 100644 src/core/system_parameters/gnss_frequencies.h diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 893c4c01e..437acd35c 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -217,8 +217,8 @@ bool rtklib_solver::get_PVT(std::map gnss_observables_map, dou nav_data.n=valid_obs; for (int i=0; i< MAXSAT;i++) { - nav_data.lam[i][0]=CLIGHT/FREQ1; /* L1/E1 */ - nav_data.lam[i][1]=CLIGHT/FREQ2; /* L2 */ + nav_data.lam[i][0]=SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ + nav_data.lam[i][1]=SPEED_OF_LIGHT/FREQ2; /* L2 */ } result=pntpos(obs_data, valid_obs, &nav_data, &rtklib_opt, &old_pvt_sol, NULL, NULL,rtklib_msg); diff --git a/src/algorithms/libs/rtklib/CMakeLists.txt b/src/algorithms/libs/rtklib/CMakeLists.txt index fd582ea9d..9a172fb53 100644 --- a/src/algorithms/libs/rtklib/CMakeLists.txt +++ b/src/algorithms/libs/rtklib/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors) +# Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) # # This file is part of GNSS-SDR. # @@ -38,14 +38,13 @@ include_directories( ${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} @@ -55,6 +54,4 @@ target_link_libraries( ${BLAS} ${LAPACK} ) - -#MESSAGE( STATUS "*****************BLAS: " ${BLAS} ) - + diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index e8bb7bd76..6d6cc26ac 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -48,356 +48,300 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#ifndef RTKLIB_H_ -#define RTKLIB_H_ + +#ifndef GNSS_SDR_RTKLIB_H_ +#define GNSS_SDR_RTKLIB_H_ #include #include #include #include -#include +#include #include #include +#include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" -#define thread_t pthread_t -#define lock_t pthread_mutex_t -#define initlock(f) pthread_mutex_init(f,NULL) -#define lock(f) pthread_mutex_lock(f) -#define unlock(f) pthread_mutex_unlock(f) -#define FILEPATHSEP '/' -#define PI 3.1415926535897932 /* pi */ -#define D2R (PI/180.0) /* deg to rad */ -#define R2D (180.0/PI) /* rad to deg */ -#define CLIGHT 299792458.0 /* speed of light (m/s) */ -#define SC2RAD 3.1415926535898 /* semi-circle to radian (IS-GPS) */ -#define AU 149597870691.0 /* 1 AU (m) */ -#define AS2R (D2R/3600.0) /* arc sec to radian */ +const int FILEPATHSEP = '/'; -#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */ +const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m) +const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84) -#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */ -#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */ +const double HION = 350000.0; //!< ionosphere height (m) -#define HION 350000.0 /* ionosphere height (m) */ +const unsigned int POLYCRC32 = 0xEDB88320u; //!< CRC32 polynomial +const unsigned int POLYCRC24Q = 0x1864CFBu; //!< CRC24Q polynomial -#define P2_5 0.03125 /* 2^-5 */ -#define P2_6 0.015625 /* 2^-6 */ -#define P2_11 4.882812500000000E-04 /* 2^-11 */ -#define P2_15 3.051757812500000E-05 /* 2^-15 */ -#define P2_17 7.629394531250000E-06 /* 2^-17 */ -#define P2_19 1.907348632812500E-06 /* 2^-19 */ -#define P2_20 9.536743164062500E-07 /* 2^-20 */ -#define P2_21 4.768371582031250E-07 /* 2^-21 */ -#define P2_23 1.192092895507810E-07 /* 2^-23 */ -#define P2_24 5.960464477539063E-08 /* 2^-24 */ -#define P2_27 7.450580596923828E-09 /* 2^-27 */ -#define P2_29 1.862645149230957E-09 /* 2^-29 */ -#define P2_30 9.313225746154785E-10 /* 2^-30 */ -#define P2_31 4.656612873077393E-10 /* 2^-31 */ -#define P2_32 2.328306436538696E-10 /* 2^-32 */ -#define P2_33 1.164153218269348E-10 /* 2^-33 */ -#define P2_35 2.910383045673370E-11 /* 2^-35 */ -#define P2_38 3.637978807091710E-12 /* 2^-38 */ -#define P2_39 1.818989403545856E-12 /* 2^-39 */ -#define P2_40 9.094947017729280E-13 /* 2^-40 */ -#define P2_43 1.136868377216160E-13 /* 2^-43 */ -#define P2_48 3.552713678800501E-15 /* 2^-48 */ -#define P2_50 8.881784197001252E-16 /* 2^-50 */ -#define P2_55 2.775557561562891E-17 /* 2^-55 */ +const unsigned int PMODE_SINGLE = 0; //!< positioning mode: single +const unsigned int PMODE_DGPS = 1; //!< positioning mode: DGPS/DGNSS +const unsigned int PMODE_KINEMA = 2; //!< positioning mode: kinematic +const unsigned int PMODE_STATIC = 3; //!< positioning mode: static +const unsigned int PMODE_MOVEB = 4; //!< positioning mode: moving-base +const unsigned int PMODE_FIXED = 5; //!< positioning mode: fixed +const unsigned int PMODE_PPP_KINEMA = 6; //!< positioning mode: PPP-kinemaric +const unsigned int PMODE_PPP_STATIC = 7; //!< positioning mode: PPP-static +const unsigned int PMODE_PPP_FIXED = 8; //!< positioning mode: PPP-fixed -#define POLYCRC32 0xEDB88320u /* CRC32 polynomial */ -#define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */ +const unsigned int SOLF_LLH = 0; //!< solution format: lat/lon/height +const unsigned int SOLF_XYZ = 1; //!< solution format: x/y/z-ecef +const unsigned int SOLF_ENU = 2; //!< solution format: e/n/u-baseline +const unsigned int SOLF_NMEA = 3; //!< solution format: NMEA-183 +const unsigned int SOLF_STAT = 4; //!< solution format: solution status +const unsigned int SOLF_GSIF = 5; //!< solution format: GSI F1/F2 -#define PMODE_SINGLE 0 /* positioning mode: single */ -#define PMODE_DGPS 1 /* positioning mode: DGPS/DGNSS */ -#define PMODE_KINEMA 2 /* positioning mode: kinematic */ -#define PMODE_STATIC 3 /* positioning mode: static */ -#define PMODE_MOVEB 4 /* positioning mode: moving-base */ -#define PMODE_FIXED 5 /* positioning mode: fixed */ -#define PMODE_PPP_KINEMA 6 /* positioning mode: PPP-kinemaric */ -#define PMODE_PPP_STATIC 7 /* positioning mode: PPP-static */ -#define PMODE_PPP_FIXED 8 /* positioning mode: PPP-fixed */ +const unsigned int SOLQ_NONE = 0; //!< solution status: no solution +const unsigned int SOLQ_FIX = 1; //!< solution status: fix +const unsigned int SOLQ_FLOAT = 2; //!< solution status: float +const unsigned int SOLQ_SBAS = 3; //!< solution status: SBAS +const unsigned int SOLQ_DGPS = 4; //!< solution status: DGPS/DGNSS +const unsigned int SOLQ_SINGLE = 5; //!< solution status: single +const unsigned int SOLQ_PPP = 6; //!< solution status: PPP +const unsigned int SOLQ_DR = 7; //!< solution status: dead reckoning +const unsigned int MAXSOLQ = 7; //!< max number of solution status -#define SOLF_LLH 0 /* solution format: lat/lon/height */ -#define SOLF_XYZ 1 /* solution format: x/y/z-ecef */ -#define SOLF_ENU 2 /* solution format: e/n/u-baseline */ -#define SOLF_NMEA 3 /* solution format: NMEA-183 */ -#define SOLF_STAT 4 /* solution format: solution status */ -#define SOLF_GSIF 5 /* solution format: GSI F1/F2 */ +const unsigned int TIMES_GPST = 0; //!< time system: gps time +const unsigned int TIMES_UTC = 1; //!< time system: utc +const unsigned int TIMES_JST = 2; //!< time system: jst -#define SOLQ_NONE 0 /* solution status: no solution */ -#define SOLQ_FIX 1 /* solution status: fix */ -#define SOLQ_FLOAT 2 /* solution status: float */ -#define SOLQ_SBAS 3 /* solution status: SBAS */ -#define SOLQ_DGPS 4 /* solution status: DGPS/DGNSS */ -#define SOLQ_SINGLE 5 /* solution status: single */ -#define SOLQ_PPP 6 /* solution status: PPP */ -#define SOLQ_DR 7 /* solution status: dead reconing */ -#define MAXSOLQ 7 /* max number of solution status */ +const unsigned int MAXFREQ = 7; //!< max NFREQ -#define TIMES_GPST 0 /* time system: gps time */ -#define TIMES_UTC 1 /* time system: utc */ -#define TIMES_JST 2 /* time system: jst */ +const unsigned int MAXLEAPS = 64; //!< max number of leap seconds table */ +const double DTTOL = 0.005; //!< tolerance of time difference (s) */ -#define MAXFREQ 7 /* max NFREQ */ +const unsigned int NFREQ = 3; +const unsigned int NEXOBS = 0; //!< number of extended obs codes */ +const unsigned int MAXANT = 64; //!< max length of station name/antenna type */ -#define FREQ1 1.57542E9 /* L1/E1 frequency (Hz) */ -#define FREQ2 1.22760E9 /* L2 frequency (Hz) */ -#define FREQ5 1.17645E9 /* L5/E5a frequency (Hz) */ -#define FREQ6 1.27875E9 /* E6/LEX frequency (Hz) */ -#define FREQ7 1.20714E9 /* E5b frequency (Hz) */ -#define FREQ8 1.191795E9 /* E5a+b frequency (Hz) */ -#define FREQ9 2.492028E9 /* S frequency (Hz) */ -#define FREQ1_GLO 1.60200E9 /* GLONASS G1 base frequency (Hz) */ -#define DFRQ1_GLO 0.56250E6 /* GLONASS G1 bias frequency (Hz/n) */ -#define FREQ2_GLO 1.24600E9 /* GLONASS G2 base frequency (Hz) */ -#define DFRQ2_GLO 0.43750E6 /* GLONASS G2 bias frequency (Hz/n) */ -#define FREQ3_GLO 1.202025E9 /* GLONASS G3 frequency (Hz) */ -#define FREQ1_CMP 1.561098E9 /* BeiDou B1 frequency (Hz) */ -#define FREQ2_CMP 1.20714E9 /* BeiDou B2 frequency (Hz) */ -#define FREQ3_CMP 1.26852E9 /* BeiDou B3 frequency (Hz) */ +const unsigned int MINPRNGPS = 1; //!< min satellite PRN number of GPS */ +const unsigned int MAXPRNGPS = 32; //!< max satellite PRN number of GPS */ +const unsigned int NSATGPS = (MAXPRNGPS - MINPRNGPS + 1); //!< number of GPS satellites */ +const unsigned int NSYSGPS = 1; -#define MAXLEAPS 64 /* max number of leap seconds table */ -#define DTTOL 0.005 /* tolerance of time difference (s) */ +const int SYS_NONE = 0x00; //!< navigation system: none */ +const int SYS_GPS = 0x01; //!< navigation system: GPS */ +const int SYS_SBS = 0x02; //!< navigation system: SBAS */ +const int SYS_GLO = 0x04; //!< navigation system: GLONASS */ +const int SYS_GAL = 0x08; //!< navigation system: Galileo */ +const int SYS_QZS = 0x10; //!< navigation system: QZSS */ +const int SYS_BDS = 0x20; //!< navigation system: BeiDou */ +const int SYS_IRN = 0x40; //!< navigation system: IRNS */ +const int SYS_LEO = 0x80; //!< navigation system: LEO */ +const int SYS_ALL = 0xFF; //!< navigation system: all */ -#define NFREQ 3 -#define NEXOBS 0 /* number of extended obs codes */ -#define MAXANT 64 /* max length of station name/antenna type */ - -#define MINPRNGPS 1 /* min satellite PRN number of GPS */ -#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ -#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */ -#define NSYSGPS 1 - -#define SYS_NONE 0x00 /* navigation system: none */ -#define SYS_GPS 0x01 /* navigation system: GPS */ -#define SYS_SBS 0x02 /* navigation system: SBAS */ -#define SYS_GLO 0x04 /* navigation system: GLONASS */ -#define SYS_GAL 0x08 /* navigation system: Galileo */ -#define SYS_QZS 0x10 /* navigation system: QZSS */ -#define SYS_CMP 0x20 /* navigation system: BeiDou */ -#define SYS_IRN 0x40 /* navigation system: IRNS */ -#define SYS_LEO 0x80 /* navigation system: LEO */ -#define SYS_ALL 0xFF /* navigation system: all */ - -#define CODE_NONE 0 /* obs code: none or unknown */ -#define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ -#define CODE_L1P 2 /* obs code: L1P,G1P (GPS,GLO) */ -#define CODE_L1W 3 /* obs code: L1 Z-track (GPS) */ -#define CODE_L1Y 4 /* obs code: L1Y (GPS) */ -#define CODE_L1M 5 /* obs code: L1M (GPS) */ -#define CODE_L1N 6 /* obs code: L1codeless (GPS) */ -#define CODE_L1S 7 /* obs code: L1C(D) (GPS,QZS) */ -#define CODE_L1L 8 /* obs code: L1C(P) (GPS,QZS) */ -#define CODE_L1E 9 /* (not used) */ -#define CODE_L1A 10 /* obs code: E1A (GAL) */ -#define CODE_L1B 11 /* obs code: E1B (GAL) */ -#define CODE_L1X 12 /* obs code: E1B+C,L1C(D+P) (GAL,QZS) */ -#define CODE_L1Z 13 /* obs code: E1A+B+C,L1SAIF (GAL,QZS) */ -#define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */ -#define CODE_L2D 15 /* obs code: L2 L1C/A-(P2-P1) (GPS) */ -#define CODE_L2S 16 /* obs code: L2C(M) (GPS,QZS) */ -#define CODE_L2L 17 /* obs code: L2C(L) (GPS,QZS) */ -#define CODE_L2X 18 /* obs code: L2C(M+L),B1I+Q (GPS,QZS,CMP) */ -#define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */ -#define CODE_L2W 20 /* obs code: L2 Z-track (GPS) */ -#define CODE_L2Y 21 /* obs code: L2Y (GPS) */ -#define CODE_L2M 22 /* obs code: L2M (GPS) */ -#define CODE_L2N 23 /* obs code: L2codeless (GPS) */ -#define CODE_L5I 24 /* obs code: L5/E5aI (GPS,GAL,QZS,SBS) */ -#define CODE_L5Q 25 /* obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ -#define CODE_L5X 26 /* obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) */ -#define CODE_L7I 27 /* obs code: E5bI,B2I (GAL,CMP) */ -#define CODE_L7Q 28 /* obs code: E5bQ,B2Q (GAL,CMP) */ -#define CODE_L7X 29 /* obs code: E5bI+Q,B2I+Q (GAL,CMP) */ -#define CODE_L6A 30 /* obs code: E6A (GAL) */ -#define CODE_L6B 31 /* obs code: E6B (GAL) */ -#define CODE_L6C 32 /* obs code: E6C (GAL) */ -#define CODE_L6X 33 /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,CMP) */ -#define CODE_L6Z 34 /* obs code: E6A+B+C (GAL) */ -#define CODE_L6S 35 /* obs code: LEXS (QZS) */ -#define CODE_L6L 36 /* obs code: LEXL (QZS) */ -#define CODE_L8I 37 /* obs code: E5(a+b)I (GAL) */ -#define CODE_L8Q 38 /* obs code: E5(a+b)Q (GAL) */ -#define CODE_L8X 39 /* obs code: E5(a+b)I+Q (GAL) */ -#define CODE_L2I 40 /* obs code: B1I (BDS) */ -#define CODE_L2Q 41 /* obs code: B1Q (BDS) */ -#define CODE_L6I 42 /* obs code: B3I (BDS) */ -#define CODE_L6Q 43 /* obs code: B3Q (BDS) */ -#define CODE_L3I 44 /* obs code: G3I (GLO) */ -#define CODE_L3Q 45 /* obs code: G3Q (GLO) */ -#define CODE_L3X 46 /* obs code: G3I+Q (GLO) */ -#define CODE_L1I 47 /* obs code: B1I (BDS) */ -#define CODE_L1Q 48 /* obs code: B1Q (BDS) */ -#define CODE_L5A 49 /* obs code: L5A SPS (IRN) */ -#define CODE_L5B 50 /* obs code: L5B RS(D) (IRN) */ -#define CODE_L5C 51 /* obs code: L5C RS(P) (IRN) */ -#define CODE_L9A 52 /* obs code: SA SPS (IRN) */ -#define CODE_L9B 53 /* obs code: SB RS(D) (IRN) */ -#define CODE_L9C 54 /* obs code: SC RS(P) (IRN) */ -#define CODE_L9X 55 /* obs code: SB+C (IRN) */ -#define MAXCODE 55 /* max number of obs code */ +const unsigned int CODE_NONE = 0; //!< obs code: none or unknown */ +const unsigned int CODE_L1C = 1; //!< obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ +const unsigned int CODE_L1P = 2; //!< obs code: L1P,G1P (GPS,GLO) */ +const unsigned int CODE_L1W = 3; //!< obs code: L1 Z-track (GPS) */ +const unsigned int CODE_L1Y = 4; //!< obs code: L1Y (GPS) */ +const unsigned int CODE_L1M = 5; //!< obs code: L1M (GPS) */ +const unsigned int CODE_L1N = 6; //!< obs code: L1codeless (GPS) */ +const unsigned int CODE_L1S = 7; //!< obs code: L1C(D) (GPS,QZS) */ +const unsigned int CODE_L1L = 8; //!< obs code: L1C(P) (GPS,QZS) */ +const unsigned int CODE_L1E = 9; //!< (not used) */ +const unsigned int CODE_L1A = 10; //!< obs code: E1A (GAL) */ +const unsigned int CODE_L1B = 11; //!< obs code: E1B (GAL) */ +const unsigned int CODE_L1X = 12; //!< obs code: E1B+C,L1C(D+P) (GAL,QZS) */ +const unsigned int CODE_L1Z = 13; //!< obs code: E1A+B+C,L1SAIF (GAL,QZS) */ +const unsigned int CODE_L2C = 14; //!< obs code: L2C/A,G1C/A (GPS,GLO) */ +const unsigned int CODE_L2D = 15; //!< obs code: L2 L1C/A-(P2-P1) (GPS) */ +const unsigned int CODE_L2S = 16; //!< obs code: L2C(M) (GPS,QZS) */ +const unsigned int CODE_L2L = 17; //!< obs code: L2C(L) (GPS,QZS) */ +const unsigned int CODE_L2X = 18; //!< obs code: L2C(M+L),B1I+Q (GPS,QZS,BDS) */ +const unsigned int CODE_L2P = 19; //!< obs code: L2P,G2P (GPS,GLO) */ +const unsigned int CODE_L2W = 20; //!< obs code: L2 Z-track (GPS) */ +const unsigned int CODE_L2Y = 21; //!< obs code: L2Y (GPS) */ +const unsigned int CODE_L2M = 22; //!< obs code: L2M (GPS) */ +const unsigned int CODE_L2N = 23; //!< obs code: L2codeless (GPS) */ +const unsigned int CODE_L5I = 24; //!< obs code: L5/E5aI (GPS,GAL,QZS,SBS) */ +const unsigned int CODE_L5Q = 25; //!< obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ +const unsigned int CODE_L5X = 26; //!< obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) */ +const unsigned int CODE_L7I = 27; //!< obs code: E5bI,B2I (GAL,BDS) */ +const unsigned int CODE_L7Q = 28; //!< obs code: E5bQ,B2Q (GAL,BDS) */ +const unsigned int CODE_L7X = 29; //!< obs code: E5bI+Q,B2I+Q (GAL,BDS) */ +const unsigned int CODE_L6A = 30; //!< obs code: E6A (GAL) */ +const unsigned int CODE_L6B = 31; //!< obs code: E6B (GAL) */ +const unsigned int CODE_L6C = 32; //!< obs code: E6C (GAL) */ +const unsigned int CODE_L6X = 33; //!< obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,BDS) */ +const unsigned int CODE_L6Z = 34; //!< obs code: E6A+B+C (GAL) */ +const unsigned int CODE_L6S = 35; //!< obs code: LEXS (QZS) */ +const unsigned int CODE_L6L = 36; //!< obs code: LEXL (QZS) */ +const unsigned int CODE_L8I = 37; //!< obs code: E5(a+b)I (GAL) */ +const unsigned int CODE_L8Q = 38; //!< obs code: E5(a+b)Q (GAL) */ +const unsigned int CODE_L8X = 39; //!< obs code: E5(a+b)I+Q (GAL) */ +const unsigned int CODE_L2I = 40; //!< obs code: B1I (BDS) */ +const unsigned int CODE_L2Q = 41; //!< obs code: B1Q (BDS) */ +const unsigned int CODE_L6I = 42; //!< obs code: B3I (BDS) */ +const unsigned int CODE_L6Q = 43; //!< obs code: B3Q (BDS) */ +const unsigned int CODE_L3I = 44; //!< obs code: G3I (GLO) */ +const unsigned int CODE_L3Q = 45; //!< obs code: G3Q (GLO) */ +const unsigned int CODE_L3X = 46; //!< obs code: G3I+Q (GLO) */ +const unsigned int CODE_L1I = 47; //!< obs code: B1I (BDS) */ +const unsigned int CODE_L1Q = 48; //!< obs code: B1Q (BDS) */ +const unsigned int CODE_L5A = 49; //!< obs code: L5A SPS (IRN) */ +const unsigned int CODE_L5B = 50; //!< obs code: L5B RS(D) (IRN) */ +const unsigned int CODE_L5C = 51; //!< obs code: L5C RS(P) (IRN) */ +const unsigned int CODE_L9A = 52; //!< obs code: SA SPS (IRN) */ +const unsigned int CODE_L9B = 53; //!< obs code: SB RS(D) (IRN) */ +const unsigned int CODE_L9C = 54; //!< obs code: SC RS(P) (IRN) */ +const unsigned int CODE_L9X = 55; //!< obs code: SB+C (IRN) */ +const unsigned int MAXCODE = 55; //!< max number of obs code */ #ifdef ENAGLO -#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ -#define MAXPRNGLO 27 /* max satellite slot number of GLONASS */ -#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */ -#define NSYSGLO 1 +const unsigned int MINPRNGLO = 1; //!< min satellite slot number of GLONASS */ +const unsigned int MAXPRNGLO = 27; //!< max satellite slot number of GLONASS */ +const unsigned int NSATGLO = (MAXPRNGLO - MINPRNGLO + 1); //!< number of GLONASS satellites */ +const unsigned int NSYSGLO = 1; #else -#define MINPRNGLO 0 -#define MAXPRNGLO 0 -#define NSATGLO 0 -#define NSYSGLO 0 +const unsigned int MINPRNGLO = 0; +const unsigned int MAXPRNGLO = 0; +const unsigned int NSATGLO = 0; +const unsigned int NSYSGLO = 0; #endif + #ifdef ENAGAL -#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ -#define MAXPRNGAL 30 /* max satellite PRN number of Galileo */ -#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */ -#define NSYSGAL 1 +const unsigned int MINPRNGAL = 1; //!< min satellite PRN number of Galileo */ +const unsigned int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo */ +const unsigned int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites */ +const unsigned int NSYSGAL = 1; #else -#define MINPRNGAL 0 -#define MAXPRNGAL 0 -#define NSATGAL 0 -#define NSYSGAL 0 +const unsigned int MINPRNGAL = 0; +const unsigned int MAXPRNGAL = 0; +const unsigned int NSATGAL = 0; +const unsigned int NSYSGAL = 0; #endif + #ifdef ENAQZS -#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ -#define MAXPRNQZS 199 /* max satellite PRN number of QZSS */ -#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */ -#define MAXPRNQZS_S 189 /* max satellite PRN number of QZSS SAIF */ -#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */ -#define NSYSQZS 1 +const unsigned int MINPRNQZS = 193; //!< min satellite PRN number of QZSS */ +const unsigned int MAXPRNQZS = 199; //!< max satellite PRN number of QZSS */ +const unsigned int MINPRNQZS_S = 183; //!< min satellite PRN number of QZSS SAIF */ +const unsigned int MAXPRNQZS_S = 189; //!< max satellite PRN number of QZSS SAIF */ +const unsigned int NSATQZS = (MAXPRNQZS - MINPRNQZS + 1); //!< number of QZSS satellites */ +const unsigned int NSYSQZS = 1; #else -#define MINPRNQZS 0 -#define MAXPRNQZS 0 -#define MINPRNQZS_S 0 -#define MAXPRNQZS_S 0 -#define NSATQZS 0 -#define NSYSQZS 0 +const unsigned int MINPRNQZS = 0; +const unsigned int MAXPRNQZS = 0; +const unsigned int MINPRNQZS_S = 0; +const unsigned int MAXPRNQZS_S = 0; +const unsigned int NSATQZS = 0; +const unsigned int NSYSQZS = 0; #endif -#ifdef ENACMP -#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ -#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */ -#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */ -#define NSYSCMP 1 + +#ifdef ENABDS +const unsigned int MINPRNBDS = 1; //!< min satellite sat number of BeiDou */ +const unsigned int MAXPRNBDS = 35; //!< max satellite sat number of BeiDou */ +const unsigned int NSATBDS = (MAXPRNBDS - MINPRNCM + 1); //!< number of BeiDou satellites */ +const unsigned int NSYSBDS = 1; #else -#define MINPRNCMP 0 -#define MAXPRNCMP 0 -#define NSATCMP 0 -#define NSYSCMP 0 +const unsigned int MINPRNBDS = 0; +const unsigned int MAXPRNBDS = 0; +const unsigned int NSATBDS = 0; +const unsigned int NSYSBDS = 0; #endif + #ifdef ENAIRN -#define MINPRNIRN 1 /* min satellite sat number of IRNSS */ -#define MAXPRNIRN 7 /* max satellite sat number of IRNSS */ -#define NSATIRN (MAXPRNIRN-MINPRNIRN+1) /* number of IRNSS satellites */ -#define NSYSIRN 1 +const unsigned int MINPRNIRN = 1; //!< min satellite sat number of IRNSS */ +const unsigned int MAXPRNIRN = 7; //!< max satellite sat number of IRNSS */ +const unsigned int NSATIRN = (MAXPRNIRN - MINPRNIRN + 1); //!< number of IRNSS satellites */ +const unsigned int NSYSIRN = 1; #else -#define MINPRNIRN 0 -#define MAXPRNIRN 0 -#define NSATIRN 0 -#define NSYSIRN 0 +const unsigned int MINPRNIRN = 0; +const unsigned int MAXPRNIRN = 0; +const unsigned int NSATIRN = 0; +const unsigned int NSYSIRN = 0; #endif #ifdef ENALEO -#define MINPRNLEO 1 /* min satellite sat number of LEO */ -#define MAXPRNLEO 10 /* max satellite sat number of LEO */ -#define NSATLEO (MAXPRNLEO-MINPRNLEO+1) /* number of LEO satellites */ -#define NSYSLEO 1 +const unsigned int MINPRNLEO = 1; //!< min satellite sat number of LEO */ +const unsigned int NSATLEO = 10; //!< max satellite sat number of LEO */ +const unsigned int NSATLEO = (MAXPRNLEO - MINPRNLEO + 1); //!< number of LEO satellites */ +const unsigned int NSYSLEO = 1; #else -#define MINPRNLEO 0 -#define MAXPRNLEO 0 -#define NSATLEO 0 -#define NSYSLEO 0 +const unsigned int MINPRNLEO = 0; +const unsigned int MAXPRNLEO = 0; +const unsigned int NSATLEO = 0; +const unsigned int NSYSLEO = 0; #endif -#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP+NSYSIRN+NSYSLEO) /* number of systems */ +const unsigned int NSYS = (NSYSGPS + NSYSGLO + NSYSGAL + NSYSQZS + NSYSBDS + NSYSIRN + NSYSLEO); /* number of systems */ -#define MINPRNSBS 120 /* min satellite PRN number of SBAS */ -#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */ -#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */ +const unsigned int MINPRNSBS = 120; //!< min satellite PRN number of SBAS */ +const unsigned int MAXPRNSBS = 142; //!< max satellite PRN number of SBAS */ +const unsigned int NSATSBS = (MAXPRNSBS - MINPRNSBS + 1); //!< number of SBAS satellites */ -#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATIRN+NSATSBS+NSATLEO) +const unsigned int MAXSAT = (NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATBDS + NSATIRN + NSATSBS + NSATLEO); -#define MAXNIGP 201 /* max number of IGP in SBAS band */ - - -#define MAXCODE 55 /* max number of obs code */ - -#define MAXSTA 255 +const unsigned int MAXSTA = 255; #ifndef MAXOBS -#define MAXOBS 64 /* max number of obs in an epoch */ +const unsigned int MAXOBS = 64; //!< max number of obs in an epoch */ #endif -#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */ -#define MAXOBSTYPE 64 /* max number of obs type in RINEX */ -#define DTTOL 0.005 /* tolerance of time difference (s) */ -#define MAXDTOE 7200.0 /* max time difference to GPS Toe (s) */ -#define MAXDTOE_QZS 7200.0 /* max time difference to QZSS Toe (s) */ -#define MAXDTOE_GAL 10800.0 /* max time difference to Galileo Toe (s) */ -#define MAXDTOE_CMP 21600.0 /* max time difference to BeiDou Toe (s) */ -#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */ -#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */ -#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */ -#define MAXGDOP 300.0 /* max GDOP */ +const unsigned int MAXRCV = 64; //!< max receiver number (1 to MAXRCV) */ +const unsigned int MAXOBSTYPE = 64; //!< max number of obs type in RINEX */ +const double MAXDTOE = 7200.0; //!< max time difference to GPS Toe (s) */ +const double MAXDTOE_QZS = 7200.0; //!< max time difference to QZSS Toe (s) */ +const double MAXDTOE_GAL = 10800.0; //!< max time difference to Galileo Toe (s) */ +const double MAXDTOE_BDS = 21600.0; //!< max time difference to BeiDou Toe (s) */ +const double MAXDTOE_GLO = 1800.0; //!< max time difference to GLONASS Toe (s) */ +const double MAXDTOE_SBS = 360.0; //!< max time difference to SBAS Toe (s) */ +const double MAXDTOE_S = 86400.0; //!< max time difference to ephem toe (s) for other */ +const double MAXGDOP = 300.0; //!< max GDOP */ -#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ -#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ -#define MAXSBSURA 8 /* max URA of SBAS satellite */ -#define MAXBAND 10 /* max SBAS band of IGP */ -#define MAXNIGP 201 /* max number of IGP in SBAS band */ -#define MAXNGEO 4 /* max number of GEO satellites */ +const unsigned int MAXSBSURA = 8; //!< max URA of SBAS satellite */ +const unsigned int MAXBAND = 10; //!< max SBAS band of IGP */ +const unsigned int MAXNIGP = 201; //!< max number of IGP in SBAS band */ +const unsigned int MAXNGEO = 4; //!< max number of GEO satellites */ -#define MAXSOLMSG 8191 /* max length of solution message */ -#define MAXERRMSG 4096 /* max length of error/warning message */ +const unsigned int MAXSOLMSG = 8191; //!< max length of solution message */ +const unsigned int MAXERRMSG = 4096; //!< max length of error/warning message */ -#define IONOOPT_OFF 0 /* ionosphere option: correction off */ -#define IONOOPT_BRDC 1 /* ionosphere option: broadcast model */ -#define IONOOPT_SBAS 2 /* ionosphere option: SBAS model */ -#define IONOOPT_IFLC 3 /* ionosphere option: L1/L2 or L1/L5 iono-free LC */ -#define IONOOPT_EST 4 /* ionosphere option: estimation */ -#define IONOOPT_TEC 5 /* ionosphere option: IONEX TEC model */ -#define IONOOPT_QZS 6 /* ionosphere option: QZSS broadcast model */ -#define IONOOPT_LEX 7 /* ionosphere option: QZSS LEX ionospehre */ -#define IONOOPT_STEC 8 /* ionosphere option: SLANT TEC model */ +const unsigned int IONOOPT_OFF = 0; //!< ionosphere option: correction off */ +const unsigned int IONOOPT_BRDC = 1; //!< ionosphere option: broadcast model */ +const unsigned int IONOOPT_SBAS = 2; //!< ionosphere option: SBAS model */ +const unsigned int IONOOPT_IFLC = 3; //!< ionosphere option: L1/L2 or L1/L5 iono-free LC */ +const unsigned int IONOOPT_EST = 4; //!< ionosphere option: estimation */ +const unsigned int IONOOPT_TEC = 5; //!< ionosphere option: IONEX TEC model */ +const unsigned int IONOOPT_QZS = 6; //!< ionosphere option: QZSS broadcast model */ +const unsigned int IONOOPT_LEX = 7; //!< ionosphere option: QZSS LEX ionospehre */ +const unsigned int IONOOPT_STEC = 8; //!< ionosphere option: SLANT TEC model */ -#define TROPOPT_OFF 0 /* troposphere option: correction off */ -#define TROPOPT_SAAS 1 /* troposphere option: Saastamoinen model */ -#define TROPOPT_SBAS 2 /* troposphere option: SBAS model */ -#define TROPOPT_EST 3 /* troposphere option: ZTD estimation */ -#define TROPOPT_ESTG 4 /* troposphere option: ZTD+grad estimation */ -#define TROPOPT_ZTD 5 /* troposphere option: ZTD correction */ +const unsigned int TROPOPT_OFF = 0; //!< troposphere option: correction off */ +const unsigned int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model */ +const unsigned int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model */ +const unsigned int TROPOPT_EST = 3; //!< troposphere option: ZTD estimation */ +const unsigned int TROPOPT_ESTG = 4; //!< troposphere option: ZTD+grad estimation */ +const unsigned int TROPOPT_ZTD = 5; //!< troposphere option: ZTD correction */ -#define EPHOPT_BRDC 0 /* ephemeris option: broadcast ephemeris */ -#define EPHOPT_PREC 1 /* ephemeris option: precise ephemeris */ -#define EPHOPT_SBAS 2 /* ephemeris option: broadcast + SBAS */ -#define EPHOPT_SSRAPC 3 /* ephemeris option: broadcast + SSR_APC */ -#define EPHOPT_SSRCOM 4 /* ephemeris option: broadcast + SSR_COM */ -#define EPHOPT_LEX 5 /* ephemeris option: QZSS LEX ephemeris */ +const unsigned int EPHOPT_BRDC = 0; //!< ephemeris option: broadcast ephemeris */ +const unsigned int EPHOPT_PREC = 1; //!< ephemeris option: precise ephemeris */ +const unsigned int EPHOPT_SBAS = 2; //!< ephemeris option: broadcast + SBAS */ +const unsigned int EPHOPT_SSRAPC = 3; //!< ephemeris option: broadcast + SSR_APC */ +const unsigned int EPHOPT_SSRCOM = 4; //!< ephemeris option: broadcast + SSR_COM */ +const unsigned int EPHOPT_LEX = 5; //!< ephemeris option: QZSS LEX ephemeris */ -#define EFACT_GPS 1.0 /* error factor: GPS */ -#define EFACT_GLO 1.5 /* error factor: GLONASS */ -#define EFACT_GAL 1.0 /* error factor: Galileo */ -#define EFACT_QZS 1.0 /* error factor: QZSS */ -#define EFACT_CMP 1.0 /* error factor: BeiDou */ -#define EFACT_IRN 1.5 /* error factor: IRNSS */ -#define EFACT_SBS 3.0 /* error factor: SBAS */ +const double EFACT_GPS = 1.0; //!< error factor: GPS */ +const double EFACT_GLO = 1.5; //!< error factor: GLONASS */ +const double EFACT_GAL = 1.0; //!< error factor: Galileo */ +const double EFACT_QZS = 1.0; //!< error factor: QZSS */ +const double EFACT_BDS = 1.0; //!< error factor: BeiDou */ +const double EFACT_IRN = 1.5; //!< error factor: IRNSS */ +const double EFACT_SBS = 3.0; //!< error factor: SBAS */ -#define MAXEXFILE 1024 /* max number of expanded files */ -#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ -#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ -#define MAXSBSURA 8 /* max URA of SBAS satellite */ -#define MAXBAND 10 /* max SBAS band of IGP */ -#define MAXNIGP 201 /* max number of IGP in SBAS band */ +const unsigned int MAXEXFILE = 1024; //!< max number of expanded files */ +const double MAXSBSAGEF = 30.0; //!< max age of SBAS fast correction (s) */ +const double MAXSBSAGEL = 1800.0; //!< max age of SBAS long term corr (s) */ + typedef void fatalfunc_t(const char *); /* fatal callback function type */ + typedef struct { /* time struct */ time_t time; /* time (s) expressed by standard time_t */ double sec; /* fraction of second under 1 s */ } gtime_t; + typedef struct { /* observation data record */ gtime_t time; /* receiver sampling time (GPST) */ unsigned char sat,rcv; /* satellite/receiver number */ @@ -409,11 +353,13 @@ typedef struct { /* observation data record */ float D[NFREQ+NEXOBS]; /* observation data doppler frequency (Hz) */ } obsd_t; + typedef struct { /* observation data */ int n,nmax; /* number of obervation data/allocated */ obsd_t *data; /* observation data records */ } obs_t; + typedef struct { /* earth rotation parameter data type */ double mjd; /* mjd (days) */ double xp,yp; /* pole offset (rad) */ @@ -422,11 +368,13 @@ typedef struct { /* earth rotation parameter data type */ double lod; /* length of day (s/day) */ } erpd_t; + typedef struct { /* earth rotation parameter type */ int n,nmax; /* number and max number of data */ erpd_t *data; /* earth rotation parameter data */ } erp_t; + typedef struct { /* antenna parameter type */ int sat; /* satellite number (0:receiver) */ char type[MAXANT]; /* antenna type */ @@ -437,11 +385,13 @@ typedef struct { /* antenna parameter type */ /* el=90,85,...,0 or nadir=0,1,2,3,... (deg) */ } pcv_t; + typedef struct { /* antenna parameters type */ int n,nmax; /* number of data/allocated */ pcv_t *pcv; /* antenna parameters data */ } pcvs_t; + typedef struct { /* almanac type */ int sat; /* satellite number */ int svh; /* sv health (0:ok) */ @@ -454,14 +404,15 @@ typedef struct { /* almanac type */ double f0,f1; /* SV clock parameters (af0,af1) */ } alm_t; + typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ int sat; /* satellite number */ int iode,iodc; /* IODE,IODC */ int sva; /* SV accuracy (URA index) */ int svh; /* SV health (0:ok) */ int week; /* GPS/QZS: gps week, GAL: galileo week */ - int code; /* GPS/QZS: code on L2, GAL/CMP: data sources */ - int flag; /* GPS/QZS: L2 P data flag, CMP: nav type */ + int code; /* GPS/QZS: code on L2, GAL/BDS: data sources */ + int flag; /* GPS/QZS: L2 P data flag, BDS: nav type */ gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */ /* SV orbit parameters */ double A,e,i0,OMG0,omg,M0,deln,OMGd,idot; @@ -472,10 +423,11 @@ typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ double tgd[4]; /* group delay parameters */ /* GPS/QZS:tgd[0]=TGD */ /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ - /* CMP :tgd[0]=BGD1,tgd[1]=BGD2 */ + /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ double Adot,ndot; /* Adot,ndot for CNAV */ } eph_t; + typedef struct { /* GLONASS broadcast ephemeris type */ int sat; /* satellite number */ int iode; /* IODE (0-6 bit of tb field) */ @@ -501,6 +453,7 @@ typedef struct { /* precise ephemeris type */ float vco[MAXSAT][3]; /* satellite velocity covariance (m^2) */ } peph_t; + typedef struct { /* precise clock type */ gtime_t time; /* time (GPST) */ int index; /* clock index for multiple files */ @@ -508,6 +461,7 @@ typedef struct { /* precise clock type */ float std[MAXSAT][1]; /* satellite clock std (s) */ } pclk_t; + typedef struct { /* SBAS ephemeris type */ int sat; /* satellite number */ gtime_t t0; /* reference epoch time (GPST) */ @@ -520,6 +474,7 @@ typedef struct { /* SBAS ephemeris type */ double af0,af1; /* satellite clock-offset/drift (s,s/s) */ } seph_t; + typedef struct { /* norad two line element data type */ char name [32]; /* common name */ char alias[32]; /* alias name */ @@ -541,11 +496,13 @@ typedef struct { /* norad two line element data type */ int rev; /* revolution number at epoch */ } tled_t; + typedef struct { /* norad two line element type */ int n,nmax; /* number/max number of two line element data */ tled_t *data; /* norad two line element data */ } tle_t; + typedef struct { /* TEC grid type */ gtime_t time; /* epoch time (GPST) */ int ndata[3]; /* TEC grid data size {nlat,nlon,nhgt} */ @@ -557,23 +514,27 @@ typedef struct { /* TEC grid type */ float *rms; /* RMS values (tecu) */ } tec_t; + typedef struct { /* satellite fcb data type */ gtime_t ts,te; /* start/end time (GPST) */ double bias[MAXSAT][3]; /* fcb value (cyc) */ double std [MAXSAT][3]; /* fcb std-dev (cyc) */ } fcbd_t; + typedef struct { /* SBAS message type */ int week,tow; /* receiption time */ int prn; /* SBAS satellite PRN number */ unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ } sbsmsg_t; + typedef struct { /* SBAS messages type */ int n,nmax; /* number of SBAS messages/allocated */ sbsmsg_t *msgs; /* SBAS messages */ } sbs_t; + typedef struct { /* SBAS fast correction type */ gtime_t t0; /* time of applicability (TOF) */ double prc; /* pseudorange correction (PRC) (m) */ @@ -584,6 +545,7 @@ typedef struct { /* SBAS fast correction type */ short ai; /* degradation factor indicator */ } sbsfcorr_t; + typedef struct { /* SBAS long term satellite error correction type */ gtime_t t0; /* correction time */ int iode; /* IODE (issue of date ephemeris) */ @@ -592,12 +554,14 @@ typedef struct { /* SBAS long term satellite error correction type */ double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ } sbslcorr_t; + typedef struct { /* SBAS satellite correction type */ int sat; /* satellite number */ sbsfcorr_t fcorr; /* fast correction */ sbslcorr_t lcorr; /* long term correction */ } sbssatp_t; + typedef struct { /* SBAS satellite corrections type */ int iodp; /* IODP (issue of date mask) */ int nsat; /* number of satellites */ @@ -605,6 +569,7 @@ typedef struct { /* SBAS satellite corrections type */ sbssatp_t sat[MAXSAT]; /* satellite correction */ } sbssat_t; + typedef struct { /* SBAS ionospheric correction type */ gtime_t t0; /* correction time */ short lat,lon; /* latitude/longitude (deg) */ @@ -612,6 +577,7 @@ typedef struct { /* SBAS ionospheric correction type */ float delay; /* vertical delay estimate (m) */ } sbsigp_t; + typedef struct { /* IGP band type */ short x; /* longitude/latitude (deg) */ const short *y; /* latitudes/longitudes (deg) */ @@ -619,12 +585,14 @@ typedef struct { /* IGP band type */ unsigned char bite; /* IGP mask end bit */ } sbsigpband_t; + typedef struct { /* SBAS ionospheric corrections type */ int iodi; /* IODI (issue of date ionos corr) */ int nigp; /* number of igps */ sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ } sbsion_t; + typedef struct { /* DGPS/GNSS correction type */ gtime_t t0; /* correction time */ double prc; /* pseudorange correction (PRC) (m) */ @@ -633,6 +601,7 @@ typedef struct { /* DGPS/GNSS correction type */ double udre; /* UDRE */ } dgps_t; + typedef struct { /* SSR correction type */ gtime_t t0[6]; /* epoch time (GPST) {eph,clk,hrclk,ura,bias,pbias} */ double udi[6]; /* SSR update interval (s) */ @@ -652,6 +621,7 @@ typedef struct { /* SSR correction type */ unsigned char update; /* update flag (0:no update,1:update) */ } ssr_t; + typedef struct { /* QZSS LEX message type */ int prn; /* satellite PRN number */ int type; /* message type */ @@ -662,11 +632,13 @@ typedef struct { /* QZSS LEX message type */ unsigned char msg[212]; /* LEX message data part 1695 bits */ } lexmsg_t; + typedef struct { /* QZSS LEX messages type */ int n,nmax; /* number of LEX messages and allocated */ lexmsg_t *msgs; /* LEX messages */ } lex_t; + typedef struct { /* QZSS LEX ephemeris type */ gtime_t toe; /* epoch time (GPST) */ gtime_t tof; /* message frame time (GPST) */ @@ -682,6 +654,7 @@ typedef struct { /* QZSS LEX ephemeris type */ double isc[8]; /* ISC */ } lexeph_t; + typedef struct { /* QZSS LEX ionosphere correction type */ gtime_t t0; /* epoch time (GPST) */ double tspan; /* valid time span (s) */ @@ -689,6 +662,7 @@ typedef struct { /* QZSS LEX ionosphere correction type */ double coef[3][2]; /* coefficients lat x lon (3 x 2) */ } lexion_t; + typedef struct { /* stec data type */ gtime_t time; /* time (GPST) */ unsigned char sat; /* satellite number */ @@ -698,12 +672,14 @@ typedef struct { /* stec data type */ unsigned char flag; /* fix flag */ } stec_t; + typedef struct { /* trop data type */ gtime_t time; /* time (GPST) */ double trp[3]; /* zenith tropos delay/gradient (m) */ float std[3]; /* std-dev (m) */ } trop_t; + typedef struct { /* ppp corrections type */ int nsta; /* number of stations */ char stas[MAXSTA][8]; /* station names */ @@ -714,6 +690,7 @@ typedef struct { /* ppp corrections type */ trop_t *trop[MAXSTA]; /* trop data */ } pppcorr_t; + typedef struct { /* navigation data type */ int n,nmax; /* number of broadcast ephemeris */ int ng,ngmax; /* number of glonass ephemeris */ @@ -761,6 +738,7 @@ typedef struct { /* navigation data type */ pppcorr_t pppcorr; /* ppp corrections */ } nav_t; + typedef struct { /* station parameter type */ char name [MAXANT]; /* marker name */ char marker [MAXANT]; /* marker number */ @@ -777,6 +755,7 @@ typedef struct { /* station parameter type */ double hgt; /* antenna height (m) */ } sta_t; + typedef struct { /* solution type */ gtime_t time; /* time (GPST) */ double rr[6]; /* position/velocity (m|m/s) */ @@ -793,6 +772,7 @@ typedef struct { /* solution type */ float thres; /* AR ratio threshold for valiation */ } sol_t; + typedef struct { /* solution buffer type */ int n,nmax; /* number of solution/max number of buffer */ int cyclic; /* cyclic buffer flag */ @@ -804,6 +784,7 @@ typedef struct { /* solution buffer type */ int nb; /* number of byte in message buffer */ } solbuf_t; + typedef struct { /* solution status type */ gtime_t time; /* time (GPST) */ unsigned char sat; /* satellite number */ @@ -819,11 +800,13 @@ typedef struct { /* solution status type */ unsigned short rejc; /* reject counter */ } solstat_t; + typedef struct { /* solution status buffer type */ int n,nmax; /* number of solution/max number of buffer */ solstat_t *data; /* solution status data */ } solstatbuf_t; + typedef struct { /* RTCM control struct type */ int staid; /* station id */ int stah; /* station health */ @@ -855,6 +838,7 @@ typedef struct { /* RTCM control struct type */ char opt[256]; /* RTCM dependent options */ } rtcm_t; + typedef struct { /* download url type */ char type[32]; /* data type */ char path[1024]; /* url path */ @@ -862,6 +846,7 @@ typedef struct { /* download url type */ double tint; /* time interval (s) */ } url_t; + typedef struct { /* option type */ const char *name; /* option name */ int format; /* option format (0:int,1:double,2:string,3:enum) */ @@ -869,6 +854,7 @@ typedef struct { /* option type */ const char *comment; /* option comment/enum labels/unit */ } opt_t; + typedef struct { /* extended receiver error model */ int ena[4]; /* model enabled */ double cerr[4][NFREQ*2]; /* code errors (m) */ @@ -877,11 +863,13 @@ typedef struct { /* extended receiver error model */ double gloicb [NFREQ]; /* glonass interchannel bias (m/fn) */ } exterr_t; + typedef struct { /* SNR mask type */ int ena[2]; /* enable flag {rover,base} */ double mask[NFREQ][9]; /* mask (dBHz) at 5,10,...85 deg */ } snrmask_t; + typedef struct { /* processing options type */ int mode; /* positioning mode (PMODE_???) */ int soltype; /* solution type (0:forward,1:backward,2:combined) */ @@ -944,6 +932,7 @@ typedef struct { /* processing options type */ char pppopt[256]; /* ppp option */ } prcopt_t; + typedef struct { /* solution options type */ int posf; /* solution format (SOLF_???) */ int times; /* time system (TIMES_???) */ @@ -966,7 +955,6 @@ typedef struct { /* solution options type */ } solopt_t; - typedef struct { /* satellite status type */ unsigned char sys; /* navigation system */ unsigned char vs; /* valid satellite flag single */ @@ -990,6 +978,7 @@ typedef struct { /* satellite status type */ double ph[2][NFREQ]; /* previous carrier-phase observable (cycle) */ } ssat_t; + typedef struct { /* ambiguity control type */ gtime_t epoch[4]; /* last epoch */ int n[4]; /* number of epochs */ @@ -999,6 +988,7 @@ typedef struct { /* ambiguity control type */ char flags[MAXSAT]; /* fix flags */ } ambc_t; + typedef struct { /* RTK control/result type */ sol_t sol; /* RTK solution */ double rb[6]; /* base position/velocity (ecef) (m|m/s) */ @@ -1014,6 +1004,7 @@ typedef struct { /* RTK control/result type */ prcopt_t opt; /* processing options */ } rtk_t; + typedef struct half_cyc_tag { /* half-cycle correction list type */ unsigned char sat; /* satellite number */ unsigned char freq; /* frequency number (0:L1,1:L2,2:L5) */ @@ -1024,21 +1015,23 @@ typedef struct half_cyc_tag { /* half-cycle correction list type */ } half_cyc_t; -const double chisqr[100]={ /* chi-sqr(n) (alpha=0.001) */ - 10.8,13.8,16.3,18.5,20.5,22.5,24.3,26.1,27.9,29.6, - 31.3,32.9,34.5,36.1,37.7,39.3,40.8,42.3,43.8,45.3, - 46.8,48.3,49.7,51.2,52.6,54.1,55.5,56.9,58.3,59.7, - 61.1,62.5,63.9,65.2,66.6,68.0,69.3,70.7,72.1,73.4, - 74.7,76.0,77.3,78.6,80.0,81.3,82.6,84.0,85.4,86.7, - 88.0,89.3,90.6,91.9,93.3,94.7,96.0,97.4,98.7,100 , - 101 ,102 ,103 ,104 ,105 ,107 ,108 ,109 ,110 ,112 , - 113 ,114 ,115 ,116 ,118 ,119 ,120 ,122 ,123 ,125 , - 126 ,127 ,128 ,129 ,131 ,132 ,133 ,134 ,135 ,137 , - 138 ,139 ,140 ,142 ,143 ,144 ,145 ,147 ,148 ,149 +const double chisqr[100] = { /* chi-sqr(n) (alpha=0.001) */ + 10.8, 13.8, 16.3, 18.5, 20.5, 22.5, 24.3, 26.1, 27.9, 29.6, + 31.3, 32.9, 34.5, 36.1, 37.7, 39.3, 40.8, 42.3, 43.8, 45.3, + 46.8, 48.3, 49.7, 51.2, 52.6, 54.1, 55.5, 56.9, 58.3, 59.7, + 61.1, 62.5, 63.9, 65.2, 66.6, 68.0, 69.3, 70.7, 72.1, 73.4, + 74.7, 76.0, 77.3, 78.6, 80.0, 81.3, 82.6, 84.0, 85.4, 86.7, + 88.0, 89.3, 90.6, 91.9, 93.3, 94.7, 96.0, 97.4, 98.7, 100 , + 101 , 102 , 103 , 104 , 105 , 107 , 108 , 109 , 110 , 112 , + 113 , 114 , 115 , 116 , 118 , 119 , 120 , 122 , 123 , 125 , + 126 , 127 , 128 , 129 , 131 , 132 , 133 , 134 , 135 , 137 , + 138 , 139 , 140 , 142 , 143 , 144 , 145 , 147 , 148 , 149 }; -const double lam_carr[MAXFREQ]={ /* carrier wave length (m) */ - CLIGHT/FREQ1,CLIGHT/FREQ2,CLIGHT/FREQ5,CLIGHT/FREQ6,CLIGHT/FREQ7, - CLIGHT/FREQ8,CLIGHT/FREQ9 + + +const double lam_carr[MAXFREQ] = { /* carrier wave length (m) */ + SPEED_OF_LIGHT / FREQ1, SPEED_OF_LIGHT / FREQ2, SPEED_OF_LIGHT / FREQ5, SPEED_OF_LIGHT / FREQ6, SPEED_OF_LIGHT / FREQ7, + SPEED_OF_LIGHT / FREQ8, SPEED_OF_LIGHT / FREQ9 }; #endif diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index 12d3108f2..b84c0ea4d 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -53,158 +53,162 @@ 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); + 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.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); + 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); + tow = time2gpst(rtklib_sat.ttr,&rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc,NULL); if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} - rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); - rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + 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.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); + 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); + tow = time2gpst(rtklib_sat.ttr,&rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc,NULL); if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} - rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); - rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + 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; + 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.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); + 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); + tow = time2gpst(rtklib_sat.ttr,&rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc,NULL); if (rtklib_sat.toestow+302400.0) {rtklib_sat.week--; tow+=604800.0;} - rtklib_sat.toe=gpst2time(rtklib_sat.week,rtklib_sat.toes); - rtklib_sat.toc=gpst2time(rtklib_sat.week,toc); - rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); + rtklib_sat.toe = gpst2time(rtklib_sat.week,rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week,toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week,tow); return rtklib_sat; - } diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h index 4a9fd2374..617557a62 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -47,8 +47,10 @@ * 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_ + + +#ifndef GNSS_SDR_RTKLIB_CONVERSIONS_H_ +#define GNSS_SDR_RTKLIB_CONVERSIONS_H_ #include "rtklib_rtkcmn.h" #include "gnss_synchro.h" @@ -62,4 +64,4 @@ 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_ */ +#endif /* GNSS_SDR_RTKLIB_CONVERSIONS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 02e671fb2..14befc2a3 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -47,751 +47,824 @@ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -* history : 2010/07/28 1.1 moved from rtkcmn.c -* added api: -* eph2clk(),geph2clk(),seph2clk(),satantoff() -* satposs() -* changed api: -* eph2pos(),geph2pos(),satpos() -* deleted api: -* satposv(),satposiode() -* 2010/08/26 1.2 add ephemeris option EPHOPT_LEX -* 2010/09/09 1.3 fix problem when precise clock outage -* 2011/01/12 1.4 add api alm2pos() -* change api satpos(),satposs() -* enable valid unhealthy satellites and output status -* fix bug on exception by glonass ephem computation -* 2013/01/10 1.5 support beidou (compass) -* use newton's method to solve kepler eq. -* update ssr correction algorithm -* 2013/03/20 1.6 fix problem on ssr clock relativitic correction -* 2013/09/01 1.7 support negative pseudorange -* fix bug on variance in case of ura ssr = 63 -* 2013/11/11 1.8 change constant MAXAGESSR 70.0 -> 90.0 -* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15 1E-13 -* set MAX_ITER_KEPLER for alm2pos() -*-----------------------------------------------------------------------------*/ + * + */ #include "rtklib_ephemeris.h" -static const char rcsid[]="$Id:$"; -/* constants and macros ------------------------------------------------------*/ +/* constants ------------------------------------------------------*/ -#define SQR(x) ((x)*(x)) +const double RE_GLO = 6378136.0; /* radius of earth (m) ref [2] */ +const double MU_GPS = 3.9860050e14; /* gravitational constant ref [1] */ +const double MU_GLO = 3.9860044e14; /* gravitational constant ref [2] */ +const double MU_GAL = 3.986004418e14; /* earth gravitational constant ref [7] */ +const double MU_BDS = 3.986004418e14; /* earth gravitational constant ref [9] */ +const double J2_GLO = 1.0826257e-3; /* 2nd zonal harmonic of geopot ref [2] */ -#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] */ +const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */ +const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */ +const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */ -#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] */ +const double SIN_5 = -0.0871557427476582; /* sin(-5.0 deg) */ +const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */ -#define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */ -#define COS_5 0.9961946980917456 /* cos(-5.0 deg) */ +const double ERREPH_GLO = 5.0; /* error of glonass ephemeris (m) */ +const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */ +const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */ -#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 */ +const double DEFURASSR = 0.15; /* default accurary of ssr corr (m) */ +const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */ +const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */ +const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */ +const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */ +const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */ -#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) */ +const unsigned int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */ -#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 + const double ura_value[] = { + 2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0, + 3072.0, 6144.0 }; - return ura<0||15=63) return SQR(5.4665); - std=(pow(3.0,(ura>>3)&7)*(1.0+(ura&7)/4.0)-1.0)*1E-3; - return SQR(std); + if (ura <= 0) return std::pow(2, DEFURASSR); + if (ura >= 63) return std::pow(2, 5.4665); + std = (pow(3.0, (ura >> 3) & 7) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1e-3; + return std::pow(2, 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] -*-----------------------------------------------------------------------------*/ + * 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; + double tk, M, E, Ek, sinE, cosE, u, r, i, O, x, y, sinO, cosO, cosi, mu; int n; - - trace(4,"alm2pos : time=%s sat=%2d\n",time_str(time,3),alm->sat); - - tk=timediff(time,alm->toa); - - if (alm->A<=0.0) { - rs[0]=rs[1]=rs[2]=*dts=0.0; - return; - } - mu=satsys(alm->sat,NULL)==SYS_GAL?MU_GAL:MU_GPS; - - M=alm->M0+sqrt(mu/(alm->A*alm->A*alm->A))*tk; - for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-alm->e*cos(E)); - } - if (n>=MAX_ITER_KEPLER) { - trace(2,"alm2pos: kepler iteration overflow sat=%2d\n",alm->sat); - return; - } - sinE=sin(E); cosE=cos(E); - u=atan2(sqrt(1.0-alm->e*alm->e)*sinE,cosE-alm->e)+alm->omg; - r=alm->A*(1.0-alm->e*cosE); - i=alm->i0; - O=alm->OMG0+(alm->OMGd-OMGE)*tk-OMGE*alm->toas; - x=r*cos(u); y=r*sin(u); sinO=sin(O); cosO=cos(O); cosi=cos(i); - rs[0]=x*cosO-y*cosi*sinO; - rs[1]=x*sinO+y*cosi*cosO; - rs[2]=y*sin(i); - *dts=alm->f0+alm->f1*tk; + + 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 - DEFAULT_OMEGA_EARTH_DOT) * tk - DEFAULT_OMEGA_EARTH_DOT * 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 -*-----------------------------------------------------------------------------*/ + * 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; + + 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) -*-----------------------------------------------------------------------------*/ + * 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 *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; + 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_BDS: mu = MU_BDS; omge = OMGE_BDS; break; + default: mu = MU_GPS; omge = DEFAULT_OMEGA_EARTH_DOT; break; } - tk=timediff(time,eph->toe); - - switch ((sys=satsys(eph->sat,&prn))) { - case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break; - case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break; - default: mu=MU_GPS; omge=OMGE; break; - } - M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk; - - for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-eph->e*cos(E)); - } - if (n>=MAX_ITER_KEPLER) { - trace(2,"eph2pos: kepler iteration overflow sat=%2d\n",eph->sat); - return; - } - sinE=sin(E); cosE=cos(E); - - trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek); - - u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg; - r=eph->A*(1.0-eph->e*cosE); - i=eph->i0+eph->idot*tk; - sin2u=sin(2.0*u); cos2u=cos(2.0*u); - u+=eph->cus*sin2u+eph->cuc*cos2u; - r+=eph->crs*sin2u+eph->crc*cos2u; - i+=eph->cis*sin2u+eph->cic*cos2u; - x=r*cos(u); y=r*sin(u); cosi=cos(i); - + 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; - + if (sys == SYS_BDS && 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); - + *dts -= 2.0 * sqrt(mu * eph->A) * eph-> e* sinE / std::pow(2, SPEED_OF_LIGHT); + /* position and clock error variance */ - *var=var_uraeph(eph->sva); + *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; - } + double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = std::pow(2, 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]; + a = 1.5 * J2_GLO * MU_GLO * std::pow(2, 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]; + 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; + + 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] -*-----------------------------------------------------------------------------*/ + * 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; + + 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] -*-----------------------------------------------------------------------------*/ + * 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]; + 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]; + + trace(4, "geph2pos: time=%s sat=%2d\n", time_str(time, 3), geph->sat); + + t = timediff(time, geph->toe); + + *dts=-geph->taun+geph->gamn*t; + + for (i = 0;i<3;i++) + { + x[i ] = geph->pos[i]; + x[i+3] = geph->vel[i]; } - for (tt=t<0.0?-TSTEP:TSTEP;fabs(t)>1E-9;t-=tt) { - if (fabs(t)acc); + for (tt = t<0.0?-TSTEP:TSTEP;fabs(t)>1E-9;t-=tt) + { + if (fabs(t)acc); } - for (i=0;i<3;i++) rs[i]=x[i]; - - *var=SQR(ERREPH_GLO); + for (i = 0;i<3;i++) rs[i] = x[i]; + + *var = std::pow(2, 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] -*-----------------------------------------------------------------------------*/ + * 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; + + 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] -*-----------------------------------------------------------------------------*/ + * 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 *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); + + 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; + 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_BDS: tmax = MAXDTOE_BDS + 1.0; break; + default: tmax = MAXDTOE + 1.0; break; } - tmin=tmax+1.0; - - for (i=0;in;i++) { - if (nav->eph[i].sat!=sat) continue; - if (iode>=0&&nav->eph[i].iode!=iode) continue; - if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue; - if (iode>=0) return nav->eph+i; - if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ - } - if (iode>=0||j<0) { - trace(3,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0), - sat,iode); - return NULL; - } - return nav->eph+j; + 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;ing;i++) { - if (nav->geph[i].sat!=sat) continue; - if (iode>=0&&nav->geph[i].iode!=iode) continue; - if ((t=fabs(timediff(nav->geph[i].toe,time)))>tmax) continue; - if (iode>=0) return nav->geph+i; - if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ - } - if (iode>=0||j<0) { - trace(3,"no glonass ephemeris : %s sat=%2d iode=%2d\n",time_str(time,0), - sat,iode); - return NULL; - } - return nav->geph+j; + 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;ins;i++) { - if (nav->seph[i].sat!=sat) continue; - if ((t=fabs(timediff(nav->seph[i].t0,time)))>tmax) continue; - if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ - } - if (j<0) { - trace(3,"no sbas ephemeris : %s sat=%2d\n",time_str(time,0),sat); - return NULL; - } - return nav->seph+j; + 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) + 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); - } + + 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_BDS) + { + 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) + 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; - } + 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_BDS) + { + 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; - + 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) + 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); - + + trace(4, "satpos_sbas: time=%s sat=%2d\n", time_str(time, 3), sat); + /* search sbas satellite correciton */ - for (i=0;isbssat.nsat;i++) { - sbs=nav->sbssat.sat+i; - if (sbs->sat==sat) break; - } - if (i>=nav->sbssat.nsat) { - trace(2,"no sbas correction for orbit: %s sat=%2d\n",time_str(time,0),sat); - ephpos(time,teph,sat,nav,-1,rs,dts,var,svh); - *svh=-1; - return 0; - } + 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; - + 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; + 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) + 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; - } + 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]); - + 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; - + if (fabs(t1) > MAXAGESSR || fabs(t2) > MAXAGESSR) + { + trace(2, "age of ssr error: %s sat=%2d t=%.0f %.0f\n", time_str(time, 0), + sat, t1, t2); + *svh = -1; + return 0; + } + if (ssr->udi[0] >= 1.0) t1 -= ssr->udi[0] / 2.0; + if (ssr->udi[1] >= 1.0) t2 -= ssr->udi[0] / 2.0; + + for (i = 0; i < 3; i++) deph[i] = ssr->deph[i] + ssr->ddeph[i] * t1; + dclk = ssr->dclk[0] + ssr->dclk[1] * t2 + ssr->dclk[2] * t2 * t2; + /* ssr highrate clock correction (ref [4]) */ - if (ssr->iod[0]==ssr->iod[2]&&ssr->t0[2].time&&fabs(t3)hrclk; - } - if (norm(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) { - trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n", - time_str(time,0),norm(deph,3),dclk); - *svh=-1; - return 0; - } + 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; - + 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; - } + sys = satsys(sat, NULL); + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) + { + 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) / SPEED_OF_LIGHT / SPEED_OF_LIGHT; + } /* 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); - + 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; - + 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) / SPEED_OF_LIGHT) (ref [10] eq.3.12-7) */ + dts[0] += dclk / SPEED_OF_LIGHT; + /* 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); - + *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) -*-----------------------------------------------------------------------------*/ + * 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) + 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; + 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; + // if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1; } - *svh=-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 -*-----------------------------------------------------------------------------*/ + * 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) + int ephopt, double *rs, double *dts, double *var, int *svh) { - gtime_t time[MAXOBS]={}; - double dt,pr; - int i,j; - - trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt); - - for (i=0;i=NFREQ) { - trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat); - continue; + 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 pseudorange */ + 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 / SPEED_OF_LIGHT); + + /* 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 = std::pow(2, STD_BRDCCLK); + } } - /* 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; + 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]); } - 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 90.0 -* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15 1E-13 -* set MAX_ITER_KEPLER for alm2pos() -*-----------------------------------------------------------------------------*/ + * + *-----------------------------------------------------------------------------*/ -#ifndef RTKLIB_EPHEMERIS_H_ -#define RTKLIB_EPHEMERIS_H_ +#ifndef GNSS_SDR_RTKLIB_EPHEMERIS_H_ +#define GNSS_SDR_RTKLIB_EPHEMERIS_H_ #include "rtklib.h" #include "rtklib_rtkcmn.h" @@ -122,4 +96,4 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, -#endif /* RTKLIB_EPHEMERIS_H_ */ +#endif /* GNSS_SDR_RTKLIB_EPHEMERIS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc index 0fd99cb44..602ea3491 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.cc +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -55,462 +55,536 @@ * 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 0.0 && (value < range[0] || range[1]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; + 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; } - nav->tec=nav_tec; - } - p=nav->tec+nav->nt; - p->time=time0; - p->rb=rb; - for (i=0;i<3;i++) { - p->ndata[i]=ndata[i]; - p->lats[i]=lats[i]; - p->lons[i]=lons[i]; - p->hgts[i]=hgts[i]; - } - n=ndata[0]*ndata[1]*ndata[2]; - - if (!(p->data=(double *)malloc(sizeof(double)*n))|| - !(p->rms =(float *)malloc(sizeof(float )*n))) { - return NULL; - } - for (i=0;idata[i]=0.0; - p->rms [i]=0.0f; - } + 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;int-1;i>=0;i--) { - if (fabs(timediff(time,nav->tec[i].time))>=1.0) continue; - p=nav->tec+i; - break; + 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)); + } } - } - else if (p) p->time=time; } - else if (strstr(label,"LAT/LON1/LON2/DLON/H")==label&&p) { - lat =str2num(buff, 2,6); - lon[0]=str2num(buff, 8,6); - lon[1]=str2num(buff,14,6); - lon[2]=str2num(buff,20,6); - hgt =str2num(buff,26,6); - - i=getindex(lat,p->lats); - k=getindex(hgt,p->hgts); - n=nitem(lon); - - for (m=0;mlons); - if ((index=dataindex(i,j,k,p->ndata))<0) continue; - - if ((x=str2num(buff,m%16*5,5))==9999.0) continue; - - if (type==1) p->data[index]=x*pow(10.0,nexp); - else p->rms[index]=(float)(x*pow(10.0,nexp)); - } - } - } return 1; } + + /* combine tec grid data -----------------------------------------------------*/ void combtec(nav_t *nav) { tec_t tmp; - int i,j,n=0; - - trace(3,"combtec : nav->nt=%d\n",nav->nt); - - for (i=0;int-1;i++) { - for (j=i+1;jnt;j++) { - if (timediff(nav->tec[j].time,nav->tec[i].time)<0.0) { - tmp=nav->tec[i]; - nav->tec[i]=nav->tec[j]; - nav->tec[j]=tmp; - } + 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;int;i++) { - if (i>0&&timediff(nav->tec[i].time,nav->tec[n-1].time)==0.0) { - free(nav->tec[n-1].data); - free(nav->tec[n-1].rms ); - nav->tec[n-1]=nav->tec[i]; - continue; + 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->tec[n++]=nav->tec[i]; - } - nav->nt=n; - - trace(4,"combtec : nav->nt=%d\n",nav->nt); + 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] -*-----------------------------------------------------------------------------*/ + * 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; + 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); - + + trace(3, "readtec : file=%s\n", file); + /* clear of tec grid data option */ - if (!opt) { - free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0; - } - for (i=0;i=0;i--) free(efiles[i]); - return; + 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;int>0) combtec(nav); - + if (nav->nt > 0) combtec(nav); + /* P1-P2 dcb */ - for (i=0;icbias[i][0]=CLIGHT*dcb[i]*1E-9; /* ns->m */ - } + for (i = 0; i < MAXSAT; i++) + { + nav->cbias[i][0] = SPEED_OF_LIGHT * 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 *rms) { - double dlat,dlon,a,b,d[4]={0},r[4]={0}; - int i,j,n,index; - - trace(3,"interptec: k=%d posp=%.2f %.2f\n",k,posp[0]*R2D,posp[1]*R2D); - *value=*rms=0.0; - - if (tec->lats[2]==0.0||tec->lons[2]==0.0) return 0; - - dlat=posp[0]*R2D-tec->lats[0]; - dlon=posp[1]*R2D-tec->lons[0]; - if (tec->lons[2]>0.0) dlon-=floor( dlon/360)*360.0; /* 0<=dlon<360 */ - else dlon+=floor(-dlon/360)*360.0; /* -360lats[2]; - b=dlon/tec->lons[2]; - i=(int)floor(a); a-=i; - j=(int)floor(b); b-=j; - + double dlat, dlon, a, b, d[4] = {0}, r[4] = {0}; + int i, j, n, index; + + trace(3, "interptec: k=%d posp=%.2f %.2f\n", k, posp[0] * R2D, posp[1] * R2D); + *value = *rms = 0.0; + + if (tec->lats[2] == 0.0 || tec->lons[2] == 0.0) return 0; + + dlat = posp[0] * R2D - tec->lats[0]; + dlon = posp[1] * R2D - tec->lons[0]; + if (tec->lons[2] > 0.0) dlon -= floor( dlon / 360) * 360.0; /* 0 <= dlon<360 */ + else dlon += floor(-dlon / 360) * 360.0; /* -360lats[2]; + b = dlon / tec->lons[2]; + i = (int)floor(a); + a -= i; + j = (int)floor(b); + b -= j; + /* get gridded tec data */ - for (n=0;n<4;n++) { - if ((index=dataindex(i+(n%2),j+(n<2?0:1),k,tec->ndata))<0) continue; - d[n]=tec->data[index]; - r[n]=tec->rms [index]; - } - if (d[0]>0.0&&d[1]>0.0&&d[2]>0.0&&d[3]>0.0) { - - /* bilinear interpolation (inside of grid) */ - *value=(1.0-a)*(1.0-b)*d[0]+a*(1.0-b)*d[1]+(1.0-a)*b*d[2]+a*b*d[3]; - *rms =(1.0-a)*(1.0-b)*r[0]+a*(1.0-b)*r[1]+(1.0-a)*b*r[2]+a*b*r[3]; - } + 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; - } + 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 *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; + const double fact = 40.30E16 / FREQ1 / FREQ1; /* tecu->L1 iono (m) */ + double fs, posp[3] = {0}, vtec, rms, hion, rp; int i; - - trace(3,"iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n",time_str(time,0), - pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D); - - *delay=*var=0.0; - - for (i=0;indata[2];i++) { /* for a layer */ - - hion=tec->hgts[0]+tec->hgts[2]*i; - - /* ionospheric pierce point position */ - fs=ionppp(pos,azel,tec->rb,hion,posp); - - if (opt&2) { - /* modified single layer mapping function (M-SLM) ref [2] */ - rp=tec->rb/(tec->rb+hion)*sin(0.9782*(PI/2.0-azel[1])); - fs=1.0/sqrt(1.0-rp*rp); + + 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; } - 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)); - + trace(4, "iondelay: delay=%7.2f std=%6.2f\n", *delay, sqrt(*var)); + return 1; } + + /* ionosphere model by tec grid data ------------------------------------------- -* compute ionospheric delay by tec grid data -* args : gtime_t time I time (gpst) -* nav_t *nav I navigation data -* double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* int opt I model option -* bit0: 0:earth-fixed,1:sun-fixed -* bit1: 0:single-layer,1:modified single-layer -* double *delay O ionospheric delay (L1) (m) -* double *var O ionospheric dealy (L1) variance (m^2) -* return : status (1:ok,0:error) -* notes : before calling the function, read tec grid data by calling readtec() -* return ok with delay=0 and var=VAR_NOTEC if elnt;i++) { - if (timediff(nav->tec[i].time,time)>0.0) break; - } - if (i==0||i>=nav->nt) { - trace(2,"%s: tec grid out of period\n",time_str(time,0)); - return 0; - } - if ((tt=timediff(nav->tec[i].time,nav->tec[i-1].time))==0.0) { - trace(2,"tec grid time interval error\n"); - return 0; - } + 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)); + stat[0] = iondelay(time, nav->tec+i-1, pos, azel, opt, dels , vars ); + stat[1] = iondelay(time, nav->tec+i , pos, azel, opt, dels+1, vars+1); + + if (!stat[0] && !stat[1]) + { + trace(2, "%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n", + time_str(time, 0), pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D); + return 0; + } + if (stat[0] && stat[1]) + { /* linear interpolation by time */ + a = timediff(time, nav->tec[i-1].time) / tt; + *delay = dels[0] * (1.0 - a) + dels[1] * a; + *var = vars[0] * (1.0 - a) + vars[1] * a; + } + else if (stat[0]) + { /* nearest-neighbour extrapolation by time */ + *delay = dels[0]; + *var = vars[0]; + } + else + { + *delay = dels[1]; + *var = vars[1]; + } + trace(3, "iontec : delay=%5.2f std=%5.2f\n", *delay, sqrt(*var)); return 1; } diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.h b/src/algorithms/libs/rtklib/rtklib_ionex.h index 155b92ff9..a30ba5b10 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.h +++ b/src/algorithms/libs/rtklib/rtklib_ionex.h @@ -1,5 +1,5 @@ /*! - * \file rtklib_ionex.cc + * \file rtklib_ionex.h * \brief ionex functions * \authors
    *
  • 2007-2013, T. Takasu @@ -47,29 +47,24 @@ * 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_ + * + * 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 + * + *-----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_IONEX_H_ +#define GNSS_SDR_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) */ +const double VAR_NOTEC = 30.0 * 30.0; /* variance of no tec */ +const double MIN_EL = 0.0; /* min elevation angle (rad) */ +const double MIN_HGT = -1000.0; /* min user height (m) */ int getindex(double value, const double *range); @@ -92,4 +87,4 @@ int iondelay(gtime_t time, const tec_t *tec, const double *pos, 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_ */ +#endif /* GNSS_SDR_RTKLIB_IONEX_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 1e45c07bc..7d47085ed 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -47,19 +47,9 @@ * 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" @@ -67,638 +57,732 @@ 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; + fact = sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); + varr = std::pow(2, opt->err[0]) * (std::pow(2, opt->err[1]) + std::pow(2, opt->err[2]) / sin(el)); + if (opt->ionoopt == IONOOPT_IFLC) varr *= std::pow(2, 3.0); /* iono-free */ + return std::pow(2, fact) * varr; } + + /* get tgd parameter (m) -----------------------------------------------------*/ double gettgd(int sat, const nav_t *nav) { int i; - for (i=0;in;i++) { - if (nav->eph[i].sat!=sat) continue; - return CLIGHT*nav->eph[i].tgd[0]; - } + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * 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) + 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; - } + 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; - - /* 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) + *var = 0.0; + + if (!(sys = satsys(obs->sat, NULL))) { - 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]); + 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)) + if (iter>0) + { + if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) { - trace(4,"prange: testsnr error\n"); + 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]; - + gamma = std::pow(2, lam[j]) / std::pow(2, 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); - + 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 = std::pow(2, 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) -*-----------------------------------------------------------------------------*/ + * 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) + 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); - + 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; - } + 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); - } + 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); - } + 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; - } + if (ionoopt == IONOOPT_QZS && norm(nav->ion_qzs, 8)>0.0) + { + *ion = ionmodel(time, nav->ion_qzs, pos, azel); + *var = std::pow(2, *ion * ERR_BRDCI); + return 1; + } /* lex ionosphere model */ - //if (ionoopt==IONOOPT_LEX) { - // return lexioncorr(time,nav,pos,azel,ion,var); + //if (ionoopt == IONOOPT_LEX) { + // return lexioncorr(time, nav, pos, azel, ion, var); //} - *ion=0.0; - *var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0; + *ion = 0.0; + *var = ionoopt == IONOOPT_OFF ? std::pow(2, 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) -*-----------------------------------------------------------------------------*/ + * 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) + 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); - + 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; - } + 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; - } + 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; + *trp = 0.0; + *var = tropopt == TROPOPT_OFF ? std::pow(2, 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) + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, const prcopt_t *opt, + double *v, double *H, double *var, double *azel, int *vsat, + double *resp, int *ns) { - double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1; - int i,j,nv=0,sys,mask[4]={0}; - - trace(3,"resprng : n=%d\n",n); - - for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3]; - - ecef2pos(rr,pos); - - for (i=*ns=0;ielmin) + { + trace(4, "geodist / satazel error\n"); + continue; + } + + /* psudorange with code bias correction */ + if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0) + { + trace(4, "prange error\n"); + continue; + } + + /* excluded satellite? */ + if (satexclude(obs[i].sat, svh[i], opt)) + { + trace(4, "satexclude error\n"); + continue; + } + + /* ionospheric corrections */ + if (!ionocorr(obs[i].time, nav, obs[i].sat, pos, azel+i*2, + iter>0 ? opt->ionoopt : IONOOPT_BRDC, &dion, &vion)) + { + trace(4, "ionocorr error\n"); + continue; + } + + /* GPS-L1 -> L1/B1 */ + if ((lam_L1 = nav->lam[obs[i].sat - 1][0]) > 0.0) + { + dion *= pow(2, 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 - SPEED_OF_LIGHT * 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_BDS) {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])); } - /* geometric distance/azimuth/elevation angle */ - if ((r=geodist(rs+i*6,rr,e))<=0.0|| - satazel(pos,e,azel+i*2)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;jnx&&vv>chisqr[nv-nx-1]) { - sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]); - return 0; - } + 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;ielmin,dop); - if (dop[0]<=0.0||dop[0]>opt->maxgdop) { - sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]); - return 0; - } + 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) + 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;irr[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] / SPEED_OF_LIGHT); + sol->dtr[0] = x[3] / SPEED_OF_LIGHT; /* receiver clock bias (s) */ + sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */ + sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */ + sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* 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; + } } - /* weight by variance */ - for (j=0;jtype=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); - + 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) +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 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); } - /* 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; + if (stat) + { + time2str(obs[0].time, tstr, 2); satno2id(sat, name); + trace(2, "%s: %s excluded by raim\n", tstr + 11, name); } - for (j=nvsat=0,rms_e=0.0;jrms) continue; - - /* save result */ - for (j=k=0;jlam[obs[i].sat-1][0]; - - if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) { - continue; + 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) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (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] - SPEED_OF_LIGHT * dts[1 + i *2]); + + /* design matrix */ + for (j = 0; j < 4; j++) H[j + nv * 4] = j < 3 ? - e[j] : 1.0; + + nv++; } - /* 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) + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + const double *azel, const int *vsat) { - double x[4]={0},dx[4],Q[16],*v,*H; - int i,j,nv; - - trace(3,"estvel : n=%d\n",n); - - v=mat(n,1); H=mat(4,n); - - for (i=0;irr,x,azel,vsat,v,H))<4) { - break; - } - /* least square estimation */ - if (lsq(H,v,4,nv,dx,Q)) break; - - for (j=0;j<4;j++) x[j]+=dx[j]; - - if (norm(dx,4)<1E-6) { - for (i=0;i<3;i++) sol->rr[i+3]=x[i]; - break; - } - } - free(v); free(H); -} -/* single-point positioning ---------------------------------------------------- -* compute receiver position, velocity, clock bias by single-point positioning -* with pseudorange and doppler observables -* args : obsd_t *obs I observation data -* int n I number of observation data -* nav_t *nav I navigation data -* prcopt_t *opt I processing options -* sol_t *sol IO solution -* double *azel IO azimuth/elevation angle (rad) (NULL: no output) -* ssat_t *ssat IO satellite status (NULL: no output) -* char *msg O error message for error exit -* return : status(1:ok,0:error) -* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and -* receiver bias are negligible (only involving glonass-gps time offset -* and receiver bias) -*-----------------------------------------------------------------------------*/ -int pntpos(const obsd_t *obs, int n, const nav_t *nav, - const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, - char *msg) -{ -// int k=0; -// for (k=0;kn;k++) -// { -// printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n", -// k, -// nav->eph[k].sat, -// nav->eph[k].A, -// nav->eph[k].Adot, -// nav->eph[k].M0, -// nav->eph[k].OMG0, -// nav->eph[k].OMGd, -// nav->eph[k].cic, -// nav->eph[k].cis, -// nav->eph[k].code, -// nav->eph[k].crc, -// nav->eph[k].crs, -// nav->eph[k].cuc, -// nav->eph[k].cus, -// nav->eph[k].deln, -// nav->eph[k].e, -// nav->eph[k].f0, -// nav->eph[k].f1, -// nav->eph[k].f2, -// nav->eph[k].fit, -// nav->eph[k].flag, -// nav->eph[k].i0, -// nav->eph[k].idot, -// nav->eph[k].iodc, -// nav->eph[k].iode, -// nav->eph[k].ndot, -// nav->eph[k].omg, -// nav->eph[k].sat, -// nav->eph[k].sva, -// nav->eph[k].svh, -// nav->eph[k].tgd[0], -// nav->eph[k].toc.sec, -// nav->eph[k].toe.sec, -// nav->eph[k].toes, -// nav->eph[k].ttr.sec, -// nav->eph[k].week); -// } + double x[4] = {0}, dx[4], Q[16], *v, *H; + int i, j, nv; - 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 */ + 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;kn;k++) + // { + // printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n", + // k, + // nav->eph[k].sat, + // nav->eph[k].A, + // nav->eph[k].Adot, + // nav->eph[k].M0, + // nav->eph[k].OMG0, + // nav->eph[k].OMGd, + // nav->eph[k].cic, + // nav->eph[k].cis, + // nav->eph[k].code, + // nav->eph[k].crc, + // nav->eph[k].crs, + // nav->eph[k].cuc, + // nav->eph[k].cus, + // nav->eph[k].deln, + // nav->eph[k].e, + // nav->eph[k].f0, + // nav->eph[k].f1, + // nav->eph[k].f2, + // nav->eph[k].fit, + // nav->eph[k].flag, + // nav->eph[k].i0, + // nav->eph[k].idot, + // nav->eph[k].iodc, + // nav->eph[k].iode, + // nav->eph[k].ndot, + // nav->eph[k].omg, + // nav->eph[k].sat, + // nav->eph[k].sva, + // nav->eph[k].svh, + // nav->eph[k].tgd[0], + // nav->eph[k].toc.sec, + // nav->eph[k].toe.sec, + // nav->eph[k].toes, + // nav->eph[k].ttr.sec, + // nav->eph[k].week); + // } + + prcopt_t opt_ = *opt; + double *rs, *dts, *var, *azel_, *resp; + int i, stat, vsat[MAXOBS] = {0}, svh[MAXOBS]; + + sol->stat = SOLQ_NONE; + + if (n <= 0) {strcpy(msg, "no observation data"); return 0;} + + trace(3, "pntpos : tobs=%s n=%d\n", time_str(obs[0].time, 3), n); + + sol->time = obs[0].time; + msg[0] = '\0'; + + rs = mat(6, n); + dts = mat(2, n); + var = mat(1, n); + azel_ = zeros(2, n); + resp = mat(1, n); + + if (opt_.mode != PMODE_SINGLE) + { /* for precise positioning */ #if 0 - opt_.sateph =EPHOPT_BRDC; + opt_.sateph = EPHOPT_BRDC; #endif - opt_.ionoopt=IONOOPT_BRDC; - opt_.tropopt=TROPOPT_SAAS; - } + 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); - + 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); - + 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); - } + 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;ine>=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; + + 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_peph; - } - nav->peph[nav->ne++]=*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) + 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; + double val, std, base; + int i, j, sat, sys, prn, n = ns*(type == 'P' ? 1 : 2), pred_o, pred_c, v; char buff[1024]; - - trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt); - - while (fgets(buff,sizeof(buff),fp)) { - - if (!strncmp(buff,"EOF",3)) break; - - if (buff[0]!='*'||str2time(buff,3,28,&time)) { - trace(2,"sp3 invalid epoch %31.31s\n",buff); - continue; - } - if (!strcmp(tsys,"UTC")) time=utc2gpst(time); /* utc->gpst */ - peph.time =time; - peph.index=index; - - for (i=0;i=76&&buff[75]=='P'; - pred_o=strlen(buff)>=80&&buff[79]=='P'; - } - for (j=0;j<4;j++) { - - /* read option for predicted value */ - if (j< 3&&(opt&1)&& pred_o) continue; - if (j< 3&&(opt&2)&&!pred_o) continue; - if (j==3&&(opt&1)&& pred_c) continue; - if (j==3&&(opt&2)&&!pred_c) continue; - - val=str2num(buff, 4+j*14,14); - std=str2num(buff,61+j* 3,j<3?2:3); - - if (buff[0]=='P') { /* position */ - if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { - peph.pos[sat-1][j]=val*(j<3?1000.0:1E-6); - v=1; /* valid epoch */ - } - if ((base=bfact[j<3?0:1])>0.0&&std>0.0) { - peph.std[sat-1][j]=(float)(pow(base,std)*(j<3?1E-3:1E-12)); - } + + 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; } - 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 (!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= 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; } - } } - if (v) { - if (!addpeph(nav,&peph)) return; - } - } } + + /* compare precise ephemeris -------------------------------------------------*/ -int cmppeph(const void *p1, const void *p2) +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); + 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) +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); - + int i, j, k, m; + + trace(3, "combpeph: ne=%d\n", nav->ne); + + qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph); + if (opt&4) return; - - for (i=0,j=1;jne;j++) { - - if (fabs(timediff(nav->peph[i].time,nav->peph[j].time))<1E-9) { - - for (k=0;kpeph[j].pos[k],4)<=0.0) continue; - for (m=0;m<4;m++) nav->peph[i].pos[k][m]=nav->peph[j].pos[k][m]; - for (m=0;m<4;m++) nav->peph[i].std[k][m]=nav->peph[j].std[k][m]; - for (m=0;m<4;m++) nav->peph[i].vel[k][m]=nav->peph[j].vel[k][m]; - for (m=0;m<4;m++) nav->peph[i].vst[k][m]=nav->peph[j].vst[k][m]; - } + + 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]; } - else if (++ipeph[i]=nav->peph[j]; - } - nav->ne=i+1; - - trace(4,"combpeph: ne=%d\n",nav->ne); + 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 -*-----------------------------------------------------------------------------*/ + * read sp3 precise ephemeris/clock files and set them to navigation data + * args : char *file I sp3-c precise ephemeris file + * (wind-card * is expanded) + * nav_t *nav IO navigation data + * int opt I options (1: only observed + 2: only predicted + + * 4: not combined) + * return : none + * notes : see ref [1] + * precise ephemeris is appended and combined + * nav->peph and nav->ne must by properly initialized before calling the + * function + * only files with extensions of .sp3, .SP3, .eph* and .EPH* are read + *-----------------------------------------------------------------------------*/ void readsp3(const char *file, nav_t *nav, int opt) { FILE *fp; - gtime_t time={}; - double bfact[2]={}; - int i,j,n,ns,sats[MAXSAT]={}; - char *efiles[MAXEXFILE],*ext,type=' ',tsys[4]=""; - - trace(3,"readpephs: file=%s\n",file); - - for (i=0;i=0;i--) free(efiles[i]); - return; + 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;ine>0) combpeph(nav,opt); + 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 -*-----------------------------------------------------------------------------*/ + * 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; + pcvs_t pcvs = {}; + pcv_t pcv0 = {}, *pcv; int i; - - trace(3,"readsap : file=%s time=%s\n",file,time_str(time,0)); - - if (!readpcv(file,&pcvs)) return 0; - - for (i=0;ipcvs[i]=pcv?*pcv:pcv0; - } + + 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;irbias[i][j][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ - } + 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; } - else if ((sat=satid2no(str1))) { /* satellite dcb */ - nav->cbias[sat-1][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ + 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 * SPEED_OF_LIGHT; /* ns -> m */ + } + } + else if ((sat = satid2no(str1))) + { /* satellite dcb */ + nav->cbias[sat-1][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* 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 -*-----------------------------------------------------------------------------*/ + * read differential code bias (dcb) parameters + * args : char *file I dcb parameters file (wild-card * expanded) + * nav_t *nav IO navigation data + * sta_t *sta I station info data to inport receiver dcb + * (NULL: no use) + * return : status (1:ok,0:error) + * notes : currently only p1-c1 bias of code *.dcb file + *-----------------------------------------------------------------------------*/ int readdcb(const char *file, nav_t *nav, const sta_t *sta) { - int i,j,n; - char *efiles[MAXEXFILE]={}; - - trace(3,"readdcb : file=%s\n",file); - - for (i=0;icbias[i][j]=0.0; - } - for (i=0;i=0;i--) free(efiles[i]); - return 0; + 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; } - } - n=expath(file,efiles,MAXEXFILE); - - for (i=0;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) + 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]; + 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; } - 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; + 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; } - nav->fcb=nav_fcb; - } - for (i=0;ifcb[nav->nf].bias[i][j]=nav->fcb[nav->nf].std[i][j]=0.0; - } - for (i=0;i<3;i++) { - nav->fcb[nav->nf].bias[sat-1][i]=bias[i]; - nav->fcb[nav->nf].std [sat-1][i]=std [i]; - } - nav->fcb[nav->nf ].ts=ts; - nav->fcb[nav->nf++].te=te; + 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; + 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; - } + + 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); + 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 -*-----------------------------------------------------------------------------*/ + * read satellite fractional cycle bias (dcb) parameters + * args : char *file I fcb parameters file (wild-card * expanded) + * nav_t *nav IO navigation data + * return : status (1:ok,0:error) + * notes : fcb data appended to navigation data + *-----------------------------------------------------------------------------*/ int readfcb(const char *file, nav_t *nav) { - char *efiles[MAXEXFILE]={}; - int i,n; - - trace(3,"readfcb : file=%s\n",file); - - for (i=0;i=0;i--) free(efiles[i]); - return 0; + 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); } - } - n=expath(file,efiles,MAXEXFILE); - - for (i=0;inf>1) { - qsort(nav->fcb,nav->nf,sizeof(fcbd_t),cmpfcb); - } return 1; } + + /* polynomial interpolation by Neville's algorithm ---------------------------*/ double interppol(const double *x, double *y, int n) { - int i,j; - - for (j=1;jnepeph[0].time)<-MAXDTE|| - timediff(time,nav->peph[nav->ne-1].time)>MAXDTE) { - trace(3,"no prec ephem %s sat=%2d\n",time_str(time,0),sat); - return 0; - } - /* binary search */ - for (i=0,j=nav->ne-1;ipeph[k].time,time)<0.0) i=k+1; else j=k; - } - index=i<=0?0:i-1; - - /* polynomial interpolation for orbit */ - i=index-(NMAX+1)/2; - if (i<0) i=0; else if (i+NMAX>=nav->ne) i=nav->ne-NMAX-1; - - for (j=0;j<=NMAX;j++) { - t[j]=timediff(nav->peph[i+j].time,time); - if (norm(nav->peph[i+j].pos[sat-1],3)<=0.0) { - trace(3,"prec ephem outage %s sat=%2d\n",time_str(time,0),sat); + 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; } - } - for (j=0;j<=NMAX;j++) { - pos=nav->peph[i+j].pos[sat-1]; + /* 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]; + 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]; + /* correciton for earh rotation ver.2.4.0 */ + sinl = sin(DEFAULT_OMEGA_EARTH_DOT * t[j]); + cosl = cos(DEFAULT_OMEGA_EARTH_DOT * 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); - } + 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 * pow(2, t[0 ]) / 2.0; + else if (t[NMAX] < 0.0) std += EXTERR_EPH * pow(2, t[NMAX]) / 2.0; + *vare = pow(2, 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]; + 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] * SPEED_OF_LIGHT - 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 (t[1] >= 0.0) + { + if ((dts[0] = c[1]) != 0.0) + { + std = nav->peph[index+1].std[sat-1][3] * SPEED_OF_LIGHT + 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); + 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 = std::pow(2, std); return 1; } + + /* satellite clock by precise clock ------------------------------------------*/ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, - double *varc) + 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; - } + double t[2], c[2], std; + int i, j, k, index; + + trace(4, "pephclk : time=%s sat=%2d\n", time_str(time, 3), sat); + + if (nav->nc < 2 || + timediff(time, nav->pclk[0].time) < -MAXDTE || + timediff(time, nav->pclk[nav->nc-1].time) > MAXDTE) + { + trace(3, "no prec clock %s sat=%2d\n", time_str(time, 0), sat); + return 1; + } /* binary search */ - for (i=0,j=nav->nc-1;ipclk[k].time,time)<0.0) i=k+1; else j=k; - } - index=i<=0?0:i-1; - + 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); + 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] * SPEED_OF_LIGHT - 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] * SPEED_OF_LIGHT + 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] * SPEED_OF_LIGHT + 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 = std::pow(2, 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 -*-----------------------------------------------------------------------------*/ + * 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) + 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); - + 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); - + 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); - + 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 = std::pow(2, lam[k]) / std::pow(2, 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; - } + 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 -*-----------------------------------------------------------------------------*/ + * 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 *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; + 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||MAXSATexsats[sat-1]==1) return 1; /* excluded satellite */ - if (opt->exsats[sat-1]==2) return 0; /* included satellite */ - if (!(sys&opt->navsys)) return 1; /* unselected sat sys */ - } + + if (opt) + { + if (opt->exsats[sat-1]==1) return 1; /* excluded satellite */ + if (opt->exsats[sat-1]==2) return 0; /* included satellite */ + if (!(sys&opt->navsys)) return 1; /* unselected sat sys */ + } if (sys==SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */ - if (svh) { - trace(3,"unhealthy satellite: sat=%3d svh=%02X\n",sat,svh); - return 1; - } + if (svh) + { + trace(3,"unhealthy satellite: sat=%3d svh=%02X\n",sat,svh); + return 1; + } return 0; } + + /* test SNR mask --------------------------------------------------------------- -* test SNR mask -* args : int base I rover or base-station (0:rover,1:base station) -* int freq I frequency (0:L1,1:L2,2:L3,...) -* double el I elevation angle (rad) -* double snr I C/N0 (dBHz) -* snrmask_t *mask I SNR mask -* return : status (1:masked,0:unmasked) -*-----------------------------------------------------------------------------*/ + * test SNR mask + * args : int base I rover or base-station (0:rover,1:base station) + * int freq I frequency (0:L1,1:L2,2:L3,...) + * double el I elevation angle (rad) + * double snr I C/N0 (dBHz) + * snrmask_t *mask I SNR mask + * return : status (1:masked,0:unmasked) + *-----------------------------------------------------------------------------*/ int testsnr(int base, int freq, double el, double snr, - const snrmask_t *mask) + const snrmask_t *mask) { double minsnr,a; int i; - + if (!mask->ena[base]||freq<0||freq>=NFREQ) return 0; - + a=(el*R2D+5.0)/10.0; i=(int)floor(a); a-=i; if (i<1) minsnr=mask->mask[freq][0]; else if (i>8) minsnr=mask->mask[freq][8]; else minsnr=(1.0-a)*mask->mask[freq][i-1]+a*mask->mask[freq][i]; - + return snr>(7-i%8))&1u); return bits; } + + int getbits(const unsigned char *buff, int pos, int len) { unsigned int bits=getbitu(buff,pos,len); if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits; return (int)(bits|(~0u<>=1) { - if (data&mask) buff[i/8]|=1u<<(7-i%8); else buff[i/8]&=~(1u<<(7-i%8)); - } + for (i=pos;i>=1) + { + if (data&mask) buff[i/8]|=1u<<(7-i%8); else buff[i/8]&=~(1u<<(7-i%8)); + } } + + void setbits(unsigned char *buff, int pos, int len, int data) { if (data<0) data|=1<<(len-1); else data&=~(1<<(len-1)); /* set sign bit */ setbitu(buff,pos,len,(unsigned int)data); } + + /* crc-32 parity --------------------------------------------------------------- -* compute crc-32 parity for novatel raw -* args : unsigned char *buff I data -* int len I data length (bytes) -* return : crc-32 parity -* notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC -*-----------------------------------------------------------------------------*/ + * compute crc-32 parity for novatel raw + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-32 parity + * notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC + *-----------------------------------------------------------------------------*/ unsigned int rtk_crc32(const unsigned char *buff, int len) { unsigned int crc=0; int i,j; - + trace(4,"rtk_crc32: len=%d\n",len); - - for (i=0;i>1)^POLYCRC32; else crc>>=1; + + for (i=0;i>1)^POLYCRC32; else crc>>=1; + } } - } return crc; } + + /* crc-24q parity -------------------------------------------------------------- -* compute crc-24q parity for sbas, rtcm3 -* args : unsigned char *buff I data -* int len I data length (bytes) -* return : crc-24Q parity -* notes : see reference [2] A.4.3.3 Parity -*-----------------------------------------------------------------------------*/ + * compute crc-24q parity for sbas, rtcm3 + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-24Q parity + * notes : see reference [2] A.4.3.3 Parity + *-----------------------------------------------------------------------------*/ unsigned int rtk_crc24q(const unsigned char *buff, int len) { unsigned int crc=0; int i; - + trace(4,"rtk_crc24q: len=%d\n",len); - + for (i=0;i>16)^buff[i]]; return crc; } + + /* crc-16 parity --------------------------------------------------------------- -* compute crc-16 parity for binex, nvs -* args : unsigned char *buff I data -* int len I data length (bytes) -* return : crc-16 parity -* notes : see reference [10] A.3. -*-----------------------------------------------------------------------------*/ + * compute crc-16 parity for binex, nvs + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-16 parity + * notes : see reference [10] A.3. + *-----------------------------------------------------------------------------*/ unsigned short rtk_crc16(const unsigned char *buff, int len) { unsigned short crc=0; int i; - + trace(4,"rtk_crc16: len=%d\n",len); - - for (i=0;i>8)^buff[i])&0xFF]; - } + + for (i=0;i>8)^buff[i])&0xFF]; + } return crc; } + + /* decode navigation data word ------------------------------------------------- -* check party and decode navigation data word -* args : unsigned int word I navigation data word (2+30bit) -* (previous word D29*-30* + current word D1-30) -* unsigned char *data O decoded navigation data without parity -* (8bitx3) -* return : status (1:ok,0:parity error) -* notes : see reference [1] 20.3.5.2 user parity algorithm -*-----------------------------------------------------------------------------*/ + * check party and decode navigation data word + * args : unsigned int word I navigation data word (2+30bit) + * (previous word D29*-30* + current word D1-30) + * unsigned char *data O decoded navigation data without parity + * (8bitx3) + * return : status (1:ok,0:parity error) + * notes : see reference [1] 20.3.5.2 user parity algorithm + *-----------------------------------------------------------------------------*/ int decode_word(unsigned int word, unsigned char *data) { const unsigned int hamming[]={ - 0xBB1F3480,0x5D8F9A40,0xAEC7CD00,0x5763E680,0x6BB1F340,0x8B7A89C0 + 0xBB1F3480,0x5D8F9A40,0xAEC7CD00,0x5763E680,0x6BB1F340,0x8B7A89C0 }; unsigned int parity=0,w; int i; - + trace(5,"decodeword: word=%08x\n",word); - + if (word&0x40000000) word^=0x3FFFFFC0; - - for (i=0;i<6;i++) { - parity<<=1; - for (w=(word&hamming[i])>>6;w;w>>=1) parity^=w&1; - } + + for (i=0;i<6;i++) + { + parity<<=1; + for (w=(word&hamming[i])>>6;w;w>>=1) parity^=w&1; + } if (parity!=(word&0x3F)) return 0; - + for (i=0;i<3;i++) data[i]=(unsigned char)(word>>(22-i*8)); return 1; } + + /* new matrix ------------------------------------------------------------------ -* allocate memory of matrix -* args : int n,m I number of rows and columns of matrix -* return : matrix pointer (if n<=0 or m<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * allocate memory of matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n<=0 or m<=0, return NULL) + *-----------------------------------------------------------------------------*/ double *mat(int n, int m) { double *p; - + if (n<=0||m<=0) return NULL; - if (!(p=(double *)malloc(sizeof(double)*n*m))) { - fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); - } + if (!(p=(double *)malloc(sizeof(double)*n*m))) + { + fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); + } return p; } + + /* new integer matrix ---------------------------------------------------------- -* allocate memory of integer matrix -* args : int n,m I number of rows and columns of matrix -* return : matrix pointer (if n<=0 or m<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * allocate memory of integer matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n<=0 or m<=0, return NULL) + *-----------------------------------------------------------------------------*/ int *imat(int n, int m) { int *p; - + if (n<=0||m<=0) return NULL; - if (!(p=(int *)malloc(sizeof(int)*n*m))) { - fatalerr("integer matrix memory allocation error: n=%d,m=%d\n",n,m); - } + if (!(p=(int *)malloc(sizeof(int)*n*m))) + { + fatalerr("integer matrix memory allocation error: n=%d,m=%d\n",n,m); + } return p; } + + /* zero matrix ----------------------------------------------------------------- -* generate new zero matrix -* args : int n,m I number of rows and columns of matrix -* return : matrix pointer (if n<=0 or m<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * generate new zero matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n<=0 or m<=0, return NULL) + *-----------------------------------------------------------------------------*/ double *zeros(int n, int m) { double *p; - + #if NOCALLOC if ((p=mat(n,m))) for (n=n*m-1;n>=0;n--) p[n]=0.0; #else if (n<=0||m<=0) return NULL; if (!(p=(double *)calloc(sizeof(double),n*m))) { - fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); + fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m); } #endif return p; } + + /* identity matrix ------------------------------------------------------------- -* generate new identity matrix -* args : int n I number of rows and columns of matrix -* return : matrix pointer (if n<=0, return NULL) -*-----------------------------------------------------------------------------*/ + * generate new identity matrix + * args : int n I number of rows and columns of matrix + * return : matrix pointer (if n<=0, return NULL) + *-----------------------------------------------------------------------------*/ double *eye(int n) { double *p; int i; - + if ((p=zeros(n,n))) for (i=0;i=0) c+=a[n]*b[n]; return c; } + + /* euclid norm ----------------------------------------------------------------- -* euclid norm of vector -* args : double *a I vector a (n x 1) -* int n I size of vector a -* return : || a || -*-----------------------------------------------------------------------------*/ + * euclid norm of vector + * args : double *a I vector a (n x 1) + * int n I size of vector a + * return : || a || + *-----------------------------------------------------------------------------*/ double norm(const double *a, int n) { return sqrt(dot(a,a,n)); } + + /* outer product of 3d vectors ------------------------------------------------- -* outer product of 3d vectors -* args : double *a,*b I vector a,b (3 x 1) -* double *c O outer product (a x b) (3 x 1) -* return : none -*-----------------------------------------------------------------------------*/ + * outer product of 3d vectors + * args : double *a,*b I vector a,b (3 x 1) + * double *c O outer product (a x b) (3 x 1) + * return : none + *-----------------------------------------------------------------------------*/ void cross3(const double *a, const double *b, double *c) { c[0]=a[1]*b[2]-a[2]*b[1]; c[1]=a[2]*b[0]-a[0]*b[2]; c[2]=a[0]*b[1]-a[1]*b[0]; } + + /* normalize 3d vector --------------------------------------------------------- -* normalize 3d vector -* args : double *a I vector a (3 x 1) -* double *b O normlized vector (3 x 1) || b || = 1 -* return : status (1:ok,0:error) -*-----------------------------------------------------------------------------*/ + * normalize 3d vector + * args : double *a I vector a (3 x 1) + * double *b O normlized vector (3 x 1) || b || = 1 + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ int normv3(const double *a, double *b) { double r; @@ -902,72 +998,79 @@ int normv3(const double *a, double *b) b[2]=a[2]/r; return 1; } + + /* copy matrix ----------------------------------------------------------------- -* copy matrix -* args : double *A O destination matrix A (n x m) -* double *B I source matrix B (n x m) -* int n,m I number of rows and columns of matrix -* return : none -*-----------------------------------------------------------------------------*/ + * copy matrix + * args : double *A O destination matrix A (n x m) + * double *B I source matrix B (n x m) + * int n,m I number of rows and columns of matrix + * return : none + *-----------------------------------------------------------------------------*/ void matcpy(double *A, const double *B, int n, int m) { memcpy(A,B,sizeof(double)*n*m); } + /* matrix routines -----------------------------------------------------------*/ /* multiply matrix (wrapper of blas dgemm) ------------------------------------- -* multiply matrix by matrix (C=alpha*A*B+beta*C) -* args : char *tr I transpose flags ("N":normal,"T":transpose) -* int n,k,m I size of (transposed) matrix A,B -* double alpha I alpha -* double *A,*B I (transposed) matrix A (n x m), B (m x k) -* double beta I beta -* double *C IO matrix C (n x k) -* return : none -*-----------------------------------------------------------------------------*/ + * multiply matrix by matrix (C=alpha*A*B+beta*C) + * args : char *tr I transpose flags ("N":normal,"T":transpose) + * int n,k,m I size of (transposed) matrix A,B + * double alpha I alpha + * double *A,*B I (transposed) matrix A (n x m), B (m x k) + * double beta I beta + * double *C IO matrix C (n x k) + * return : none + *-----------------------------------------------------------------------------*/ void matmul(const char *tr, int n, int k, int m, double alpha, - const double *A, const double *B, double beta, double *C) + const double *A, const double *B, double beta, double *C) { int lda=tr[0]=='T'?m:n,ldb=tr[1]=='T'?k:m; - + dgemm_((char *)tr,(char *)tr+1,&n,&k,&m,&alpha,(double *)A,&lda,(double *)B, - &ldb,&beta,C,&n); + &ldb,&beta,C,&n); } + + /* inverse of matrix ----------------------------------------------------------- -* inverse of matrix (A=A^-1) -* args : double *A IO matrix (n x n) -* int n I size of matrix A -* return : status (0:ok,0>:error) -*-----------------------------------------------------------------------------*/ + * inverse of matrix (A=A^-1) + * args : double *A IO matrix (n x n) + * int n I size of matrix A + * return : status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ int matinv(double *A, int n) { double *work; int info,lwork=n*16,*ipiv=imat(n,1); - + work=mat(lwork,1); dgetrf_(&n,&n,A,&n,ipiv,&info); if (!info) dgetri_(&n,A,&n,ipiv,work,&lwork,&info); free(ipiv); free(work); return info; } + + /* solve linear equation ------------------------------------------------------- -* solve linear equation (X=A\Y or X=A'\Y) -* args : char *tr I transpose flag ("N":normal,"T":transpose) -* double *A I input matrix A (n x n) -* double *Y I input matrix Y (n x m) -* int n,m I size of matrix A,Y -* double *X O X=A\Y or X=A'\Y (n x m) -* return : status (0:ok,0>:error) -* notes : matirix stored by column-major order (fortran convention) -* X can be same as Y -*-----------------------------------------------------------------------------*/ + * solve linear equation (X=A\Y or X=A'\Y) + * args : char *tr I transpose flag ("N":normal,"T":transpose) + * double *A I input matrix A (n x n) + * double *Y I input matrix Y (n x m) + * int n,m I size of matrix A,Y + * double *X O X=A\Y or X=A'\Y (n x m) + * return : status (0:ok,0>:error) + * notes : matirix stored by column-major order (fortran convention) + * X can be same as Y + *-----------------------------------------------------------------------------*/ int solve(const char *tr, const double *A, const double *Y, int n, - int m, double *X) + int m, double *X) { double *B=mat(n,n); int info,*ipiv=imat(n,1); - + matcpy(B,A,n,n); matcpy(X,Y,n,m); dgetrf_(&n,&n,B,&n,ipiv,&info); @@ -980,22 +1083,22 @@ int solve(const char *tr, const double *A, const double *Y, int n, /* end of matrix routines ----------------------------------------------------*/ /* least square estimation ----------------------------------------------------- -* least square estimation by solving normal equation (x=(A*A')^-1*A*y) -* args : double *A I transpose of (weighted) design matrix (n x m) -* double *y I (weighted) measurements (m x 1) -* int n,m I number of parameters and measurements (n<=m) -* double *x O estmated parameters (n x 1) -* double *Q O esimated parameters covariance matrix (n x n) -* return : status (0:ok,0>:error) -* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2)) -* matirix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ + * least square estimation by solving normal equation (x=(A*A')^-1*A*y) + * args : double *A I transpose of (weighted) design matrix (n x m) + * double *y I (weighted) measurements (m x 1) + * int n,m I number of parameters and measurements (n<=m) + * double *x O estmated parameters (n x 1) + * double *Q O esimated parameters covariance matrix (n x n) + * return : status (0:ok,0>:error) + * notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2)) + * matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ int lsq(const double *A, const double *y, int n, int m, double *x, - double *Q) + double *Q) { double *Ay; int info; - + if (m0.0) ix[k++]=i; x_=mat(k,1); xp_=mat(k,1); P_=mat(k,k); Pp_=mat(k,k); H_=mat(k,m); - for (i=0;i:error) -* notes : see reference [4] 5.2 -* matirix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ + * combine forward and backward filters by fixed-interval smoother as follows: + * + * xs=Qs*(Qf^-1*xf+Qb^-1*xb), Qs=(Qf^-1+Qb^-1)^-1) + * + * args : double *xf I forward solutions (n x 1) + * args : double *Qf I forward solutions covariance matrix (n x n) + * double *xb I backward solutions (n x 1) + * double *Qb I backward solutions covariance matrix (n x n) + * int n I number of solutions + * double *xs O smoothed solutions (n x 1) + * double *Qs O smoothed solutions covariance matrix (n x n) + * return : status (0:ok,0>:error) + * notes : see reference [4] 5.2 + * matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ int smoother(const double *xf, const double *Qf, const double *xb, - const double *Qb, int n, double *xs, double *Qs) + const double *Qb, int n, double *xs, double *Qs) { double *invQf=mat(n,n),*invQb=mat(n,n),*xx=mat(n,1); int i,info=-1; - + matcpy(invQf,Qf,n,n); matcpy(invQb,Qb,n,n); - if (!matinv(invQf,n)&&!matinv(invQb,n)) { - for (i=0;i=0;s++) *p++=*s=='d'||*s=='D'?'E':*s; *p='\0'; return sscanf(str,"%lf",&value)==1?value:0.0; } + + /* string to time -------------------------------------------------------------- -* convert substring in string to gtime_t struct -* args : char *s I string ("... yyyy mm dd hh mm ss ...") -* int i,n I substring position and width -* gtime_t *t O gtime_t struct -* return : status (0:ok,0>:error) -*-----------------------------------------------------------------------------*/ + * convert substring in string to gtime_t struct + * args : char *s I string ("... yyyy mm dd hh mm ss ...") + * int i,n I substring position and width + * gtime_t *t O gtime_t struct + * return : status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ int str2time(const char *s, int i, int n, gtime_t *t) { double ep[6]; char str[256],*p=str; - + if (i<0||(int)strlen(s)=0;) *p++=*s++; *p='\0'; if (sscanf(str,"%lf %lf %lf %lf %lf %lf",ep,ep+1,ep+2,ep+3,ep+4,ep+5)<6) @@ -1154,20 +1276,22 @@ int str2time(const char *s, int i, int n, gtime_t *t) *t=epoch2time(ep); return 0; } + + /* convert calendar day/time to time ------------------------------------------- -* convert calendar day/time to gtime_t struct -* args : double *ep I day/time {year,month,day,hour,min,sec} -* return : gtime_t struct -* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) -*-----------------------------------------------------------------------------*/ + * convert calendar day/time to gtime_t struct + * args : double *ep I day/time {year,month,day,hour,min,sec} + * return : gtime_t struct + * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) + *-----------------------------------------------------------------------------*/ gtime_t epoch2time(const double *ep) { const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335}; gtime_t time={}; int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2]; - + if (year<1970||2099=3?1:0); sec=(int)floor(ep[5]); @@ -1175,305 +1299,346 @@ gtime_t epoch2time(const double *ep) time.sec=ep[5]-sec; return time; } + + /* time to calendar day/time --------------------------------------------------- -* convert gtime_t struct to calendar day/time -* args : gtime_t t I gtime_t struct -* double *ep O day/time {year,month,day,hour,min,sec} -* return : none -* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) -*-----------------------------------------------------------------------------*/ + * convert gtime_t struct to calendar day/time + * args : gtime_t t I gtime_t struct + * double *ep O day/time {year,month,day,hour,min,sec} + * return : none + * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) + *-----------------------------------------------------------------------------*/ void time2epoch(gtime_t t, double *ep) { const int mday[]={ /* # of days in a month */ - 31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31, - 31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31 + 31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31, + 31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31 }; int days,sec,mon,day; - + /* leap year if year%4==0 in 1901-2099 */ days=(int)(t.time/86400); sec=(int)(t.time-(time_t)days*86400); - for (day=days%1461,mon=0;mon<48;mon++) { - if (day>=mday[mon]) day-=mday[mon]; else break; - } + for (day=days%1461,mon=0;mon<48;mon++) + { + if (day>=mday[mon]) day-=mday[mon]; else break; + } ep[0]=1970+days/1461*4+mon/12; ep[1]=mon%12+1; ep[2]=day+1; ep[3]=sec/3600; ep[4]=sec%3600/60; ep[5]=sec%60+t.sec; } + + /* gps time to time ------------------------------------------------------------ -* convert week and tow in gps time to gtime_t struct -* args : int week I week number in gps time -* double sec I time of week in gps time (s) -* return : gtime_t struct -*-----------------------------------------------------------------------------*/ + * convert week and tow in gps time to gtime_t struct + * args : int week I week number in gps time + * double sec I time of week in gps time (s) + * return : gtime_t struct + *-----------------------------------------------------------------------------*/ gtime_t gpst2time(int week, double sec) { gtime_t t=epoch2time(gpst0); - - if (sec<-1E9||1E9tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday; - ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1E-6; - } + + if (!gettimeofday(&tv,NULL)&&(tt=gmtime(&tv.tv_sec))) + { + ep[0]=tt->tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday; + ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1e-6; + } time=epoch2time(ep); - + #ifdef CPUTIME_IN_GPST /* cputime operated in gpst */ time=gpst2utc(time); #endif return time; } + + /* set current time in utc ----------------------------------------------------- -* set current time in utc -* args : gtime_t I current time in utc -* return : none -* notes : just set time offset between cpu time and current time -* the time offset is reflected to only timeget() -* not reentrant -*----------------------------------------------------------------------------- + * set current time in utc + * args : gtime_t I current time in utc + * return : none + * notes : just set time offset between cpu time and current time + * the time offset is reflected to only timeget() + * not reentrant + *----------------------------------------------------------------------------- void timeset(gtime_t t) { timeoffset_+=timediff(t,timeget()); } -*/ + */ /* read leap seconds table by text -------------------------------------------*/ int read_leaps_text(FILE *fp) { char buff[256],*p; int i,n=0,ep[6],ls; - + rewind(fp); - - while (fgets(buff,sizeof(buff),fp)&&n=13) continue; - ls[n][0]=y; - ls[n][1]=m; - ls[n][2]=d; - ls[n++][6]=(char)(19.0-tai_utc); - } - for (i=0;i=13) continue; + ls[n][0]=y; + ls[n][1]=m; + ls[n][2]=d; + ls[n++][6]=(char)(19.0-tai_utc); + } + for (i=0;i0;i++) { - tu=timeadd(t,leaps[i][6]); - if (timediff(tu,epoch2time(leaps[i]))>=0.0) return tu; + tu=timeadd(t,leaps[i][6]); + if (timediff(tu,epoch2time(leaps[i]))>=0.0) return tu; } return t; } + + /* utc to gpstime -------------------------------------------------------------- -* convert utc to gpstime considering leap seconds -* args : gtime_t t I time expressed in utc -* return : time expressed in gpstime -* notes : ignore slight time offset under 100 ns -*-----------------------------------------------------------------------------*/ + * convert utc to gpstime considering leap seconds + * args : gtime_t t I time expressed in utc + * return : time expressed in gpstime + * notes : ignore slight time offset under 100 ns + *-----------------------------------------------------------------------------*/ gtime_t utc2gpst(gtime_t t) { int i; - + for (i=0;leaps[i][0]>0;i++) { - if (timediff(t,epoch2time(leaps[i]))>=0.0) return timeadd(t,-leaps[i][6]); + if (timediff(t,epoch2time(leaps[i]))>=0.0) return timeadd(t,-leaps[i][6]); } return t; } + + /* gpstime to bdt -------------------------------------------------------------- -* convert gpstime to bdt (beidou navigation satellite system time) -* args : gtime_t t I time expressed in gpstime -* return : time expressed in bdt -* notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC -* no leap seconds in BDT -* ignore slight time offset under 100 ns -*-----------------------------------------------------------------------------*/ + * convert gpstime to bdt (beidou navigation satellite system time) + * args : gtime_t t I time expressed in gpstime + * return : time expressed in bdt + * notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC + * no leap seconds in BDT + * ignore slight time offset under 100 ns + *-----------------------------------------------------------------------------*/ gtime_t gpst2bdt(gtime_t t) { return timeadd(t,-14.0); } + + /* bdt to gpstime -------------------------------------------------------------- -* convert bdt (beidou navigation satellite system time) to gpstime -* args : gtime_t t I time expressed in bdt -* return : time expressed in gpstime -* notes : see gpst2bdt() -*-----------------------------------------------------------------------------*/ + * convert bdt (beidou navigation satellite system time) to gpstime + * args : gtime_t t I time expressed in bdt + * return : time expressed in gpstime + * notes : see gpst2bdt() + *-----------------------------------------------------------------------------*/ gtime_t bdt2gpst(gtime_t t) { return timeadd(t,14.0); } + + /* time to day and sec -------------------------------------------------------*/ double time2sec(gtime_t time, gtime_t *day) { @@ -1484,75 +1649,85 @@ double time2sec(gtime_t time, gtime_t *day) *day=epoch2time(ep); return sec; } + + /* utc to gmst ----------------------------------------------------------------- -* convert utc to gmst (Greenwich mean sidereal time) -* args : gtime_t t I time expressed in utc -* double ut1_utc I UT1-UTC (s) -* return : gmst (rad) -*-----------------------------------------------------------------------------*/ + * convert utc to gmst (Greenwich mean sidereal time) + * args : gtime_t t I time expressed in utc + * double ut1_utc I UT1-UTC (s) + * return : gmst (rad) + *-----------------------------------------------------------------------------*/ double utc2gmst(gtime_t t, double ut1_utc) { const double ep2000[]={2000,1,1,12,0,0}; gtime_t tut,tut0; double ut,t1,t2,t3,gmst0,gmst; - + tut=timeadd(t,ut1_utc); ut=time2sec(tut,&tut0); t1=timediff(tut0,epoch2time(ep2000))/86400.0/36525.0; t2=t1*t1; t3=t2*t1; gmst0=24110.54841+8640184.812866*t1+0.093104*t2-6.2E-6*t3; gmst=gmst0+1.002737909350795*ut; - + return fmod(gmst,86400.0)*PI/43200.0; /* 0 <= gmst <= 2*PI */ } + + /* time to string -------------------------------------------------------------- -* convert gtime_t struct to string -* args : gtime_t t I gtime_t struct -* char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") -* int n I number of decimals -* return : none -*-----------------------------------------------------------------------------*/ + * convert gtime_t struct to string + * args : gtime_t t I gtime_t struct + * char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") + * int n I number of decimals + * return : none + *-----------------------------------------------------------------------------*/ void time2str(gtime_t t, char *s, int n) { double ep[6]; - + if (n<0) n=0; else if (n>12) n=12; if (1.0-t.sec<0.5/pow(10.0,n)) {t.time++; t.sec=0.0;}; time2epoch(t,ep); sprintf(s,"%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f",ep[0],ep[1],ep[2], ep[3],ep[4],n<=0?2:n+3,n<=0?0:n,ep[5]); } + + /* get time string ------------------------------------------------------------- -* get time string -* args : gtime_t t I gtime_t struct -* int n I number of decimals -* return : time string -* notes : not reentrant, do not use multiple in a function -*-----------------------------------------------------------------------------*/ + * get time string + * args : gtime_t t I gtime_t struct + * int n I number of decimals + * return : time string + * notes : not reentrant, do not use multiple in a function + *-----------------------------------------------------------------------------*/ char *time_str(gtime_t t, int n) { static char buff[64]; time2str(t,buff,n); return buff; } + + /* time to day of year --------------------------------------------------------- -* convert time to day of year -* args : gtime_t t I gtime_t struct -* return : day of year (days) -*-----------------------------------------------------------------------------*/ + * convert time to day of year + * args : gtime_t t I gtime_t struct + * return : day of year (days) + *-----------------------------------------------------------------------------*/ double time2doy(gtime_t t) { double ep[6]; - + time2epoch(t,ep); ep[1]=ep[2]=1.0; ep[3]=ep[4]=ep[5]=0.0; return timediff(t,epoch2time(ep))/86400.0+1.0; } + + /* adjust gps week number ------------------------------------------------------ -* adjust gps week number using cpu time -* args : int week I not-adjusted gps week number -* return : adjusted gps week number -*-----------------------------------------------------------------------------*/ + * adjust gps week number using cpu time + * args : int week I not-adjusted gps week number + * return : adjusted gps week number + *-----------------------------------------------------------------------------*/ int adjgpsweek(int week) { int w; @@ -1560,52 +1735,58 @@ int adjgpsweek(int week) if (w<1560) w=1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */ return week+(w-week+512)/1024*1024; } + + /* get tick time --------------------------------------------------------------- -* get current tick in ms -* args : none -* return : current tick in ms -*-----------------------------------------------------------------------------*/ + * get current tick in ms + * args : none + * return : current tick in ms + *-----------------------------------------------------------------------------*/ unsigned int tickget(void) { - struct timespec tp={}; struct timeval tv={}; - + #ifdef CLOCK_MONOTONIC_RAW /* linux kernel > 2.6.28 */ - if (!clock_gettime(CLOCK_MONOTONIC_RAW,&tp)) { - return tp.tv_sec*1000u+tp.tv_nsec/1000000u; - } - else { - gettimeofday(&tv,NULL); - return tv.tv_sec*1000u+tv.tv_usec/1000u; - } + if (!clock_gettime(CLOCK_MONOTONIC_RAW,&tp)) + { + return tp.tv_sec*1000u+tp.tv_nsec/1000000u; + } + else + { + gettimeofday(&tv,NULL); + return tv.tv_sec*1000u+tv.tv_usec/1000u; + } #else gettimeofday(&tv,NULL); return tv.tv_sec*1000u+tv.tv_usec/1000u; #endif } + + /* sleep ms -------------------------------------------------------------------- -* sleep ms -* args : int ms I miliseconds to sleep (<0:no sleep) -* return : none -*-----------------------------------------------------------------------------*/ + * sleep ms + * args : int ms I miliseconds to sleep (<0:no sleep) + * return : none + *-----------------------------------------------------------------------------*/ void sleepms(int ms) { - struct timespec ts; if (ms<=0) return; ts.tv_sec=(time_t)(ms/1000); ts.tv_nsec=(long)(ms%1000*1000000); nanosleep(&ts,NULL); } + + /* convert degree to deg-min-sec ----------------------------------------------- -* convert degree to degree-minute-second -* args : double deg I degree -* double *dms O degree-minute-second {deg,min,sec} -* int ndec I number of decimals of second -* return : none -*-----------------------------------------------------------------------------*/ + * convert degree to degree-minute-second + * args : double deg I degree + * double *dms O degree-minute-second {deg,min,sec} + * int ndec I number of decimals of second + * return : none + *-----------------------------------------------------------------------------*/ void deg2dms(double deg, double *dms, int ndec) { double sign=deg<0.0?-1.0:1.0,a=fabs(deg); @@ -1613,132 +1794,151 @@ void deg2dms(double deg, double *dms, int ndec) dms[0]=floor(a); a=(a-dms[0])*60.0; dms[1]=floor(a); a=(a-dms[1])*60.0; dms[2]=floor(a/unit+0.5)*unit; - if (dms[2]>=60.0) { - dms[2]=0.0; - dms[1]+=1.0; - if (dms[1]>=60.0) { - dms[1]=0.0; - dms[0]+=1.0; + if (dms[2]>=60.0) + { + dms[2]=0.0; + dms[1]+=1.0; + if (dms[1]>=60.0) + { + dms[1]=0.0; + dms[0]+=1.0; + } } - } dms[0]*=sign; } + + /* convert deg-min-sec to degree ----------------------------------------------- -* convert degree-minute-second to degree -* args : double *dms I degree-minute-second {deg,min,sec} -* return : degree -*-----------------------------------------------------------------------------*/ + * convert degree-minute-second to degree + * args : double *dms I degree-minute-second {deg,min,sec} + * return : degree + *-----------------------------------------------------------------------------*/ double dms2deg(const double *dms) { double sign=dms[0]<0.0?-1.0:1.0; return sign*(fabs(dms[0])+dms[1]/60.0+dms[2]/3600.0); } + + /* transform ecef to geodetic postion ------------------------------------------ -* transform ecef position to geodetic position -* args : double *r I ecef position {x,y,z} (m) -* double *pos O geodetic position {lat,lon,h} (rad,m) -* return : none -* notes : WGS84, ellipsoidal height -*-----------------------------------------------------------------------------*/ + * transform ecef position to geodetic position + * args : double *r I ecef position {x,y,z} (m) + * double *pos O geodetic position {lat,lon,h} (rad,m) + * return : none + * notes : WGS84, ellipsoidal height + *-----------------------------------------------------------------------------*/ void ecef2pos(const double *r, double *pos) { double e2=FE_WGS84*(2.0-FE_WGS84),r2=dot(r,r,2),z,zk,v=RE_WGS84,sinp; - - for (z=r[2],zk=0.0;fabs(z-zk)>=1E-4;) { - zk=z; - sinp=z/sqrt(r2+z*z); - v=RE_WGS84/sqrt(1.0-e2*sinp*sinp); - z=r[2]+v*e2*sinp; - } - pos[0]=r2>1E-12?atan(z/sqrt(r2)):(r[2]>0.0?PI/2.0:-PI/2.0); - pos[1]=r2>1E-12?atan2(r[1],r[0]):0.0; + + for (z=r[2],zk=0.0;fabs(z-zk)>=1e-4;) + { + zk=z; + sinp=z/sqrt(r2+z*z); + v=RE_WGS84/sqrt(1.0-e2*sinp*sinp); + z=r[2]+v*e2*sinp; + } + pos[0]=r2>1e-12?atan(z/sqrt(r2)):(r[2]>0.0?PI/2.0:-PI/2.0); + pos[1]=r2>1e-12?atan2(r[1],r[0]):0.0; pos[2]=sqrt(r2+z*z)-v; } + + /* transform geodetic to ecef position ----------------------------------------- -* transform geodetic position to ecef position -* args : double *pos I geodetic position {lat,lon,h} (rad,m) -* double *r O ecef position {x,y,z} (m) -* return : none -* notes : WGS84, ellipsoidal height -*-----------------------------------------------------------------------------*/ + * transform geodetic position to ecef position + * args : double *pos I geodetic position {lat,lon,h} (rad,m) + * double *r O ecef position {x,y,z} (m) + * return : none + * notes : WGS84, ellipsoidal height + *-----------------------------------------------------------------------------*/ void pos2ecef(const double *pos, double *r) { double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]); double e2=FE_WGS84*(2.0-FE_WGS84),v=RE_WGS84/sqrt(1.0-e2*sinp*sinp); - + r[0]=(v+pos[2])*cosp*cosl; r[1]=(v+pos[2])*cosp*sinl; r[2]=(v*(1.0-e2)+pos[2])*sinp; } + + /* ecef to local coordinate transfromation matrix ------------------------------ -* compute ecef to local coordinate transfromation matrix -* args : double *pos I geodetic position {lat,lon} (rad) -* double *E O ecef to local coord transformation matrix (3x3) -* return : none -* notes : matirix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ + * compute ecef to local coordinate transfromation matrix + * args : double *pos I geodetic position {lat,lon} (rad) + * double *E O ecef to local coord transformation matrix (3x3) + * return : none + * notes : matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ void xyz2enu(const double *pos, double *E) { double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]); - + E[0]=-sinl; E[3]=cosl; E[6]=0.0; E[1]=-sinp*cosl; E[4]=-sinp*sinl; E[7]=cosp; E[2]=cosp*cosl; E[5]=cosp*sinl; E[8]=sinp; } + + /* transform ecef vector to local tangental coordinate ------------------------- -* transform ecef vector to local tangental coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *r I vector in ecef coordinate {x,y,z} -* double *e O vector in local tangental coordinate {e,n,u} -* return : none -*-----------------------------------------------------------------------------*/ + * transform ecef vector to local tangental coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *r I vector in ecef coordinate {x,y,z} + * double *e O vector in local tangental coordinate {e,n,u} + * return : none + *-----------------------------------------------------------------------------*/ void ecef2enu(const double *pos, const double *r, double *e) { double E[9]; - + xyz2enu(pos,E); matmul("NN",3,1,3,1.0,E,r,0.0,e); } + + /* transform local vector to ecef coordinate ----------------------------------- -* transform local tangental coordinate vector to ecef -* args : double *pos I geodetic position {lat,lon} (rad) -* double *e I vector in local tangental coordinate {e,n,u} -* double *r O vector in ecef coordinate {x,y,z} -* return : none -*-----------------------------------------------------------------------------*/ + * transform local tangental coordinate vector to ecef + * args : double *pos I geodetic position {lat,lon} (rad) + * double *e I vector in local tangental coordinate {e,n,u} + * double *r O vector in ecef coordinate {x,y,z} + * return : none + *-----------------------------------------------------------------------------*/ void enu2ecef(const double *pos, const double *e, double *r) { double E[9]; - + xyz2enu(pos,E); matmul("TN",3,1,3,1.0,E,e,0.0,r); } + + /* transform covariance to local tangental coordinate -------------------------- -* transform ecef covariance to local tangental coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *P I covariance in ecef coordinate -* double *Q O covariance in local tangental coordinate -* return : none -*-----------------------------------------------------------------------------*/ + * transform ecef covariance to local tangental coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *P I covariance in ecef coordinate + * double *Q O covariance in local tangental coordinate + * return : none + *-----------------------------------------------------------------------------*/ void covenu(const double *pos, const double *P, double *Q) { double E[9],EP[9]; - + xyz2enu(pos,E); matmul("NN",3,3,3,1.0,E,P,0.0,EP); matmul("NT",3,3,3,1.0,EP,E,0.0,Q); } + + /* transform local enu coordinate covariance to xyz-ecef ----------------------- -* transform local enu covariance to xyz-ecef coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *Q I covariance in local enu coordinate -* double *P O covariance in xyz-ecef coordinate -* return : none -*-----------------------------------------------------------------------------*/ + * transform local enu covariance to xyz-ecef coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *Q I covariance in local enu coordinate + * double *P O covariance in xyz-ecef coordinate + * return : none + *-----------------------------------------------------------------------------*/ void covecef(const double *pos, const double *Q, double *P) { double E[9],EQ[9]; - + xyz2enu(pos,E); matmul("TN",3,3,3,1.0,E,Q,0.0,EQ); matmul("NN",3,3,3,1.0,EQ,E,0.0,P); @@ -1749,158 +1949,164 @@ void covecef(const double *pos, const double *Q, double *P) void ast_args(double t, double *f) { static const double fc[][5]={ /* coefficients for iau 1980 nutation */ - { 134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470}, - { 357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149}, - { 93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417}, - { 297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169}, - { 125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939} + { 134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470}, + { 357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149}, + { 93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417}, + { 297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169}, + { 125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939} }; double tt[4]; int i,j; - + for (tt[0]=t,i=1;i<4;i++) tt[i]=tt[i-1]*t; - for (i=0;i<5;i++) { - f[i]=fc[i][0]*3600.0; - for (j=0;j<4;j++) f[i]+=fc[i][j+1]*tt[j]; - f[i]=fmod(f[i]*AS2R,2.0*PI); - } + for (i=0;i<5;i++) + { + f[i]=fc[i][0]*3600.0; + for (j=0;j<4;j++) f[i]+=fc[i][j+1]*tt[j]; + f[i]=fmod(f[i]*AS2R,2.0*PI); + } } + + /* iau 1980 nutation ---------------------------------------------------------*/ void nut_iau1980(double t, const double *f, double *dpsi, double *deps) { static const double nut[106][10]={ - { 0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9}, - { 0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1}, - { 0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5}, - { 0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5}, - { 0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1}, - { 1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0}, - { 0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6}, - { 0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0}, - { 1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1}, - { 0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3}, - { -1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0}, - { 0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0}, - { -1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0}, - { 1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0}, - { 0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0}, - { -1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0}, - { -1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0}, - { 1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0}, - { -2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0}, - { -2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0}, - { 0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0}, - { 2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0}, - { 2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0}, - { 1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0}, - { 0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0}, - { 0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0}, - { -1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0}, - { 0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0}, - { 0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0}, - { -1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0}, - { 0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0}, - { 1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0}, - { 0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0}, - { 2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0}, - { -1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0}, - { 1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0}, - { 0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0}, - { 0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0}, - { 1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0}, - { 0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0}, - { -2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0}, - { 0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0}, - { 2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0}, - { 1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0}, - { 1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0}, - { 0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0}, - { 0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0}, - { 2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0}, - { 1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0}, - { 1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0}, - { 0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0}, - { 0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0}, - { 1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0}, - { 2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0}, - { 0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0}, - { 1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0}, - { 1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0}, - { -1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0}, - { 0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0}, - { 1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0}, - { 3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0}, - { -2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0}, - { 1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0}, - { -1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0}, - { 1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0}, - { -1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0}, - { 0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0}, - { -2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0}, - { 2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0}, - { 3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0}, - { 1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0}, - { 0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0}, - { 1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0}, - { 1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0}, - { 1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0}, - { 0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0}, - { 0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0}, - { 0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0}, - { 1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0}, - { 1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0}, - { 1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0}, - { 1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0}, - { 2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0}, - { 0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0}, - { 0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0}, - { -2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0}, - { 2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0}, - { 0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0}, - { 0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0}, - { 0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0}, - { 0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0}, - { 1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0}, - { 3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0}, - { -2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0}, - { -1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0}, - { 0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0}, - { 0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0}, - { -1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0}, - { 2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0}, - { 2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0}, - { 2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0}, - { 2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0}, - { 1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0}, - { -1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0}, - { -1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0}, - { 0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0} + { 0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9}, + { 0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1}, + { 0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5}, + { 0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5}, + { 0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1}, + { 1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0}, + { 0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6}, + { 0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0}, + { 1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1}, + { 0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3}, + { -1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0}, + { -1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0}, + { 1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0}, + { 0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0}, + { -1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0}, + { -1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0}, + { 1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0}, + { -2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0}, + { 0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0}, + { 2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0}, + { 2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0}, + { 1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0}, + { 0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0}, + { -1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0}, + { 0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0}, + { 0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0}, + { -1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0}, + { 0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0}, + { 1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0}, + { 0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0}, + { 2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0}, + { -1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0}, + { 1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0}, + { 0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0}, + { 0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0}, + { 1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0}, + { -2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0}, + { 0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0}, + { 2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0}, + { 1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0}, + { 0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0}, + { 0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0}, + { 2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0}, + { 1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0}, + { 1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0}, + { 0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0}, + { 0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0}, + { 1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0}, + { 2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0}, + { 0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0}, + { 1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0}, + { 1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0}, + { -1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0}, + { 0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0}, + { 1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0}, + { 3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0}, + { 1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0}, + { -1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0}, + { 1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0}, + { -1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0}, + { 0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0}, + { -2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0}, + { 2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0}, + { 3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0}, + { 1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0}, + { 0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0}, + { 1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0}, + { 1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0}, + { 1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0}, + { 0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0}, + { 0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0}, + { 0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0}, + { 1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0}, + { 2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0}, + { 0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0}, + { 0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0}, + { -2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0}, + { 2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0}, + { 0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0}, + { 0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0}, + { 0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0}, + { 1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0}, + { 3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0}, + { -2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0}, + { -1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0}, + { 0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0}, + { -1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0}, + { 2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0}, + { 2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0}, + { 2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0}, + { 2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0}, + { 1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0}, + { -1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0}, + { -1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0} }; double ang; int i,j; - + *dpsi=*deps=0.0; - - for (i=0;i<106;i++) { - ang=0.0; - for (j=0;j<5;j++) ang+=nut[i][j]*f[j]; - *dpsi+=(nut[i][6]+nut[i][7]*t)*sin(ang); - *deps+=(nut[i][8]+nut[i][9]*t)*cos(ang); - } - *dpsi*=1E-4*AS2R; /* 0.1 mas -> rad */ - *deps*=1E-4*AS2R; + + for (i=0;i<106;i++) + { + ang=0.0; + for (j=0;j<5;j++) ang+=nut[i][j]*f[j]; + *dpsi+=(nut[i][6]+nut[i][7]*t)*sin(ang); + *deps+=(nut[i][8]+nut[i][9]*t)*cos(ang); + } + *dpsi*=1e-4*AS2R; /* 0.1 mas -> rad */ + *deps*=1e-4*AS2R; } + + /* eci to ecef transformation matrix ------------------------------------------- -* compute eci to ecef transformation matrix -* args : gtime_t tutc I time in utc -* double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) -* double *U O eci to ecef transformation matrix (3 x 3) -* double *gmst IO greenwich mean sidereal time (rad) -* (NULL: no output) -* return : none -* note : see ref [3] chap 5 -* not thread-safe -*-----------------------------------------------------------------------------*/ + * compute eci to ecef transformation matrix + * args : gtime_t tutc I time in utc + * double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * double *U O eci to ecef transformation matrix (3 x 3) + * double *gmst IO greenwich mean sidereal time (rad) + * (NULL: no output) + * return : none + * note : see ref [3] chap 5 + * not thread-safe + *-----------------------------------------------------------------------------*/ void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst) { const double ep2000[]={2000,1,1,12,0,0}; @@ -1910,24 +2116,25 @@ void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst) double eps,ze,th,z,t,t2,t3,dpsi,deps,gast,f[5]; double R1[9],R2[9],R3[9],R[9],W[9],N[9],P[9],NP[9]; int i; - + trace(4,"eci2ecef: tutc=%s\n",time_str(tutc,3)); - - if (fabs(timediff(tutc,tutc_))<0.01) { /* read cache */ - for (i=0;i<9;i++) U[i]=U_[i]; - if (gmst) *gmst=gmst_; - return; - } + + if (fabs(timediff(tutc,tutc_))<0.01) + { /* read cache */ + for (i=0;i<9;i++) U[i]=U_[i]; + if (gmst) *gmst=gmst_; + return; + } tutc_=tutc; - + /* terrestrial time */ tgps=utc2gpst(tutc_); t=(timediff(tgps,epoch2time(ep2000))+19.0+32.184)/86400.0/36525.0; t2=t*t; t3=t2*t; - + /* astronomical arguments */ ast_args(t,f); - + /* iau 1976 precession */ ze=(2306.2181*t+0.30188*t2+0.017998*t3)*AS2R; th=(2004.3109*t-0.42665*t2-0.041833*t3)*AS2R; @@ -1936,61 +2143,70 @@ void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst) Rz(-z,R1); Ry(th,R2); Rz(-ze,R3); matmul("NN",3,3,3,1.0,R1,R2,0.0,R); matmul("NN",3,3,3,1.0,R, R3,0.0,P); /* P=Rz(-z)*Ry(th)*Rz(-ze) */ - + /* iau 1980 nutation */ nut_iau1980(t,f,&dpsi,&deps); Rx(-eps-deps,R1); Rz(-dpsi,R2); Rx(eps,R3); matmul("NN",3,3,3,1.0,R1,R2,0.0,R); matmul("NN",3,3,3,1.0,R ,R3,0.0,N); /* N=Rx(-eps)*Rz(-dspi)*Rx(eps) */ - + /* greenwich aparent sidereal time (rad) */ gmst_=utc2gmst(tutc_,erpv[2]); gast=gmst_+dpsi*cos(eps); gast+=(0.00264*sin(f[4])+0.000063*sin(2.0*f[4]))*AS2R; - + /* eci to ecef transformation matrix */ Ry(-erpv[0],R1); Rx(-erpv[1],R2); Rz(gast,R3); matmul("NN",3,3,3,1.0,R1,R2,0.0,W ); matmul("NN",3,3,3,1.0,W ,R3,0.0,R ); /* W=Ry(-xp)*Rx(-yp) */ matmul("NN",3,3,3,1.0,N ,P ,0.0,NP); matmul("NN",3,3,3,1.0,R ,NP,0.0,U_); /* U=W*Rz(gast)*N*P */ - + for (i=0;i<9;i++) U[i]=U_[i]; if (gmst) *gmst=gmst_; - + trace(5,"gmst=%.12f gast=%.12f\n",gmst_,gast); trace(5,"P=\n"); tracemat(5,P,3,3,15,12); trace(5,"N=\n"); tracemat(5,N,3,3,15,12); trace(5,"W=\n"); tracemat(5,W,3,3,15,12); trace(5,"U=\n"); tracemat(5,U,3,3,15,12); } + + /* decode antenna parameter field --------------------------------------------*/ int decodef(char *p, int n, double *v) { int i; - + for (i=0;inmax<=pcvs->n) { - pcvs->nmax+=256; - if (!(pcvs_pcv=(pcv_t *)realloc(pcvs->pcv,sizeof(pcv_t)*pcvs->nmax))) { - trace(1,"addpcv: memory allocation error\n"); - free(pcvs->pcv); pcvs->pcv=NULL; pcvs->n=pcvs->nmax=0; - return; + + if (pcvs->nmax<=pcvs->n) + { + pcvs->nmax+=256; + if (!(pcvs_pcv=(pcv_t *)realloc(pcvs->pcv,sizeof(pcv_t)*pcvs->nmax))) + { + trace(1,"addpcv: memory allocation error\n"); + free(pcvs->pcv); pcvs->pcv=NULL; pcvs->n=pcvs->nmax=0; + return; + } + pcvs->pcv=pcvs_pcv; } - pcvs->pcv=pcvs_pcv; - } pcvs->pcv[pcvs->n++]=*pcv; } + + /* read ngs antenna parameter file -------------------------------------------*/ int readngspcv(const char *file, pcvs_t *pcvs) { @@ -2000,44 +2216,51 @@ int readngspcv(const char *file, pcvs_t *pcvs) double neu[3]; int n=0; char buff[256]; - - if (!(fp=fopen(file,"r"))) { - trace(2,"ngs pcv file open error: %s\n",file); - return 0; - } - while (fgets(buff,sizeof(buff),fp)) { - - if (strlen(buff)>=62&&buff[61]=='|') continue; - - if (buff[0]!=' ') n=0; /* start line */ - if (++n==1) { - pcv=pcv0; - strncpy(pcv.type,buff,61); pcv.type[61]='\0'; + + if (!(fp=fopen(file,"r"))) + { + trace(2,"ngs pcv file open error: %s\n",file); + return 0; } - else if (n==2) { - if (decodef(buff,3,neu)<3) continue; - pcv.off[0][0]=neu[1]; - pcv.off[0][1]=neu[0]; - pcv.off[0][2]=neu[2]; + while (fgets(buff,sizeof(buff),fp)) + { + if (strlen(buff)>=62&&buff[61]=='|') continue; + + if (buff[0]!=' ') n=0; /* start line */ + if (++n==1) + { + pcv=pcv0; + strncpy(pcv.type,buff,61); pcv.type[61]='\0'; + } + else if (n==2) + { + if (decodef(buff,3,neu)<3) continue; + pcv.off[0][0]=neu[1]; + pcv.off[0][1]=neu[0]; + pcv.off[0][2]=neu[2]; + } + else if (n==3) decodef(buff,10,pcv.var[0]); + else if (n==4) decodef(buff,9,pcv.var[0]+10); + else if (n==5) + { + if (decodef(buff,3,neu)<3) continue;; + pcv.off[1][0]=neu[1]; + pcv.off[1][1]=neu[0]; + pcv.off[1][2]=neu[2]; + } + else if (n==6) decodef(buff,10,pcv.var[1]); + else if (n==7) + { + decodef(buff,9,pcv.var[1]+10); + addpcv(&pcv,pcvs); + } } - else if (n==3) decodef(buff,10,pcv.var[0]); - else if (n==4) decodef(buff,9,pcv.var[0]+10); - else if (n==5) { - if (decodef(buff,3,neu)<3) continue;; - pcv.off[1][0]=neu[1]; - pcv.off[1][1]=neu[0]; - pcv.off[1][2]=neu[2]; - } - else if (n==6) decodef(buff,10,pcv.var[1]); - else if (n==7) { - decodef(buff,9,pcv.var[1]+10); - addpcv(&pcv,pcvs); - } - } fclose(fp); - + return 1; } + + /* read antex file ----------------------------------------------------------*/ int readantex(const char *file, pcvs_t *pcvs) { @@ -2047,159 +2270,183 @@ int readantex(const char *file, pcvs_t *pcvs) double neu[3]; int i,f,freq=0,state=0,freqs[]={1,2,5,6,7,8,0}; char buff[256]; - + trace(3,"readantex: file=%s\n",file); - - if (!(fp=fopen(file,"r"))) { - trace(2,"antex pcv file open error: %s\n",file); - return 0; - } - while (fgets(buff,sizeof(buff),fp)) { - - if (strlen(buff)<60||strstr(buff+60,"COMMENT")) continue; - - if (strstr(buff+60,"START OF ANTENNA")) { - pcv=pcv0; - state=1; + + if (!(fp=fopen(file,"r"))) + { + trace(2,"antex pcv file open error: %s\n",file); + return 0; } - if (strstr(buff+60,"END OF ANTENNA")) { - addpcv(&pcv,pcvs); - state=0; + while (fgets(buff,sizeof(buff),fp)) + { + if (strlen(buff)<60||strstr(buff+60,"COMMENT")) continue; + + if (strstr(buff+60,"START OF ANTENNA")) + { + pcv=pcv0; + state=1; + } + if (strstr(buff+60,"END OF ANTENNA")) + { + addpcv(&pcv,pcvs); + state=0; + } + if (!state) continue; + + if (strstr(buff+60,"TYPE / SERIAL NO")) + { + strncpy(pcv.type,buff ,20); pcv.type[20]='\0'; + strncpy(pcv.code,buff+20,20); pcv.code[20]='\0'; + if (!strncmp(pcv.code+3," ",8)) + { + pcv.sat=satid2no(pcv.code); + } + } + else if (strstr(buff+60,"VALID FROM")) + { + if (!str2time(buff,0,43,&pcv.ts)) continue; + } + else if (strstr(buff+60,"VALID UNTIL")) + { + if (!str2time(buff,0,43,&pcv.te)) continue; + } + else if (strstr(buff+60,"START OF FREQUENCY")) + { + if (sscanf(buff+4,"%d",&f)<1) continue; + for (i=0;in;i++) { - pcv=pcvs->pcv+i; - trace(4,"sat=%2d type=%20s code=%s off=%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", - pcv->sat,pcv->type,pcv->code,pcv->off[0][0],pcv->off[0][1], - pcv->off[0][2],pcv->off[1][0],pcv->off[1][1],pcv->off[1][2]); - } + { + ext=""; + } + + if (!strcmp(ext,".atx")||!strcmp(ext,".ATX")) + { + stat=readantex(file,pcvs); + } + else + { + stat=readngspcv(file,pcvs); + } + for (i=0;in;i++) + { + pcv=pcvs->pcv+i; + trace(4,"sat=%2d type=%20s code=%s off=%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", + pcv->sat,pcv->type,pcv->code,pcv->off[0][0],pcv->off[0][1], + pcv->off[0][2],pcv->off[1][0],pcv->off[1][1],pcv->off[1][2]); + } return stat; } + + /* search antenna parameter ---------------------------------------------------- -* read satellite antenna phase center position -* args : int sat I satellite number (0: receiver antenna) -* char *type I antenna type for receiver antenna -* gtime_t time I time to search parameters -* pcvs_t *pcvs IO antenna parameters -* return : antenna parameter (NULL: no antenna) -*-----------------------------------------------------------------------------*/ + * read satellite antenna phase center position + * args : int sat I satellite number (0: receiver antenna) + * char *type I antenna type for receiver antenna + * gtime_t time I time to search parameters + * pcvs_t *pcvs IO antenna parameters + * return : antenna parameter (NULL: no antenna) + *-----------------------------------------------------------------------------*/ pcv_t *searchpcv(int sat, const char *type, gtime_t time, - const pcvs_t *pcvs) + const pcvs_t *pcvs) { pcv_t *pcv; char buff[MAXANT],*types[2],*p; int i,j,n=0; - + trace(3,"searchpcv: sat=%2d type=%s\n",sat,type); - + if (sat) { /* search satellite antenna */ - for (i=0;in;i++) { - pcv=pcvs->pcv+i; - if (pcv->sat!=sat) continue; - if (pcv->ts.time!=0&&timediff(pcv->ts,time)>0.0) continue; - if (pcv->te.time!=0&&timediff(pcv->te,time)<0.0) continue; - return pcv; - } + for (i=0;in;i++) + { + pcv=pcvs->pcv+i; + if (pcv->sat!=sat) continue; + if (pcv->ts.time!=0&&timediff(pcv->ts,time)>0.0) continue; + if (pcv->te.time!=0&&timediff(pcv->te,time)<0.0) continue; + return pcv; + } } - else { - strcpy(buff,type); - for (p=strtok(buff," ");p&&n<2;p=strtok(NULL," ")) types[n++]=p; - if (n<=0) return NULL; - - /* search receiver antenna with radome at first */ - for (i=0;in;i++) { - pcv=pcvs->pcv+i; - for (j=0;jtype,types[j])) break; - if (j>=n) return pcv; + else + { + strcpy(buff,type); + for (p=strtok(buff," ");p&&n<2;p=strtok(NULL," ")) types[n++]=p; + if (n<=0) return NULL; + + /* search receiver antenna with radome at first */ + for (i=0;in;i++) + { + pcv=pcvs->pcv+i; + for (j=0;jtype,types[j])) break; + if (j>=n) return pcv; + } + /* search receiver antenna without radome */ + for (i=0;in;i++) + { + pcv=pcvs->pcv+i; + if (strstr(pcv->type,types[0])!=pcv->type) continue; + + trace(2,"pcv without radome is used type=%s\n",type); + return pcv; + } } - /* search receiver antenna without radome */ - for (i=0;in;i++) { - pcv=pcvs->pcv+i; - if (strstr(pcv->type,types[0])!=pcv->type) continue; - - trace(2,"pcv without radome is used type=%s\n",type); - return pcv; - } - } return NULL; } + + /* read station positions ------------------------------------------------------ -* read positions from station position file -* args : char *file I station position file containing -* lat(deg) lon(deg) height(m) name in a line -* char *rcvs I station name -* double *pos O station position {lat,lon,h} (rad/m) -* (all 0 if search error) -* return : none -*-----------------------------------------------------------------------------*/ + * read positions from station position file + * args : char *file I station position file containing + * lat(deg) lon(deg) height(m) name in a line + * char *rcvs I station name + * double *pos O station position {lat,lon,h} (rad/m) + * (all 0 if search error) + * return : none + *-----------------------------------------------------------------------------*/ void readpos(const char *file, const char *rcv, double *pos) { static double poss[2048][3]; @@ -2207,314 +2454,364 @@ void readpos(const char *file, const char *rcv, double *pos) FILE *fp; int i,j,len,np=0; char buff[256],str[256]; - + trace(3,"readpos: file=%s\n",file); - - if (!(fp=fopen(file,"r"))) { - fprintf(stderr,"reference position file open error : %s\n",file); - return; - } - while (np<2048&&fgets(buff,sizeof(buff),fp)) { - if (buff[0]=='%'||buff[0]=='#') continue; - if (sscanf(buff,"%lf %lf %lf %s",&poss[np][0],&poss[np][1],&poss[np][2], - str)<4) continue; - strncpy(stas[np],str,15); stas[np++][15]='\0'; - } + + if (!(fp=fopen(file,"r"))) + { + fprintf(stderr,"reference position file open error : %s\n",file); + return; + } + while (np<2048&&fgets(buff,sizeof(buff),fp)) + { + if (buff[0]=='%'||buff[0]=='#') continue; + if (sscanf(buff,"%lf %lf %lf %s",&poss[np][0],&poss[np][1],&poss[np][2], + str)<4) continue; + strncpy(stas[np],str,15); stas[np++][15]='\0'; + } fclose(fp); len=(int)strlen(rcv); - for (i=0;in>=erp->nmax) { - erp->nmax=erp->nmax<=0?128:erp->nmax*2; - erp_data=(erpd_t *)realloc(erp->data,sizeof(erpd_t)*erp->nmax); - if (!erp_data) { - free(erp->data); erp->data=NULL; erp->n=erp->nmax=0; - fclose(fp); - return 0; + while (fgets(buff,sizeof(buff),fp)) + { + if (sscanf(buff,"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + v,v+1,v+2,v+3,v+4,v+5,v+6,v+7,v+8,v+9,v+10,v+11,v+12,v+13)<5) { + continue; } - erp->data=erp_data; + if (erp->n>=erp->nmax) + { + erp->nmax=erp->nmax<=0?128:erp->nmax*2; + erp_data=(erpd_t *)realloc(erp->data,sizeof(erpd_t)*erp->nmax); + if (!erp_data) + { + free(erp->data); erp->data=NULL; erp->n=erp->nmax=0; + fclose(fp); + return 0; + } + erp->data=erp_data; + } + erp->data[erp->n].mjd=v[0]; + erp->data[erp->n].xp=v[1]*1e-6*AS2R; + erp->data[erp->n].yp=v[2]*1e-6*AS2R; + erp->data[erp->n].ut1_utc=v[3]*1e-7; + erp->data[erp->n].lod=v[4]*1e-7; + erp->data[erp->n].xpr=v[12]*1e-6*AS2R; + erp->data[erp->n++].ypr=v[13]*1e-6*AS2R; } - erp->data[erp->n].mjd=v[0]; - erp->data[erp->n].xp=v[1]*1E-6*AS2R; - erp->data[erp->n].yp=v[2]*1E-6*AS2R; - erp->data[erp->n].ut1_utc=v[3]*1E-7; - erp->data[erp->n].lod=v[4]*1E-7; - erp->data[erp->n].xpr=v[12]*1E-6*AS2R; - erp->data[erp->n++].ypr=v[13]*1E-6*AS2R; - } fclose(fp); return 1; } + + + /* get earth rotation parameter values ----------------------------------------- -* get earth rotation parameter values -* args : erp_t *erp I earth rotation parameters -* gtime_t time I time (gpst) -* double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) -* return : status (1:ok,0:error) -*-----------------------------------------------------------------------------*/ + * get earth rotation parameter values + * args : erp_t *erp I earth rotation parameters + * gtime_t time I time (gpst) + * double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ int geterp(const erp_t *erp, gtime_t time, double *erpv) { const double ep[]={2000,1,1,12,0,0}; double mjd,day,a; int i,j,k; - + trace(4,"geterp:\n"); - + if (erp->n<=0) return 0; - + mjd=51544.5+(timediff(gpst2utc(time),epoch2time(ep)))/86400.0; - - if (mjd<=erp->data[0].mjd) { - day=mjd-erp->data[0].mjd; - erpv[0]=erp->data[0].xp +erp->data[0].xpr*day; - erpv[1]=erp->data[0].yp +erp->data[0].ypr*day; - erpv[2]=erp->data[0].ut1_utc-erp->data[0].lod*day; - erpv[3]=erp->data[0].lod; - return 1; - } - if (mjd>=erp->data[erp->n-1].mjd) { - day=mjd-erp->data[erp->n-1].mjd; - erpv[0]=erp->data[erp->n-1].xp +erp->data[erp->n-1].xpr*day; - erpv[1]=erp->data[erp->n-1].yp +erp->data[erp->n-1].ypr*day; - erpv[2]=erp->data[erp->n-1].ut1_utc-erp->data[erp->n-1].lod*day; - erpv[3]=erp->data[erp->n-1].lod; - return 1; - } - for (j=0,k=erp->n-1;jdata[i].mjd) k=i; else j=i; - } - if (erp->data[j].mjd==erp->data[j+1].mjd) { - a=0.5; - } - else { - a=(mjd-erp->data[j].mjd)/(erp->data[j+1].mjd-erp->data[j].mjd); - } + + if (mjd<=erp->data[0].mjd) + { + day=mjd-erp->data[0].mjd; + erpv[0]=erp->data[0].xp +erp->data[0].xpr*day; + erpv[1]=erp->data[0].yp +erp->data[0].ypr*day; + erpv[2]=erp->data[0].ut1_utc-erp->data[0].lod*day; + erpv[3]=erp->data[0].lod; + return 1; + } + if (mjd>=erp->data[erp->n-1].mjd) + { + day=mjd-erp->data[erp->n-1].mjd; + erpv[0]=erp->data[erp->n-1].xp +erp->data[erp->n-1].xpr*day; + erpv[1]=erp->data[erp->n-1].yp +erp->data[erp->n-1].ypr*day; + erpv[2]=erp->data[erp->n-1].ut1_utc-erp->data[erp->n-1].lod*day; + erpv[3]=erp->data[erp->n-1].lod; + return 1; + } + for (j=0,k=erp->n-1;jdata[i].mjd) k=i; else j=i; + } + if (erp->data[j].mjd==erp->data[j+1].mjd) + { + a=0.5; + } + else + { + a=(mjd-erp->data[j].mjd)/(erp->data[j+1].mjd-erp->data[j].mjd); + } erpv[0]=(1.0-a)*erp->data[j].xp +a*erp->data[j+1].xp; erpv[1]=(1.0-a)*erp->data[j].yp +a*erp->data[j+1].yp; erpv[2]=(1.0-a)*erp->data[j].ut1_utc+a*erp->data[j+1].ut1_utc; erpv[3]=(1.0-a)*erp->data[j].lod +a*erp->data[j+1].lod; return 1; } + /* compare ephemeris ---------------------------------------------------------*/ int cmpeph(const void *p1, const void *p2) { eph_t *q1=(eph_t *)p1,*q2=(eph_t *)p2; return q1->ttr.time!=q2->ttr.time?(int)(q1->ttr.time-q2->ttr.time): - (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): - q1->sat-q2->sat); + (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): + q1->sat-q2->sat); } + + /* sort and unique ephemeris -------------------------------------------------*/ void uniqeph(nav_t *nav) { eph_t *nav_eph; int i,j; - + trace(3,"uniqeph: n=%d\n",nav->n); - + if (nav->n<=0) return; - + qsort(nav->eph,nav->n,sizeof(eph_t),cmpeph); - - for (i=1,j=0;in;i++) { - if (nav->eph[i].sat!=nav->eph[j].sat|| - nav->eph[i].iode!=nav->eph[j].iode) { - nav->eph[++j]=nav->eph[i]; + + for (i=1,j=0;in;i++) + { + if (nav->eph[i].sat!=nav->eph[j].sat|| + nav->eph[i].iode!=nav->eph[j].iode) + { + nav->eph[++j]=nav->eph[i]; + } } - } nav->n=j+1; - - if (!(nav_eph=(eph_t *)realloc(nav->eph,sizeof(eph_t)*nav->n))) { - trace(1,"uniqeph malloc error n=%d\n",nav->n); - free(nav->eph); nav->eph=NULL; nav->n=nav->nmax=0; - return; - } + + if (!(nav_eph=(eph_t *)realloc(nav->eph,sizeof(eph_t)*nav->n))) + { + trace(1,"uniqeph malloc error n=%d\n",nav->n); + free(nav->eph); nav->eph=NULL; nav->n=nav->nmax=0; + return; + } nav->eph=nav_eph; nav->nmax=nav->n; - + trace(4,"uniqeph: n=%d\n",nav->n); } + + /* compare glonass ephemeris -------------------------------------------------*/ int cmpgeph(const void *p1, const void *p2) { geph_t *q1=(geph_t *)p1,*q2=(geph_t *)p2; return q1->tof.time!=q2->tof.time?(int)(q1->tof.time-q2->tof.time): - (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): - q1->sat-q2->sat); + (q1->toe.time!=q2->toe.time?(int)(q1->toe.time-q2->toe.time): + q1->sat-q2->sat); } + + /* sort and unique glonass ephemeris -----------------------------------------*/ void uniqgeph(nav_t *nav) { geph_t *nav_geph; int i,j; - + trace(3,"uniqgeph: ng=%d\n",nav->ng); - + if (nav->ng<=0) return; - + qsort(nav->geph,nav->ng,sizeof(geph_t),cmpgeph); - - for (i=j=0;ing;i++) { - if (nav->geph[i].sat!=nav->geph[j].sat|| - nav->geph[i].toe.time!=nav->geph[j].toe.time|| - nav->geph[i].svh!=nav->geph[j].svh) { - nav->geph[++j]=nav->geph[i]; + + for (i=j=0;ing;i++) + { + if (nav->geph[i].sat!=nav->geph[j].sat|| + nav->geph[i].toe.time!=nav->geph[j].toe.time|| + nav->geph[i].svh!=nav->geph[j].svh) + { + nav->geph[++j]=nav->geph[i]; + } } - } nav->ng=j+1; - - if (!(nav_geph=(geph_t *)realloc(nav->geph,sizeof(geph_t)*nav->ng))) { - trace(1,"uniqgeph malloc error ng=%d\n",nav->ng); - free(nav->geph); nav->geph=NULL; nav->ng=nav->ngmax=0; - return; - } + + if (!(nav_geph=(geph_t *)realloc(nav->geph,sizeof(geph_t)*nav->ng))) + { + trace(1,"uniqgeph malloc error ng=%d\n",nav->ng); + free(nav->geph); nav->geph=NULL; nav->ng=nav->ngmax=0; + return; + } nav->geph=nav_geph; nav->ngmax=nav->ng; - + trace(4,"uniqgeph: ng=%d\n",nav->ng); } + + /* compare sbas ephemeris ----------------------------------------------------*/ int cmpseph(const void *p1, const void *p2) { seph_t *q1=(seph_t *)p1,*q2=(seph_t *)p2; return q1->tof.time!=q2->tof.time?(int)(q1->tof.time-q2->tof.time): - (q1->t0.time!=q2->t0.time?(int)(q1->t0.time-q2->t0.time): - q1->sat-q2->sat); + (q1->t0.time!=q2->t0.time?(int)(q1->t0.time-q2->t0.time): + q1->sat-q2->sat); } + + /* sort and unique sbas ephemeris --------------------------------------------*/ void uniqseph(nav_t *nav) { seph_t *nav_seph; int i,j; - + trace(3,"uniqseph: ns=%d\n",nav->ns); - + if (nav->ns<=0) return; - + qsort(nav->seph,nav->ns,sizeof(seph_t),cmpseph); - - for (i=j=0;ins;i++) { - if (nav->seph[i].sat!=nav->seph[j].sat|| - nav->seph[i].t0.time!=nav->seph[j].t0.time) { - nav->seph[++j]=nav->seph[i]; + + for (i=j=0;ins;i++) + { + if (nav->seph[i].sat!=nav->seph[j].sat|| + nav->seph[i].t0.time!=nav->seph[j].t0.time) + { + nav->seph[++j]=nav->seph[i]; + } } - } nav->ns=j+1; - - if (!(nav_seph=(seph_t *)realloc(nav->seph,sizeof(seph_t)*nav->ns))) { - trace(1,"uniqseph malloc error ns=%d\n",nav->ns); - free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0; - return; - } + + if (!(nav_seph=(seph_t *)realloc(nav->seph,sizeof(seph_t)*nav->ns))) + { + trace(1,"uniqseph malloc error ns=%d\n",nav->ns); + free(nav->seph); nav->seph=NULL; nav->ns=nav->nsmax=0; + return; + } nav->seph=nav_seph; nav->nsmax=nav->ns; - + trace(4,"uniqseph: ns=%d\n",nav->ns); } + + /* unique ephemerides ---------------------------------------------------------- -* unique ephemerides in navigation data and update carrier wave length -* args : nav_t *nav IO navigation data -* return : number of epochs -*-----------------------------------------------------------------------------*/ + * unique ephemerides in navigation data and update carrier wave length + * args : nav_t *nav IO navigation data + * return : number of epochs + *-----------------------------------------------------------------------------*/ void uniqnav(nav_t *nav) { int i,j; - + trace(3,"uniqnav: neph=%d ngeph=%d nseph=%d\n",nav->n,nav->ng,nav->ns); - + /* unique ephemeris */ uniqeph (nav); uniqgeph(nav); uniqseph(nav); - + /* update carrier wave length */ - for (i=0;ilam[i][j]=satwavelen(i+1,j,nav); - } + for (i=0;ilam[i][j]=satwavelen(i+1,j,nav); + } } + + /* compare observation data -------------------------------------------------*/ int cmpobs(const void *p1, const void *p2) { @@ -2524,58 +2821,68 @@ int cmpobs(const void *p1, const void *p2) if (q1->rcv!=q2->rcv) return (int)q1->rcv-(int)q2->rcv; return (int)q1->sat-(int)q2->sat; } + + /* sort and unique observation data -------------------------------------------- -* sort and unique observation data by time, rcv, sat -* args : obs_t *obs IO observation data -* return : number of epochs -*-----------------------------------------------------------------------------*/ + * sort and unique observation data by time, rcv, sat + * args : obs_t *obs IO observation data + * return : number of epochs + *-----------------------------------------------------------------------------*/ int sortobs(obs_t *obs) { int i,j,n; - + trace(3,"sortobs: nobs=%d\n",obs->n); - + if (obs->n<=0) return 0; - + qsort(obs->data,obs->n,sizeof(obsd_t),cmpobs); - + /* delete duplicated data */ - for (i=j=0;in;i++) { - if (obs->data[i].sat!=obs->data[j].sat|| - obs->data[i].rcv!=obs->data[j].rcv|| - timediff(obs->data[i].time,obs->data[j].time)!=0.0) { - obs->data[++j]=obs->data[i]; + for (i=j=0;in;i++) + { + if (obs->data[i].sat!=obs->data[j].sat|| + obs->data[i].rcv!=obs->data[j].rcv|| + timediff(obs->data[i].time,obs->data[j].time)!=0.0) + { + obs->data[++j]=obs->data[i]; + } } - } obs->n=j+1; - - for (i=n=0;in;i=j,n++) { - for (j=i+1;jn;j++) { - if (timediff(obs->data[j].time,obs->data[i].time)>DTTOL) break; + + for (i=n=0;in;i=j,n++) + { + for (j=i+1;jn;j++) + { + if (timediff(obs->data[j].time,obs->data[i].time)>DTTOL) break; + } } - } return n; } + + /* screen by time -------------------------------------------------------------- -* screening by time start, time end, and time interval -* args : gtime_t time I time -* gtime_t ts I time start (ts.time==0:no screening by ts) -* gtime_t te I time end (te.time==0:no screening by te) -* double tint I time interval (s) (0.0:no screen by tint) -* return : 1:on condition, 0:not on condition -*-----------------------------------------------------------------------------*/ + * screening by time start, time end, and time interval + * args : gtime_t time I time + * gtime_t ts I time start (ts.time==0:no screening by ts) + * gtime_t te I time end (te.time==0:no screening by te) + * double tint I time interval (s) (0.0:no screen by tint) + * return : 1:on condition, 0:not on condition + *-----------------------------------------------------------------------------*/ int screent(gtime_t time, gtime_t ts, gtime_t te, double tint) { return (tint<=0.0||fmod(time2gpst(time,NULL)+DTTOL,tint)<=DTTOL*2.0)&& - (ts.time==0||timediff(time,ts)>=-DTTOL)&& - (te.time==0||timediff(time,te)< DTTOL); + (ts.time==0||timediff(time,ts)>=-DTTOL)&& + (te.time==0||timediff(time,te)< DTTOL); } + + /* read/save navigation data --------------------------------------------------- -* save or load navigation data -* args : char file I file path -* nav_t nav O/I navigation data -* return : status (1:ok,0:no file) -*-----------------------------------------------------------------------------*/ + * save or load navigation data + * args : char file I file path + * nav_t nav O/I navigation data + * return : status (1:ok,0:no file) + *-----------------------------------------------------------------------------*/ int readnav(const char *file, nav_t *nav) { FILE *fp; @@ -2584,134 +2891,146 @@ int readnav(const char *file, nav_t *nav) char buff[4096],*p; long toe_time,tof_time,toc_time,ttr_time; int i,sat,prn; - + trace(3,"loadnav: file=%s\n",file); - + if (!(fp=fopen(file,"r"))) return 0; - - while (fgets(buff,sizeof(buff),fp)) { - if (!strncmp(buff,"IONUTC",6)) { - for (i=0;i<8;i++) nav->ion_gps[i]=0.0; - for (i=0;i<4;i++) nav->utc_gps[i]=0.0; - nav->leaps=0; - sscanf(buff,"IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d", - &nav->ion_gps[0],&nav->ion_gps[1],&nav->ion_gps[2],&nav->ion_gps[3], - &nav->ion_gps[4],&nav->ion_gps[5],&nav->ion_gps[6],&nav->ion_gps[7], - &nav->utc_gps[0],&nav->utc_gps[1],&nav->utc_gps[2],&nav->utc_gps[3], - &nav->leaps); - continue; + + while (fgets(buff,sizeof(buff),fp)) + { + if (!strncmp(buff,"IONUTC",6)) + { + for (i=0;i<8;i++) nav->ion_gps[i]=0.0; + for (i=0;i<4;i++) nav->utc_gps[i]=0.0; + nav->leaps=0; + sscanf(buff,"IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d", + &nav->ion_gps[0],&nav->ion_gps[1],&nav->ion_gps[2],&nav->ion_gps[3], + &nav->ion_gps[4],&nav->ion_gps[5],&nav->ion_gps[6],&nav->ion_gps[7], + &nav->utc_gps[0],&nav->utc_gps[1],&nav->utc_gps[2],&nav->utc_gps[3], + &nav->leaps); + continue; + } + if ((p=strchr(buff,','))) *p='\0'; else continue; + if (!(sat=satid2no(buff))) continue; + if (satsys(sat,&prn)==SYS_GLO) + { + nav->geph[prn-1]=geph0; + nav->geph[prn-1].sat=sat; + toe_time=tof_time=0; + sscanf(p+1,"%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf", + &nav->geph[prn-1].iode,&nav->geph[prn-1].frq,&nav->geph[prn-1].svh, + &nav->geph[prn-1].sva,&nav->geph[prn-1].age, + &toe_time,&tof_time, + &nav->geph[prn-1].pos[0],&nav->geph[prn-1].pos[1],&nav->geph[prn-1].pos[2], + &nav->geph[prn-1].vel[0],&nav->geph[prn-1].vel[1],&nav->geph[prn-1].vel[2], + &nav->geph[prn-1].acc[0],&nav->geph[prn-1].acc[1],&nav->geph[prn-1].acc[2], + &nav->geph[prn-1].taun ,&nav->geph[prn-1].gamn ,&nav->geph[prn-1].dtaun); + nav->geph[prn-1].toe.time=toe_time; + nav->geph[prn-1].tof.time=tof_time; + } + else + { + nav->eph[sat-1]=eph0; + nav->eph[sat-1].sat=sat; + toe_time=toc_time=ttr_time=0; + sscanf(p+1,"%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d", + &nav->eph[sat-1].iode,&nav->eph[sat-1].iodc,&nav->eph[sat-1].sva , + &nav->eph[sat-1].svh , + &toe_time,&toc_time,&ttr_time, + &nav->eph[sat-1].A ,&nav->eph[sat-1].e ,&nav->eph[sat-1].i0 , + &nav->eph[sat-1].OMG0,&nav->eph[sat-1].omg ,&nav->eph[sat-1].M0 , + &nav->eph[sat-1].deln,&nav->eph[sat-1].OMGd,&nav->eph[sat-1].idot, + &nav->eph[sat-1].crc ,&nav->eph[sat-1].crs ,&nav->eph[sat-1].cuc , + &nav->eph[sat-1].cus ,&nav->eph[sat-1].cic ,&nav->eph[sat-1].cis , + &nav->eph[sat-1].toes,&nav->eph[sat-1].fit ,&nav->eph[sat-1].f0 , + &nav->eph[sat-1].f1 ,&nav->eph[sat-1].f2 ,&nav->eph[sat-1].tgd[0], + &nav->eph[sat-1].code, &nav->eph[sat-1].flag); + nav->eph[sat-1].toe.time=toe_time; + nav->eph[sat-1].toc.time=toc_time; + nav->eph[sat-1].ttr.time=ttr_time; + } } - if ((p=strchr(buff,','))) *p='\0'; else continue; - if (!(sat=satid2no(buff))) continue; - if (satsys(sat,&prn)==SYS_GLO) { - nav->geph[prn-1]=geph0; - nav->geph[prn-1].sat=sat; - toe_time=tof_time=0; - sscanf(p+1,"%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," - "%lf,%lf,%lf,%lf", - &nav->geph[prn-1].iode,&nav->geph[prn-1].frq,&nav->geph[prn-1].svh, - &nav->geph[prn-1].sva,&nav->geph[prn-1].age, - &toe_time,&tof_time, - &nav->geph[prn-1].pos[0],&nav->geph[prn-1].pos[1],&nav->geph[prn-1].pos[2], - &nav->geph[prn-1].vel[0],&nav->geph[prn-1].vel[1],&nav->geph[prn-1].vel[2], - &nav->geph[prn-1].acc[0],&nav->geph[prn-1].acc[1],&nav->geph[prn-1].acc[2], - &nav->geph[prn-1].taun ,&nav->geph[prn-1].gamn ,&nav->geph[prn-1].dtaun); - nav->geph[prn-1].toe.time=toe_time; - nav->geph[prn-1].tof.time=tof_time; - } - else { - nav->eph[sat-1]=eph0; - nav->eph[sat-1].sat=sat; - toe_time=toc_time=ttr_time=0; - sscanf(p+1,"%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," - "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d", - &nav->eph[sat-1].iode,&nav->eph[sat-1].iodc,&nav->eph[sat-1].sva , - &nav->eph[sat-1].svh , - &toe_time,&toc_time,&ttr_time, - &nav->eph[sat-1].A ,&nav->eph[sat-1].e ,&nav->eph[sat-1].i0 , - &nav->eph[sat-1].OMG0,&nav->eph[sat-1].omg ,&nav->eph[sat-1].M0 , - &nav->eph[sat-1].deln,&nav->eph[sat-1].OMGd,&nav->eph[sat-1].idot, - &nav->eph[sat-1].crc ,&nav->eph[sat-1].crs ,&nav->eph[sat-1].cuc , - &nav->eph[sat-1].cus ,&nav->eph[sat-1].cic ,&nav->eph[sat-1].cis , - &nav->eph[sat-1].toes,&nav->eph[sat-1].fit ,&nav->eph[sat-1].f0 , - &nav->eph[sat-1].f1 ,&nav->eph[sat-1].f2 ,&nav->eph[sat-1].tgd[0], - &nav->eph[sat-1].code, &nav->eph[sat-1].flag); - nav->eph[sat-1].toe.time=toe_time; - nav->eph[sat-1].toc.time=toc_time; - nav->eph[sat-1].ttr.time=ttr_time; - } - } fclose(fp); return 1; } + + int savenav(const char *file, const nav_t *nav) { FILE *fp; int i; char id[32]; - + trace(3,"savenav: file=%s\n",file); - + if (!(fp=fopen(file,"w"))) return 0; - - for (i=0;ieph[i].ttr.time==0) continue; - satno2id(nav->eph[i].sat,id); - fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", - id,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].sva , - nav->eph[i].svh ,(int)nav->eph[i].toe.time, - (int)nav->eph[i].toc.time,(int)nav->eph[i].ttr.time, - nav->eph[i].A ,nav->eph[i].e ,nav->eph[i].i0 ,nav->eph[i].OMG0, - nav->eph[i].omg ,nav->eph[i].M0 ,nav->eph[i].deln,nav->eph[i].OMGd, - nav->eph[i].idot,nav->eph[i].crc,nav->eph[i].crs ,nav->eph[i].cuc , - nav->eph[i].cus ,nav->eph[i].cic,nav->eph[i].cis ,nav->eph[i].toes, - nav->eph[i].fit ,nav->eph[i].f0 ,nav->eph[i].f1 ,nav->eph[i].f2 , - nav->eph[i].tgd[0],nav->eph[i].code,nav->eph[i].flag); - } - for (i=0;igeph[i].tof.time==0) continue; - satno2id(nav->geph[i].sat,id); - fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", - id,nav->geph[i].iode,nav->geph[i].frq,nav->geph[i].svh, - nav->geph[i].sva,nav->geph[i].age,(int)nav->geph[i].toe.time, - (int)nav->geph[i].tof.time, - nav->geph[i].pos[0],nav->geph[i].pos[1],nav->geph[i].pos[2], - nav->geph[i].vel[0],nav->geph[i].vel[1],nav->geph[i].vel[2], - nav->geph[i].acc[0],nav->geph[i].acc[1],nav->geph[i].acc[2], - nav->geph[i].taun,nav->geph[i].gamn,nav->geph[i].dtaun); - } + + for (i=0;ieph[i].ttr.time==0) continue; + satno2id(nav->eph[i].sat,id); + fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", + id,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].sva , + nav->eph[i].svh ,(int)nav->eph[i].toe.time, + (int)nav->eph[i].toc.time,(int)nav->eph[i].ttr.time, + nav->eph[i].A ,nav->eph[i].e ,nav->eph[i].i0 ,nav->eph[i].OMG0, + nav->eph[i].omg ,nav->eph[i].M0 ,nav->eph[i].deln,nav->eph[i].OMGd, + nav->eph[i].idot,nav->eph[i].crc,nav->eph[i].crs ,nav->eph[i].cuc , + nav->eph[i].cus ,nav->eph[i].cic,nav->eph[i].cis ,nav->eph[i].toes, + nav->eph[i].fit ,nav->eph[i].f0 ,nav->eph[i].f1 ,nav->eph[i].f2 , + nav->eph[i].tgd[0],nav->eph[i].code,nav->eph[i].flag); + } + for (i=0;igeph[i].tof.time==0) continue; + satno2id(nav->geph[i].sat,id); + fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", + id,nav->geph[i].iode,nav->geph[i].frq,nav->geph[i].svh, + nav->geph[i].sva,nav->geph[i].age,(int)nav->geph[i].toe.time, + (int)nav->geph[i].tof.time, + nav->geph[i].pos[0],nav->geph[i].pos[1],nav->geph[i].pos[2], + nav->geph[i].vel[0],nav->geph[i].vel[1],nav->geph[i].vel[2], + nav->geph[i].acc[0],nav->geph[i].acc[1],nav->geph[i].acc[2], + nav->geph[i].taun,nav->geph[i].gamn,nav->geph[i].dtaun); + } fprintf(fp,"IONUTC,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%d", + "%.14E,%.14E,%.14E,%d", nav->ion_gps[0],nav->ion_gps[1],nav->ion_gps[2],nav->ion_gps[3], nav->ion_gps[4],nav->ion_gps[5],nav->ion_gps[6],nav->ion_gps[7], nav->utc_gps[0],nav->utc_gps[1],nav->utc_gps[2],nav->utc_gps[3], nav->leaps); - + fclose(fp); return 1; } + + /* free observation data ------------------------------------------------------- -* free memory for observation data -* args : obs_t *obs IO observation data -* return : none -*-----------------------------------------------------------------------------*/ + * free memory for observation data + * args : obs_t *obs IO observation data + * return : none + *-----------------------------------------------------------------------------*/ void freeobs(obs_t *obs) { free(obs->data); obs->data=NULL; obs->n=obs->nmax=0; } + + /* free navigation data --------------------------------------------------------- -* free memory for navigation data -* args : nav_t *nav IO navigation data -* int opt I option (or of followings) -* (0x01: gps/qzs ephmeris, 0x02: glonass ephemeris, -* 0x04: sbas ephemeris, 0x08: precise ephemeris, -* 0x10: precise clock 0x20: almanac, -* 0x40: tec data) -* return : none -*-----------------------------------------------------------------------------*/ + * free memory for navigation data + * args : nav_t *nav IO navigation data + * int opt I option (or of followings) + * (0x01: gps/qzs ephmeris, 0x02: glonass ephemeris, + * 0x04: sbas ephemeris, 0x08: precise ephemeris, + * 0x10: precise clock 0x20: almanac, + * 0x40: tec data) + * return : none + *-----------------------------------------------------------------------------*/ void freenav(nav_t *nav, int opt) { if (opt&0x01) {free(nav->eph ); nav->eph =NULL; nav->n =nav->nmax =0;} @@ -2723,6 +3042,8 @@ void freenav(nav_t *nav, int opt) if (opt&0x40) {free(nav->tec ); nav->tec =NULL; nav->nt=nav->ntmax=0;} if (opt&0x80) {free(nav->fcb ); nav->fcb =NULL; nav->nf=nav->nfmax=0;} } + + /* debug trace functions -----------------------------------------------------*/ //#ifdef TRACE // @@ -2731,7 +3052,7 @@ void freenav(nav_t *nav, int opt) //static int level_trace=0; /* level of trace */ //static unsigned int tick_trace=0; /* tick time at traceopen (ms) */ //static gtime_t time_trace={0}; /* time at traceopen */ -//static lock_t lock_trace; /* lock for trace */ +//static pthread_mutex_t lock_trace; /* lock for trace */ // //static void traceswap(void) //{ @@ -2856,7 +3177,7 @@ void freenav(nav_t *nav, int opt) // time2str(nav->geph[i].tof,s2,0); // satno2id(nav->geph[i].sat,id); // fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d %8.3f\n",i+1, -// id,s1,s2,nav->geph[i].frq,nav->geph[i].svh,nav->geph[i].taun*1E6); +// id,s1,s2,nav->geph[i].frq,nav->geph[i].svh,nav->geph[i].taun*1e6); // } //} //extern void tracehnav(int level, const nav_t *nav) @@ -2887,9 +3208,9 @@ void freenav(nav_t *nav, int opt) // fprintf(fp_trace,"%-3s %d %-3s %13.3f %13.3f %13.3f %13.3f %6.3f %6.3f %6.3f %6.3f\n", // s,nav->peph[i].index,id, // nav->peph[i].pos[j][0],nav->peph[i].pos[j][1], -// nav->peph[i].pos[j][2],nav->peph[i].pos[j][3]*1E9, +// nav->peph[i].pos[j][2],nav->peph[i].pos[j][3]*1e9, // nav->peph[i].std[j][0],nav->peph[i].std[j][1], -// nav->peph[i].std[j][2],nav->peph[i].std[j][3]*1E9); +// nav->peph[i].std[j][2],nav->peph[i].std[j][3]*1e9); // } // } //} @@ -2906,7 +3227,7 @@ void freenav(nav_t *nav, int opt) // satno2id(j+1,id); // fprintf(fp_trace,"%-3s %d %-3s %13.3f %6.3f\n", // s,nav->pclk[i].index,id, -// nav->pclk[i].clk[j][0]*1E9,nav->pclk[i].std[j][0]*1E9); +// nav->pclk[i].clk[j][0]*1e9,nav->pclk[i].std[j][0]*1e9); // } // } //} @@ -2918,10 +3239,12 @@ void freenav(nav_t *nav, int opt) // fprintf(fp_trace,"\n"); //} //#else + void traceopen(const char *file) {} void traceclose(void) {} void tracelevel(int level) {} -void trace (int level, const char *format, ...) { +void trace (int level, const char *format, ...) +{ /* va_list ap;*/ /* print error message to stderr */ /*printf("RTKLIB TRACE[%i]:",level); @@ -2940,501 +3263,547 @@ void traceb (int level, const unsigned char *p, int n) {} //#endif /* TRACE */ + /* execute command ------------------------------------------------------------- -* execute command line by operating system shell -* args : char *cmd I command line -* return : execution status (0:ok,0>:error) -*-----------------------------------------------------------------------------*/ + * execute command line by operating system shell + * args : char *cmd I command line + * return : execution status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ int execcmd(const char *cmd) { - trace(3,"execcmd: cmd=%s\n",cmd); - return system(cmd); } + /* create directory ------------------------------------------------------------ -* create directory if not exist -* args : char *path I file path to be saved -* return : none -* notes : not recursive. only one level -*-----------------------------------------------------------------------------*/ + * create directory if not exist + * args : char *path I file path to be saved + * return : none + * notes : not recursive. only one level + *-----------------------------------------------------------------------------*/ void createdir(const char *path) { char buff[1024],*p; - + tracet(3,"createdir: path=%s\n",path); - + strcpy(buff,path); if (!(p=strrchr(buff,FILEPATHSEP))) return; *p='\0'; - + mkdir(buff,0777); } + + /* replace string ------------------------------------------------------------*/ int repstr(char *str, const char *pat, const char *rep) { int len=(int)strlen(pat); char buff[1024],*p,*q,*r; - - for (p=str,r=buff;*p;p=q+len) { - if (!(q=strstr(p,pat))) break; - strncpy(r,p,q-p); - r+=q-p; - r+=sprintf(r,"%s",rep); - } + + for (p=str,r=buff;*p;p=q+len) + { + if (!(q=strstr(p,pat))) break; + strncpy(r,p,q-p); + r+=q-p; + r+=sprintf(r,"%s",rep); + } if (p<=str) return 0; strcpy(r,p); strcpy(str,buff); return 1; } + + /* replace keywords in file path ----------------------------------------------- -* replace keywords in file path with date, time, rover and base station id -* args : char *path I file path (see below) -* char *rpath O file path in which keywords replaced (see below) -* gtime_t time I time (gpst) (time.time==0: not replaced) -* char *rov I rover id string ("": not replaced) -* char *base I base station id string ("": not replaced) -* return : status (1:keywords replaced, 0:no valid keyword in the path, -* -1:no valid time) -* notes : the following keywords in path are replaced by date, time and name -* %Y -> yyyy : year (4 digits) (1900-2099) -* %y -> yy : year (2 digits) (00-99) -* %m -> mm : month (01-12) -* %d -> dd : day of month (01-31) -* %h -> hh : hours (00-23) -* %M -> mm : minutes (00-59) -* %S -> ss : seconds (00-59) -* %n -> ddd : day of year (001-366) -* %W -> wwww : gps week (0001-9999) -* %D -> d : day of gps week (0-6) -* %H -> h : hour code (a=0,b=1,c=2,...,x=23) -* %ha-> hh : 3 hours (00,03,06,...,21) -* %hb-> hh : 6 hours (00,06,12,18) -* %hc-> hh : 12 hours (00,12) -* %t -> mm : 15 minutes (00,15,30,45) -* %r -> rrrr : rover id -* %b -> bbbb : base station id -*-----------------------------------------------------------------------------*/ + * replace keywords in file path with date, time, rover and base station id + * args : char *path I file path (see below) + * char *rpath O file path in which keywords replaced (see below) + * gtime_t time I time (gpst) (time.time==0: not replaced) + * char *rov I rover id string ("": not replaced) + * char *base I base station id string ("": not replaced) + * return : status (1:keywords replaced, 0:no valid keyword in the path, + * -1:no valid time) + * notes : the following keywords in path are replaced by date, time and name + * %Y -> yyyy : year (4 digits) (1900-2099) + * %y -> yy : year (2 digits) (00-99) + * %m -> mm : month (01-12) + * %d -> dd : day of month (01-31) + * %h -> hh : hours (00-23) + * %M -> mm : minutes (00-59) + * %S -> ss : seconds (00-59) + * %n -> ddd : day of year (001-366) + * %W -> wwww : gps week (0001-9999) + * %D -> d : day of gps week (0-6) + * %H -> h : hour code (a=0,b=1,c=2,...,x=23) + * %ha-> hh : 3 hours (00,03,06,...,21) + * %hb-> hh : 6 hours (00,06,12,18) + * %hc-> hh : 12 hours (00,12) + * %t -> mm : 15 minutes (00,15,30,45) + * %r -> rrrr : rover id + * %b -> bbbb : base station id + *-----------------------------------------------------------------------------*/ int reppath(const char *path, char *rpath, gtime_t time, const char *rov, - const char *base) + const char *base) { double ep[6],ep0[6]={2000,1,1,0,0,0}; int week,dow,doy,stat=0; char rep[64]; - + strcpy(rpath,path); - + if (!strstr(rpath,"%")) return 0; if (*rov ) stat|=repstr(rpath,"%r",rov ); if (*base) stat|=repstr(rpath,"%b",base); - if (time.time!=0) { - time2epoch(time,ep); - ep0[0]=ep[0]; - dow=(int)floor(time2gpst(time,&week)/86400.0); - doy=(int)floor(timediff(time,epoch2time(ep0))/86400.0)+1; - sprintf(rep,"%02d", ((int)ep[3]/3)*3); stat|=repstr(rpath,"%ha",rep); - sprintf(rep,"%02d", ((int)ep[3]/6)*6); stat|=repstr(rpath,"%hb",rep); - sprintf(rep,"%02d", ((int)ep[3]/12)*12); stat|=repstr(rpath,"%hc",rep); - sprintf(rep,"%04.0f",ep[0]); stat|=repstr(rpath,"%Y",rep); - sprintf(rep,"%02.0f",fmod(ep[0],100.0)); stat|=repstr(rpath,"%y",rep); - sprintf(rep,"%02.0f",ep[1]); stat|=repstr(rpath,"%m",rep); - sprintf(rep,"%02.0f",ep[2]); stat|=repstr(rpath,"%d",rep); - sprintf(rep,"%02.0f",ep[3]); stat|=repstr(rpath,"%h",rep); - sprintf(rep,"%02.0f",ep[4]); stat|=repstr(rpath,"%M",rep); - sprintf(rep,"%02.0f",floor(ep[5])); stat|=repstr(rpath,"%S",rep); - sprintf(rep,"%03d", doy); stat|=repstr(rpath,"%n",rep); - sprintf(rep,"%04d", week); stat|=repstr(rpath,"%W",rep); - sprintf(rep,"%d", dow); stat|=repstr(rpath,"%D",rep); - sprintf(rep,"%c", 'a'+(int)ep[3]); stat|=repstr(rpath,"%H",rep); - sprintf(rep,"%02d", ((int)ep[4]/15)*15); stat|=repstr(rpath,"%t",rep); - } + if (time.time!=0) + { + time2epoch(time,ep); + ep0[0]=ep[0]; + dow=(int)floor(time2gpst(time,&week)/86400.0); + doy=(int)floor(timediff(time,epoch2time(ep0))/86400.0)+1; + sprintf(rep,"%02d", ((int)ep[3]/3)*3); stat|=repstr(rpath,"%ha",rep); + sprintf(rep,"%02d", ((int)ep[3]/6)*6); stat|=repstr(rpath,"%hb",rep); + sprintf(rep,"%02d", ((int)ep[3]/12)*12); stat|=repstr(rpath,"%hc",rep); + sprintf(rep,"%04.0f",ep[0]); stat|=repstr(rpath,"%Y",rep); + sprintf(rep,"%02.0f",fmod(ep[0],100.0)); stat|=repstr(rpath,"%y",rep); + sprintf(rep,"%02.0f",ep[1]); stat|=repstr(rpath,"%m",rep); + sprintf(rep,"%02.0f",ep[2]); stat|=repstr(rpath,"%d",rep); + sprintf(rep,"%02.0f",ep[3]); stat|=repstr(rpath,"%h",rep); + sprintf(rep,"%02.0f",ep[4]); stat|=repstr(rpath,"%M",rep); + sprintf(rep,"%02.0f",floor(ep[5])); stat|=repstr(rpath,"%S",rep); + sprintf(rep,"%03d", doy); stat|=repstr(rpath,"%n",rep); + sprintf(rep,"%04d", week); stat|=repstr(rpath,"%W",rep); + sprintf(rep,"%d", dow); stat|=repstr(rpath,"%D",rep); + sprintf(rep,"%c", 'a'+(int)ep[3]); stat|=repstr(rpath,"%H",rep); + sprintf(rep,"%02d", ((int)ep[4]/15)*15); stat|=repstr(rpath,"%t",rep); + } else if (strstr(rpath,"%ha")||strstr(rpath,"%hb")||strstr(rpath,"%hc")|| - strstr(rpath,"%Y" )||strstr(rpath,"%y" )||strstr(rpath,"%m" )|| - strstr(rpath,"%d" )||strstr(rpath,"%h" )||strstr(rpath,"%M" )|| - strstr(rpath,"%S" )||strstr(rpath,"%n" )||strstr(rpath,"%W" )|| - strstr(rpath,"%D" )||strstr(rpath,"%H" )||strstr(rpath,"%t" )) { - return -1; /* no valid time */ - } + strstr(rpath,"%Y" )||strstr(rpath,"%y" )||strstr(rpath,"%m" )|| + strstr(rpath,"%d" )||strstr(rpath,"%h" )||strstr(rpath,"%M" )|| + strstr(rpath,"%S" )||strstr(rpath,"%n" )||strstr(rpath,"%W" )|| + strstr(rpath,"%D" )||strstr(rpath,"%H" )||strstr(rpath,"%t" )) + { + return -1; /* no valid time */ + } return stat; } + + /* replace keywords in file path and generate multiple paths ------------------- -* replace keywords in file path with date, time, rover and base station id -* generate multiple keywords-replaced paths -* args : char *path I file path (see below) -* char *rpath[] O file paths in which keywords replaced -* int nmax I max number of output file paths -* gtime_t ts I time start (gpst) -* gtime_t te I time end (gpst) -* char *rov I rover id string ("": not replaced) -* char *base I base station id string ("": not replaced) -* return : number of replaced file paths -* notes : see reppath() for replacements of keywords. -* minimum interval of time replaced is 900s. -*-----------------------------------------------------------------------------*/ + * replace keywords in file path with date, time, rover and base station id + * generate multiple keywords-replaced paths + * args : char *path I file path (see below) + * char *rpath[] O file paths in which keywords replaced + * int nmax I max number of output file paths + * gtime_t ts I time start (gpst) + * gtime_t te I time end (gpst) + * char *rov I rover id string ("": not replaced) + * char *base I base station id string ("": not replaced) + * return : number of replaced file paths + * notes : see reppath() for replacements of keywords. + * minimum interval of time replaced is 900s. + *-----------------------------------------------------------------------------*/ int reppaths(const char *path, char *rpath[], int nmax, gtime_t ts, - gtime_t te, const char *rov, const char *base) + gtime_t te, const char *rov, const char *base) { gtime_t time; double tow,tint=86400.0; int i,n=0,week; - + trace(3,"reppaths: path =%s nmax=%d rov=%s base=%s\n",path,nmax,rov,base); - + if (ts.time==0||te.time==0||timediff(ts,te)>0.0) return 0; - + if (strstr(path,"%S")||strstr(path,"%M")||strstr(path,"%t")) tint=900.0; else if (strstr(path,"%h")||strstr(path,"%H")) tint=3600.0; - + tow=time2gpst(ts,&week); time=gpst2time(week,floor(tow/tint)*tint); - - while (timediff(time,te)<=0.0&&nng;i++) { - if (nav->geph[i].sat!=sat) continue; - return CLIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); - } + + if (sys==SYS_GLO) + { + if (0<=frq&&frq<=1) + { + for (i=0;ing;i++) + { + if (nav->geph[i].sat!=sat) continue; + return SPEED_OF_LIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); + } + } + else if (frq==2) + { /* L3 */ + return SPEED_OF_LIGHT/FREQ3_GLO; + } } - else if (frq==2) { /* L3 */ - return CLIGHT/FREQ3_GLO; + else if (sys==SYS_BDS) + { + if (frq==0) return SPEED_OF_LIGHT/FREQ1_BDS; /* B1 */ + else if (frq==1) return SPEED_OF_LIGHT/FREQ2_BDS; /* B2 */ + else if (frq==2) return SPEED_OF_LIGHT/FREQ3_BDS; /* B3 */ + } + else + { + if (frq==0) return SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ + else if (frq==1) return SPEED_OF_LIGHT/FREQ2; /* L2 */ + else if (frq==2) return SPEED_OF_LIGHT/FREQ5; /* L5/E5a */ + else if (frq==3) return SPEED_OF_LIGHT/FREQ6; /* L6/LEX */ + else if (frq==4) return SPEED_OF_LIGHT/FREQ7; /* E5b */ + else if (frq==5) return SPEED_OF_LIGHT/FREQ8; /* E5a+b */ + else if (frq==6) return SPEED_OF_LIGHT/FREQ9; /* S */ } - } - else if (sys==SYS_CMP) { - if (frq==0) return CLIGHT/FREQ1_CMP; /* B1 */ - else if (frq==1) return CLIGHT/FREQ2_CMP; /* B2 */ - else if (frq==2) return CLIGHT/FREQ3_CMP; /* B3 */ - } - else { - if (frq==0) return CLIGHT/FREQ1; /* L1/E1 */ - else if (frq==1) return CLIGHT/FREQ2; /* L2 */ - else if (frq==2) return CLIGHT/FREQ5; /* L5/E5a */ - else if (frq==3) return CLIGHT/FREQ6; /* L6/LEX */ - else if (frq==4) return CLIGHT/FREQ7; /* E5b */ - else if (frq==5) return CLIGHT/FREQ8; /* E5a+b */ - else if (frq==6) return CLIGHT/FREQ9; /* S */ - } return 0.0; } + + /* geometric distance ---------------------------------------------------------- -* compute geometric distance and receiver-to-satellite unit vector -* args : double *rs I satellilte position (ecef at transmission) (m) -* double *rr I receiver position (ecef at reception) (m) -* double *e O line-of-sight vector (ecef) -* return : geometric distance (m) (0>:error/no satellite position) -* notes : distance includes sagnac effect correction -*-----------------------------------------------------------------------------*/ + * compute geometric distance and receiver-to-satellite unit vector + * args : double *rs I satellilte position (ecef at transmission) (m) + * double *rr I receiver position (ecef at reception) (m) + * double *e O line-of-sight vector (ecef) + * return : geometric distance (m) (0>:error/no satellite position) + * notes : distance includes sagnac effect correction + *-----------------------------------------------------------------------------*/ double geodist(const double *rs, const double *rr, double *e) { double r; int i; - + if (norm(rs,3)-RE_WGS84) { - ecef2enu(pos,e,enu); - az=dot(enu,enu,2)<1E-12?0.0:atan2(enu[0],enu[1]); - if (az<0.0) az+=2*PI; - el=asin(enu[2]); - } + + if (pos[2]>-RE_WGS84) + { + ecef2enu(pos,e,enu); + az=dot(enu,enu,2)<1e-12?0.0:atan2(enu[0],enu[1]); + if (az<0.0) az+=2*PI; + el=asin(enu[2]); + } if (azel) {azel[0]=az; azel[1]=el;} return el; } + + /* compute dops ---------------------------------------------------------------- -* compute DOP (dilution of precision) -* args : int ns I number of satellites -* double *azel I satellite azimuth/elevation angle (rad) -* double elmin I elevation cutoff angle (rad) -* double *dop O DOPs {GDOP,PDOP,HDOP,VDOP} -* return : none -* notes : dop[0]-[3] return 0 in case of dop computation error -*-----------------------------------------------------------------------------*/ - - + * compute DOP (dilution of precision) + * args : int ns I number of satellites + * double *azel I satellite azimuth/elevation angle (rad) + * double elmin I elevation cutoff angle (rad) + * double *dop O DOPs {GDOP,PDOP,HDOP,VDOP} + * return : none + * notes : dop[0]-[3] return 0 in case of dop computation error + *-----------------------------------------------------------------------------*/ void dops(int ns, const double *azel, double elmin, double *dop) { double H[4*MAXSAT],Q[16],cosel,sinel; int i,n; - + for (i=0;i<4;i++) dop[i]=0.0; - for (i=n=0;i 0.416) phi= 0.416; else if (phi<-0.416) phi=-0.416; lam=pos[1]/PI+psi*sin(azel[0])/cos(phi*PI); - + /* geomagnetic latitude (semi-circle) */ phi+=0.064*cos((lam-1.617)*PI); - + /* local time (s) */ tt=43200.0*lam+time2gpst(t,&week); tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 */ - + /* slant factor */ f=1.0+16.0*pow(0.53-azel[1]/PI,3.0); - + /* ionospheric delay */ amp=ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3])); per=ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7])); amp=amp< 0.0? 0.0:amp; per=per<72000.0?72000.0:per; x=2.0*PI*(tt-50400.0)/per; - - return CLIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9); + + return SPEED_OF_LIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9); } + + /* ionosphere mapping function ------------------------------------------------- -* compute ionospheric delay mapping function by single layer model -* args : double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* return : ionospheric mapping function -*-----------------------------------------------------------------------------*/ + * compute ionospheric delay mapping function by single layer model + * args : double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * return : ionospheric mapping function + *-----------------------------------------------------------------------------*/ double ionmapf(const double *pos, const double *azel) { if (pos[2]>=HION) return 1.0; return 1.0/cos(asin((RE_WGS84+pos[2])/(RE_WGS84+HION)*sin(PI/2.0-azel[1]))); } + + /* ionospheric pierce point position ------------------------------------------- -* compute ionospheric pierce point (ipp) position and slant factor -* args : double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double re I earth radius (km) -* double hion I altitude of ionosphere (km) -* double *posp O pierce point position {lat,lon,h} (rad,m) -* return : slant factor -* notes : see ref [2], only valid on the earth surface -* fixing bug on ref [2] A.4.4.10.1 A-22,23 -*-----------------------------------------------------------------------------*/ + * compute ionospheric pierce point (ipp) position and slant factor + * args : double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double re I earth radius (km) + * double hion I altitude of ionosphere (km) + * double *posp O pierce point position {lat,lon,h} (rad,m) + * return : slant factor + * notes : see ref [2], only valid on the earth surface + * fixing bug on ref [2] A.4.4.10.1 A-22,23 + *-----------------------------------------------------------------------------*/ double ionppp(const double *pos, const double *azel, double re, - double hion, double *posp) + double hion, double *posp) { double cosaz,rp,ap,sinap,tanap; - + rp=re/(re+hion)*cos(azel[1]); ap=PI/2.0-azel[1]-asin(rp); sinap=sin(ap); tanap=tan(ap); cosaz=cos(azel[0]); posp[0]=asin(sin(pos[0])*cos(ap)+cos(pos[0])*sinap*cosaz); - + if ((pos[0]> 70.0*D2R&& tanap*cosaz>tan(PI/2.0-pos[0]))|| - (pos[0]<-70.0*D2R&&-tanap*cosaz>tan(PI/2.0+pos[0]))) { - posp[1]=pos[1]+PI-asin(sinap*sin(azel[0])/cos(posp[0])); - } - else { - posp[1]=pos[1]+asin(sinap*sin(azel[0])/cos(posp[0])); - } + (pos[0]<-70.0*D2R&&-tanap*cosaz>tan(PI/2.0+pos[0]))) + { + posp[1]=pos[1]+PI-asin(sinap*sin(azel[0])/cos(posp[0])); + } + else + { + posp[1]=pos[1]+asin(sinap*sin(azel[0])/cos(posp[0])); + } return 1.0/sqrt(1.0-rp*rp); } + + /* troposphere model ----------------------------------------------------------- -* compute tropospheric delay by standard atmosphere and saastamoinen model -* args : gtime_t time I time -* double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double humi I relative humidity -* return : tropospheric delay (m) -*-----------------------------------------------------------------------------*/ + * compute tropospheric delay by standard atmosphere and saastamoinen model + * args : gtime_t time I time + * double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double humi I relative humidity + * return : tropospheric delay (m) + *-----------------------------------------------------------------------------*/ double tropmodel(gtime_t time, const double *pos, const double *azel, - double humi) + double humi) { const double temp0=15.0; /* temparature at sea level */ double hgt,pres,temp,e,z,trph,trpw; - - if (pos[2]<-100.0||1E44) return coef[4]; return coef[i-1]*(1.0-lat/15.0+i)+coef[i]*(lat/15.0-i); } + + double mapf(double el, double a, double b, double c) { double sinel=sin(el); return (1.0+a/(1.0+b/(1.0+c)))/(sinel+(a/(sinel+b/(sinel+c)))); } + + double nmf(gtime_t time, const double pos[], const double azel[], - double *mapfw) + double *mapfw) { /* ref [5] table 3 */ /* hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75 */ const double coef[][5]={ - { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, - { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, - { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, - - { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191E-5}, - { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, - { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, - - { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, - { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, - { 4.3472961E-2, 4.6729510E-2, 4.3908931E-2, 4.4626982E-2, 5.4736038E-2} + { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, + { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, + { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, + + { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191e-5}, + { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, + { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, + + { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, + { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, + { 4.3472961e-2, 4.6729510E-2, 4.3908931e-2, 4.4626982E-2, 5.4736038E-2} }; const double aht[]={ 2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */ - + double y,cosy,ah[3],aw[3],dm,el=azel[1],lat=pos[0]*R2D,hgt=pos[2]; int i; - - if (el<=0.0) { - if (mapfw) *mapfw=0.0; - return 0.0; - } + + if (el<=0.0) + { + if (mapfw) *mapfw=0.0; + return 0.0; + } /* year from doy 28, added half a year for southern latitudes */ y=(time2doy(time)-28.0)/365.25+(lat<0.0?0.5:0.0); - + cosy=cos(2.0*PI*y); lat=fabs(lat); - - for (i=0;i<3;i++) { - ah[i]=interpc(coef[i ],lat)-interpc(coef[i+3],lat)*cosy; - aw[i]=interpc(coef[i+6],lat); - } + + for (i=0;i<3;i++) + { + ah[i]=interpc(coef[i ],lat)-interpc(coef[i+3],lat)*cosy; + aw[i]=interpc(coef[i+6],lat); + } /* ellipsoidal height is used instead of height above sea level */ - dm=(1.0/sin(el)-mapf(el,aht[0],aht[1],aht[2]))*hgt/1E3; - + dm=(1.0/sin(el)-mapf(el,aht[0],aht[1],aht[2]))*hgt/1e3; + if (mapfw) *mapfw=mapf(el,aw[0],aw[1],aw[2]); - + return mapf(el,ah[0],ah[1],ah[2])+dm; } #endif /* !IERS_MODEL */ + /* troposphere mapping function ------------------------------------------------ -* compute tropospheric mapping function by NMF -* args : gtime_t t I time -* double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double *mapfw IO wet mapping function (NULL: not output) -* return : dry mapping function -* note : see ref [5] (NMF) and [9] (GMF) -* original JGR paper of [5] has bugs in eq.(4) and (5). the corrected -* paper is obtained from: -* ftp://web.haystack.edu/pub/aen/nmf/NMF_JGR.pdf -*-----------------------------------------------------------------------------*/ + * compute tropospheric mapping function by NMF + * args : gtime_t t I time + * double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double *mapfw IO wet mapping function (NULL: not output) + * return : dry mapping function + * note : see ref [5] (NMF) and [9] (GMF) + * original JGR paper of [5] has bugs in eq.(4) and (5). the corrected + * paper is obtained from: + * ftp://web.haystack.edu/pub/aen/nmf/NMF_JGR.pdf + *-----------------------------------------------------------------------------*/ double tropmapf(gtime_t time, const double pos[], const double azel[], - double *mapfw) + double *mapfw) { #ifdef IERS_MODEL const double ep[]={2000,1,1,12,0,0}; double mjd,lat,lon,hgt,zd,gmfh,gmfw; #endif trace(4,"tropmapf: pos=%10.6f %11.6f %6.1f azel=%5.1f %4.1f\n", - pos[0]*R2D,pos[1]*R2D,pos[2],azel[0]*R2D,azel[1]*R2D); - - if (pos[2]<-1000.0||pos[2]>20000.0) { - if (mapfw) *mapfw=0.0; - return 0.0; - } + pos[0]*R2D,pos[1]*R2D,pos[2],azel[0]*R2D,azel[1]*R2D); + + if (pos[2]<-1000.0||pos[2]>20000.0) + { + if (mapfw) *mapfw=0.0; + return 0.0; + } #ifdef IERS_MODEL mjd=51544.5+(timediff(time,epoch2time(ep)))/86400.0; lat=pos[0]; lon=pos[1]; hgt=pos[2]-geoidh(pos); /* height in m (mean sea level) */ zd =PI/2.0-azel[1]; - + /* call GMF */ gmf_(&mjd,&lat,&lon,&hgt,&zd,&gmfh,&gmfw); - + if (mapfw) *mapfw=gmfw; return gmfh; #else return nmf(time,pos,azel,mapfw); /* NMF */ #endif } + + /* interpolate antenna phase center variation --------------------------------*/ double interpvar(double ang, const double *var) { @@ -3443,235 +3812,260 @@ double interpvar(double ang, const double *var) if (i<0) return var[0]; else if (i>=18) return var[18]; return var[i]*(1.0-a+i)+var[i+1]*(a-i); } + + /* receiver antenna model ------------------------------------------------------ -* compute antenna offset by antenna phase center parameters -* args : pcv_t *pcv I antenna phase center parameters -* double *azel I azimuth/elevation for receiver {az,el} (rad) -* int opt I option (0:only offset,1:offset+pcv) -* double *dant O range offsets for each frequency (m) -* return : none -* notes : current version does not support azimuth dependent terms -*-----------------------------------------------------------------------------*/ + * compute antenna offset by antenna phase center parameters + * args : pcv_t *pcv I antenna phase center parameters + * double *azel I azimuth/elevation for receiver {az,el} (rad) + * int opt I option (0:only offset,1:offset+pcv) + * double *dant O range offsets for each frequency (m) + * return : none + * notes : current version does not support azimuth dependent terms + *-----------------------------------------------------------------------------*/ void antmodel(const pcv_t *pcv, const double *del, const double *azel, - int opt, double *dant) + int opt, double *dant) { double e[3],off[3],cosel=cos(azel[1]); int i,j; - + trace(4,"antmodel: azel=%6.1f %4.1f opt=%d\n",azel[0]*R2D,azel[1]*R2D,opt); - + e[0]=sin(azel[0])*cosel; e[1]=cos(azel[0])*cosel; e[2]=sin(azel[1]); - - for (i=0;ioff[i][j]+del[j]; - - dant[i]=-dot(off,e,3)+(opt?interpvar(90.0-azel[1]*R2D,pcv->var[i]):0.0); - } + + for (i=0;ioff[i][j]+del[j]; + + dant[i]=-dot(off,e,3)+(opt?interpvar(90.0-azel[1]*R2D,pcv->var[i]):0.0); + } trace(5,"antmodel: dant=%6.3f %6.3f\n",dant[0],dant[1]); } + + /* satellite antenna model ------------------------------------------------------ -* compute satellite antenna phase center parameters -* args : pcv_t *pcv I antenna phase center parameters -* double nadir I nadir angle for satellite (rad) -* double *dant O range offsets for each frequency (m) -* return : none -*-----------------------------------------------------------------------------*/ + * compute satellite antenna phase center parameters + * args : pcv_t *pcv I antenna phase center parameters + * double nadir I nadir angle for satellite (rad) + * double *dant O range offsets for each frequency (m) + * return : none + *-----------------------------------------------------------------------------*/ void antmodel_s(const pcv_t *pcv, double nadir, double *dant) { int i; - + trace(4,"antmodel_s: nadir=%6.1f\n",nadir*R2D); - - for (i=0;ivar[i]); - } + + for (i=0;ivar[i]); + } trace(5,"antmodel_s: dant=%6.3f %6.3f\n",dant[0],dant[1]); } + + /* sun and moon position in eci (ref [4] 5.1.1, 5.2.1) -----------------------*/ void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon) { const double ep2000[]={2000,1,1,12,0,0}; double t,f[5],eps,Ms,ls,rs,lm,pm,rm,sine,cose,sinp,cosp,sinl,cosl; - + trace(4,"sunmoonpos_eci: tut=%s\n",time_str(tut,3)); - + t=timediff(tut,epoch2time(ep2000))/86400.0/36525.0; - + /* astronomical arguments */ ast_args(t,f); - + /* obliquity of the ecliptic */ eps=23.439291-0.0130042*t; sine=sin(eps*D2R); cose=cos(eps*D2R); - + /* sun position in eci */ - if (rsun) { - Ms=357.5277233+35999.05034*t; - ls=280.460+36000.770*t+1.914666471*sin(Ms*D2R)+0.019994643*sin(2.0*Ms*D2R); - rs=AU*(1.000140612-0.016708617*cos(Ms*D2R)-0.000139589*cos(2.0*Ms*D2R)); - sinl=sin(ls*D2R); cosl=cos(ls*D2R); - rsun[0]=rs*cosl; - rsun[1]=rs*cose*sinl; - rsun[2]=rs*sine*sinl; - - trace(5,"rsun =%.3f %.3f %.3f\n",rsun[0],rsun[1],rsun[2]); - } + if (rsun) + { + Ms=357.5277233+35999.05034*t; + ls=280.460+36000.770*t+1.914666471*sin(Ms*D2R)+0.019994643*sin(2.0*Ms*D2R); + rs=AU*(1.000140612-0.016708617*cos(Ms*D2R)-0.000139589*cos(2.0*Ms*D2R)); + sinl=sin(ls*D2R); cosl=cos(ls*D2R); + rsun[0]=rs*cosl; + rsun[1]=rs*cose*sinl; + rsun[2]=rs*sine*sinl; + + trace(5,"rsun =%.3f %.3f %.3f\n",rsun[0],rsun[1],rsun[2]); + } /* moon position in eci */ - if (rmoon) { - lm=218.32+481267.883*t+6.29*sin(f[0])-1.27*sin(f[0]-2.0*f[3])+ - 0.66*sin(2.0*f[3])+0.21*sin(2.0*f[0])-0.19*sin(f[1])-0.11*sin(2.0*f[2]); - pm=5.13*sin(f[2])+0.28*sin(f[0]+f[2])-0.28*sin(f[2]-f[0])- - 0.17*sin(f[2]-2.0*f[3]); - rm=RE_WGS84/sin((0.9508+0.0518*cos(f[0])+0.0095*cos(f[0]-2.0*f[3])+ - 0.0078*cos(2.0*f[3])+0.0028*cos(2.0*f[0]))*D2R); - sinl=sin(lm*D2R); cosl=cos(lm*D2R); - sinp=sin(pm*D2R); cosp=cos(pm*D2R); - rmoon[0]=rm*cosp*cosl; - rmoon[1]=rm*(cose*cosp*sinl-sine*sinp); - rmoon[2]=rm*(sine*cosp*sinl+cose*sinp); - - trace(5,"rmoon=%.3f %.3f %.3f\n",rmoon[0],rmoon[1],rmoon[2]); - } + if (rmoon) + { + lm=218.32+481267.883*t+6.29*sin(f[0])-1.27*sin(f[0]-2.0*f[3])+ + 0.66*sin(2.0*f[3])+0.21*sin(2.0*f[0])-0.19*sin(f[1])-0.11*sin(2.0*f[2]); + pm=5.13*sin(f[2])+0.28*sin(f[0]+f[2])-0.28*sin(f[2]-f[0])- + 0.17*sin(f[2]-2.0*f[3]); + rm=RE_WGS84/sin((0.9508+0.0518*cos(f[0])+0.0095*cos(f[0]-2.0*f[3])+ + 0.0078*cos(2.0*f[3])+0.0028*cos(2.0*f[0]))*D2R); + sinl=sin(lm*D2R); cosl=cos(lm*D2R); + sinp=sin(pm*D2R); cosp=cos(pm*D2R); + rmoon[0]=rm*cosp*cosl; + rmoon[1]=rm*(cose*cosp*sinl-sine*sinp); + rmoon[2]=rm*(sine*cosp*sinl+cose*sinp); + + trace(5,"rmoon=%.3f %.3f %.3f\n",rmoon[0],rmoon[1],rmoon[2]); + } } + + /* sun and moon position ------------------------------------------------------- -* get sun and moon position in ecef -* args : gtime_t tut I time in ut1 -* double *erpv I erp value {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) -* double *rsun IO sun position in ecef (m) (NULL: not output) -* double *rmoon IO moon position in ecef (m) (NULL: not output) -* double *gmst O gmst (rad) -* return : none -*-----------------------------------------------------------------------------*/ + * get sun and moon position in ecef + * args : gtime_t tut I time in ut1 + * double *erpv I erp value {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * double *rsun IO sun position in ecef (m) (NULL: not output) + * double *rmoon IO moon position in ecef (m) (NULL: not output) + * double *gmst O gmst (rad) + * return : none + *-----------------------------------------------------------------------------*/ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, - double *rmoon, double *gmst) + double *rmoon, double *gmst) { gtime_t tut; double rs[3],rm[3],U[9],gmst_; - + trace(4,"sunmoonpos: tutc=%s\n",time_str(tutc,3)); - + tut=timeadd(tutc,erpv[2]); /* utc -> ut1 */ - + /* sun and moon position in eci */ sunmoonpos_eci(tut,rsun?rs:NULL,rmoon?rm:NULL); - + /* eci to ecef transformation matrix */ eci2ecef(tutc,erpv,U,&gmst_); - + /* sun and moon postion in ecef */ if (rsun ) matmul("NN",3,1,3,1.0,U,rs,0.0,rsun ); if (rmoon) matmul("NN",3,1,3,1.0,U,rm,0.0,rmoon); if (gmst ) *gmst=gmst_; } + + /* carrier smoothing ----------------------------------------------------------- -* carrier smoothing by Hatch filter -* args : obs_t *obs IO raw observation data/smoothed observation data -* int ns I smoothing window size (epochs) -* return : none -*-----------------------------------------------------------------------------*/ + * carrier smoothing by Hatch filter + * args : obs_t *obs IO raw observation data/smoothed observation data + * int ns I smoothing window size (epochs) + * return : none + *-----------------------------------------------------------------------------*/ void csmooth(obs_t *obs, int ns) { double Ps[2][MAXSAT][NFREQ]={},Lp[2][MAXSAT][NFREQ]={},dcp; int i,j,s,r,n[2][MAXSAT][NFREQ]={}; obsd_t *p; - + trace(3,"csmooth: nobs=%d,ns=%d\n",obs->n,ns); - - for (i=0;in;i++) { - p=&obs->data[i]; s=p->sat; r=p->rcv; - for (j=0;jP[j]==0.0||p->L[j]==0.0) continue; - if (p->LLI[j]) n[r-1][s-1][j]=0; - if (n[r-1][s-1][j]==0) Ps[r-1][s-1][j]=p->P[j]; - else { - dcp=lam_carr[j]*(p->L[j]-Lp[r-1][s-1][j]); - Ps[r-1][s-1][j]=p->P[j]/ns+(Ps[r-1][s-1][j]+dcp)*(ns-1)/ns; - } - if (++n[r-1][s-1][j]P[j]=0.0; else p->P[j]=Ps[r-1][s-1][j]; - Lp[r-1][s-1][j]=p->L[j]; + + for (i=0;in;i++) + { + p=&obs->data[i]; s=p->sat; r=p->rcv; + for (j=0;jP[j]==0.0||p->L[j]==0.0) continue; + if (p->LLI[j]) n[r-1][s-1][j]=0; + if (n[r-1][s-1][j]==0) Ps[r-1][s-1][j]=p->P[j]; + else + { + dcp=lam_carr[j]*(p->L[j]-Lp[r-1][s-1][j]); + Ps[r-1][s-1][j]=p->P[j]/ns+(Ps[r-1][s-1][j]+dcp)*(ns-1)/ns; + } + if (++n[r-1][s-1][j]P[j]=0.0; else p->P[j]=Ps[r-1][s-1][j]; + Lp[r-1][s-1][j]=p->L[j]; + } } - } } + + /* uncompress file ------------------------------------------------------------- -* uncompress (uncompress/unzip/uncompact hatanaka-compression/tar) file -* args : char *file I input file -* char *uncfile O uncompressed file -* return : status (-1:error,0:not compressed file,1:uncompress completed) -* note : creates uncompressed file in tempolary directory -* gzip and crx2rnx commands have to be installed in commands path -*-----------------------------------------------------------------------------*/ + * uncompress (uncompress/unzip/uncompact hatanaka-compression/tar) file + * args : char *file I input file + * char *uncfile O uncompressed file + * return : status (-1:error,0:not compressed file,1:uncompress completed) + * note : creates uncompressed file in tempolary directory + * gzip and crx2rnx commands have to be installed in commands path + *-----------------------------------------------------------------------------*/ int rtk_uncompress(const char *file, char *uncfile) { int stat=0; char *p,cmd[2048]="",tmpfile[1024]="",buff[1024],*fname,*dir=(char*)""; - + trace(3,"rtk_uncompress: file=%s\n",file); - + strcpy(tmpfile,file); if (!(p=strrchr(tmpfile,'.'))) return 0; - + /* uncompress by gzip */ if (!strcmp(p,".z" )||!strcmp(p,".Z" )|| - !strcmp(p,".gz" )||!strcmp(p,".GZ" )|| - !strcmp(p,".zip")||!strcmp(p,".ZIP")) { - - strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; - sprintf(cmd,"gzip -f -d -c \"%s\" > \"%s\"",tmpfile,uncfile); - - if (execcmd(cmd)) { - remove(uncfile); - return -1; + !strcmp(p,".gz" )||!strcmp(p,".GZ" )|| + !strcmp(p,".zip")||!strcmp(p,".ZIP")) + { + strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; + sprintf(cmd,"gzip -f -d -c \"%s\" > \"%s\"",tmpfile,uncfile); + + if (execcmd(cmd)) + { + remove(uncfile); + return -1; + } + strcpy(tmpfile,uncfile); + stat=1; } - strcpy(tmpfile,uncfile); - stat=1; - } /* extract tar file */ - if ((p=strrchr(tmpfile,'.'))&&!strcmp(p,".tar")) { - - strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; - strcpy(buff,tmpfile); - fname=buff; - if ((p=strrchr(buff,'/'))) { - *p='\0'; dir=fname; fname=p+1; - } - sprintf(cmd,"tar -C \"%s\" -xf \"%s\"",dir,tmpfile); - if (execcmd(cmd)) { + if ((p=strrchr(tmpfile,'.'))&&!strcmp(p,".tar")) + { + strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; + strcpy(buff,tmpfile); + fname=buff; + if ((p=strrchr(buff,'/'))) + { + *p='\0'; dir=fname; fname=p+1; + } + sprintf(cmd,"tar -C \"%s\" -xf \"%s\"",dir,tmpfile); + if (execcmd(cmd)) + { + if (stat) remove(tmpfile); + return -1; + } if (stat) remove(tmpfile); - return -1; + stat=1; } - if (stat) remove(tmpfile); - stat=1; - } /* extract hatanaka-compressed file by cnx2rnx */ - else if ((p=strrchr(tmpfile,'.'))&&strlen(p)>3&&(*(p+3)=='d'||*(p+3)=='D')) { - - strcpy(uncfile,tmpfile); - uncfile[p-tmpfile+3]=*(p+3)=='D'?'O':'o'; - sprintf(cmd,"crx2rnx < \"%s\" > \"%s\"",tmpfile,uncfile); - - if (execcmd(cmd)) { - remove(uncfile); + else if ((p=strrchr(tmpfile,'.'))&&strlen(p)>3&&(*(p+3)=='d'||*(p+3)=='D')) + { + + strcpy(uncfile,tmpfile); + uncfile[p-tmpfile+3]=*(p+3)=='D'?'O':'o'; + sprintf(cmd,"crx2rnx < \"%s\" > \"%s\"",tmpfile,uncfile); + + if (execcmd(cmd)) + { + remove(uncfile); + if (stat) remove(tmpfile); + return -1; + } if (stat) remove(tmpfile); - return -1; + stat=1; } - if (stat) remove(tmpfile); - stat=1; - } trace(3,"rtk_uncompress: stat=%d\n",stat); return stat; } + /* expand file path ------------------------------------------------------------ -* expand file path with wild-card (*) in file -* args : char *path I file path to expand (captal insensitive) -* char *paths O expanded file paths -* int nmax I max number of expanded file paths -* return : number of expanded file paths -* notes : the order of expanded files is alphabetical order -*-----------------------------------------------------------------------------*/ + * expand file path with wild-card (*) in file + * args : char *path I file path to expand (captal insensitive) + * char *paths O expanded file paths + * int nmax I max number of expanded file paths + * return : number of expanded file paths + * notes : the order of expanded files is alphabetical order + *-----------------------------------------------------------------------------*/ int expath(const char *path, char *paths[], int nmax) { int i,j,n=0; @@ -3688,29 +4082,34 @@ int expath(const char *path, char *paths[], int nmax) // file=p+1; strncpy(dir,path,p-path+1); dir[p-path+1]='\0'; //} if (!(dp=opendir(*dir?dir:"."))) return 0; - while ((d=readdir(dp))) { - if (*(d->d_name)=='.') continue; - sprintf(s1,"^%s$",d->d_name); - sprintf(s2,"^%s$",file); - for (p=s1;*p;p++) *p=(char)tolower((int)*p); - for (p=s2;*p;p++) *p=(char)tolower((int)*p); + while ((d=readdir(dp))) + { + if (*(d->d_name)=='.') continue; + sprintf(s1,"^%s$",d->d_name); + sprintf(s2,"^%s$",file); + for (p=s1;*p;p++) *p=(char)tolower((int)*p); + for (p=s2;*p;p++) *p=(char)tolower((int)*p); - for (p=s1,q=strtok_r(s2,"*",&r);q;q=strtok_r(NULL,"*",&r)) { - if ((p=strstr(p,q))) p+=strlen(q); else break; + for (p=s1,q=strtok_r(s2,"*",&r);q;q=strtok_r(NULL,"*",&r)) + { + if ((p=strstr(p,q))) p+=strlen(q); else break; + } + if (p&&nd_name); } - if (p&&nd_name); - } closedir(dp); /* sort paths in alphabetical order */ - for (i=0;i0) { - strcpy(tmp,paths[i]); - strcpy(paths[i],paths[j]); - strcpy(paths[j],tmp); - } + for (i=0;i0) + { + strcpy(tmp,paths[i]); + strcpy(paths[i],paths[j]); + strcpy(paths[j],tmp); + } + } } - } for (i=0;i 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_ + * + * 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 + * + *-----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_RTKCMN_H_ +#define GNSS_SDR_RTKLIB_RTKCMN_H_ #include "rtklib.h" @@ -360,4 +272,4 @@ 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_ */ +#endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.cc b/src/algorithms/libs/rtklib/rtklib_sbas.cc index 623bd9cf0..3b4013723 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.cc +++ b/src/algorithms/libs/rtklib/rtklib_sbas.cc @@ -48,863 +48,960 @@ * (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() -*-----------------------------------------------------------------------------*/ + * + * 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 + * + *-----------------------------------------------------------------------------*/ #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; + 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 + const double var[14] = { + 0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326, + 5.1968, 20.7870, 230.9661, 2078.695 }; - return 0msg,13+i,1)) { - if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */ - else if (i<= 61) sat=satno(SYS_GLO,i-37); /* 38- 61: glonass */ - else if (i<=119) sat=0; /* 62-119: future gnss */ - else if (i<=138) sat=satno(SYS_SBS,i); /* 120-138: geo/waas */ - else if (i<=182) sat=0; /* 139-182: reserved */ - else if (i<=192) sat=satno(SYS_SBS,i+10); /* 183-192: qzss ref [2] */ - else if (i<=202) sat=satno(SYS_QZS,i); /* 193-202: qzss ref [2] */ - else sat=0; /* 203- : reserved */ - sbssat->sat[n++].sat=sat; + int i, n, sat; + + trace(4, "decode_sbstype1:\n"); + + for (i = 1, n = 0;i<=210 && nmsg, 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); + 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; + int i, j, iodf, type, udre; + double prc, dt; gtime_t t0; - - trace(4,"decode_sbstype2:\n"); - - if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)) return 0; - - type=getbitu(msg->msg, 8,6); - iodf=getbitu(msg->msg,14,2); - - for (i=0;i<13;i++) { - if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break; - udre=getbitu(msg->msg,174+4*i,4); - t0 =sbssat->sat[j].fcorr.t0; - prc=sbssat->sat[j].fcorr.prc; - sbssat->sat[j].fcorr.t0=gpst2time(msg->week,msg->tow); - sbssat->sat[j].fcorr.prc=getbits(msg->msg,18+i*12,12)*0.125f; - sbssat->sat[j].fcorr.udre=udre+1; - dt=timediff(sbssat->sat[j].fcorr.t0,t0); - if (t0.time==0||dt<=0.0||18.0sat[j].fcorr.ai==0) { - sbssat->sat[j].fcorr.rrc=0.0; - sbssat->sat[j].fcorr.dt=0.0; + + trace(4, "decode_sbstype2:\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 16, 2)) return 0; + + type = getbitu(msg->msg, 8, 6); + iodf = getbitu(msg->msg, 14, 2); + + for (i = 0;i<13;i++) + { + if ((j = 13*((type==0?2:type)-2)+i)>=sbssat->nsat) break; + udre = getbitu(msg->msg, 174+4*i, 4); + t0 = sbssat->sat[j].fcorr.t0; + prc = sbssat->sat[j].fcorr.prc; + sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); + sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18+i*12, 12)*0.125f; + sbssat->sat[j].fcorr.udre = udre+1; + dt = timediff(sbssat->sat[j].fcorr.t0, t0); + if (t0.time==0||dt<=0.0||18.0sat[j].fcorr.ai==0) + { + sbssat->sat[j].fcorr.rrc = 0.0; + sbssat->sat[j].fcorr.dt = 0.0; + } + else + { + sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc-prc)/dt; + sbssat->sat[j].fcorr.dt = dt; + } + sbssat->sat[j].fcorr.iodf = iodf; } - 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); + trace(5, "decode_sbstype2: type=%d iodf=%d\n", type, iodf); return 1; } + + /* decode type 6: integrity info ---------------------------------------------*/ int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat) { - int i,iodf[4],udre; - - trace(4,"decode_sbstype6:\n"); - - for (i=0;i<4;i++) { - iodf[i]=getbitu(msg->msg,14+i*2,2); - } - for (i=0;insat&&isat[i].fcorr.iodf!=iodf[i/13]) continue; - udre=getbitu(msg->msg,22+i*4,4); - sbssat->sat[i].fcorr.udre=udre+1; - } - trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]); + int i, iodf[4], udre; + + trace(4, "decode_sbstype6:\n"); + + for (i = 0;i<4;i++) + { + iodf[i] = getbitu(msg->msg, 14+i*2, 2); + } + for (i = 0;insat && isat[i].fcorr.iodf!=iodf[i/13]) continue; + udre = getbitu(msg->msg, 22+i*4, 4); + sbssat->sat[i].fcorr.udre = udre+1; + } + trace(5, "decode_sbstype6: iodf=%d %d %d %d\n", iodf[0], iodf[1], iodf[2], iodf[3]); return 1; } + + /* decode type 7: fast correction degradation factor -------------------------*/ int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) { int i; - - trace(4,"decode_sbstype7\n"); - - if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)) return 0; - - sbssat->tlat=getbitu(msg->msg,14,4); - - for (i=0;insat&&isat[i].fcorr.ai=getbitu(msg->msg,22+i*4,4); - } + + trace(4, "decode_sbstype7\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 18, 2)) return 0; + + sbssat->tlat = getbitu(msg->msg, 14, 4); + + for (i = 0;insat && isat[i].fcorr.ai = getbitu(msg->msg, 22+i*4, 4); + } return 1; } + + /* decode type 9: geo navigation message -------------------------------------*/ int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) { - seph_t seph={}; - int i,sat,t; - - trace(4,"decode_sbstype9:\n"); - - if (!(sat=satno(SYS_SBS,msg->prn))) { - trace(2,"invalid prn in sbas type 9: prn=%3d\n",msg->prn); - return 0; - } - t=(int)getbitu(msg->msg,22,13)*16-(int)msg->tow%86400; + 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); + 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)*TWO_N31; + seph.af1 = getbits(msg->msg, 218, 8)*TWO_N39/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;} + int i, j, n, m, band = getbitu(msg->msg, 18, 4); + + trace(4, "decode_sbstype18:\n"); + + if (0<=band && band<= 8) {p = igpband1[band ]; m = 8;} + else if (9<=band && band<=10) {p = igpband2[band-9]; m = 5;} else return 0; - - sbsion[band].iodi=(short)getbitu(msg->msg,22,2); - - for (i=1,n=0;i<=201;i++) { - if (!getbitu(msg->msg,23+i,1)) continue; - for (j=0;jmsg, 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); + 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"); - + 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); + + 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) * TWO_N31; + 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"); - + 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; + + 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) * TWO_N11; + } + sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p+47, 11) * TWO_N31; + sbssat->sat[n-1].lcorr.daf1 = getbits(msg->msg, p+82, 8) * TWO_N39; + 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); + 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); + 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); } - } - 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); + 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); + 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; + 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); + 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 -*-----------------------------------------------------------------------------*/ + * 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;*/ + 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; + return stat ? type : -1; } + + /* read sbas log file --------------------------------------------------------*/ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te, - sbs_t *sbs) + sbs_t *sbs) { sbsmsg_t *sbs_msgs; - int i,week,prn,ch,msg; + int i, week, prn, ch, msg; unsigned int b; - double tow,ep[6]={}; - char buff[256],*p; + 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 */ + + 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; } - 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); + 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; } - else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) { /* NovAtel OEM4/V */ - if (!(p=getfield(buff,6))) continue; - if (sscanf(p,"%d,%lf",&week,&tow)<2) continue; - if (!(p=strchr(++p,';'))) continue; - if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue; - if (!(p=getfield(p,4))) continue; - } - else if (!strncmp(buff,"$FRMA",5)) { /* NovAtel OEM3 */ - if (!(p=getfield(buff,2))) continue; - if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue; - if (!(p=getfield(p,6))) continue; - if (weekn>=sbs->nmax) { - sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2; - if (!(sbs_msgs=(sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) { - trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax); - free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0; - return; - } - sbs->msgs=sbs_msgs; - } - sbs->msgs[sbs->n].week=week; - sbs->msgs[sbs->n].tow=(int)(tow+0.5); - sbs->msgs[sbs->n].prn=prn; - for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0; - for (i=0;*(p-1)&&*p&&i<29;p+=2,i++) { - if (sscanf(p,"%2X",&b)==1) sbs->msgs[sbs->n].msg[i]=(unsigned char)b; - } - sbs->msgs[sbs->n++].msg[28]&=0xC0; - } fclose(fp); } + + /* compare sbas messages -----------------------------------------------------*/ int cmpmsgs(const void *p1, const void *p2) { - sbsmsg_t *q1=(sbsmsg_t *)p1,*q2=(sbsmsg_t *)p2; + sbsmsg_t *q1 = (sbsmsg_t *)p1,*q2 = (sbsmsg_t *)p2; return q1->week!=q2->week?q1->week-q2->week: - (q1->towtow?-1:(q1->tow>q2->tow?1:q1->prn-q2->prn)); + (q1->towtow?-1:(q1->tow>q2->tow?1:q1->prn-q2->prn)); } + + /* read sbas message file ------------------------------------------------------ -* read sbas message file -* args : char *file I sbas message file (wind-card * is expanded) -* int sel I sbas satellite prn number selection (0:all) -* (gtime_t ts I start time) -* (gtime_t te I end time ) -* sbs_t *sbs IO sbas messages -* return : number of sbas messages -* notes : sbas message are appended and sorted. before calling the funciton, -* sbs->n, sbs->nmax and sbs->msgs must be set properly. (initially -* sbs->n=sbs->nmax=0, sbs->msgs=NULL) -* only the following file extentions after wild card expanded are valid -* to read. others are skipped -* .sbs, .SBS, .ems, .EMS -*-----------------------------------------------------------------------------*/ + * 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) + sbs_t *sbs) { - char *efiles[MAXEXFILE]={},*ext; + char *efiles[MAXEXFILE] = {},*ext; int i,n; - + trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel); - - for (i=0;i=0;i--) free(efiles[i]); - return 0; + + for (i = 0;i=0;i--) free(efiles[i]); + return 0; + } } - } /* expand wild card in file path */ - n=expath(file,efiles,MAXEXFILE); - - for (i=0;in>0) { - qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs); - } + 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={}; - + 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 -*-----------------------------------------------------------------------------*/ + * 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; - + 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]); + 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) + const sbsigp_t **igp, double *x, double *y) { int i,latp[2],lonp[4]; - double lat=pos[0]*R2D,lon=pos[1]*R2D; + 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; + 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 if (-85.0<=lat&&lat<-75.0) { - lonp[0]=(int)floor((lon-50.0)/90.0)*90+40; - lonp[2]=lonp[0]+90; + 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; + } } - else if (lat>=85.0) { - for (i=0;i<4;i++) lonp[i]=(int)floor(lon/90.0)*90; + for (i = 0;i<4;i++) if (lonp[i]==180) lonp[i]=-180; + for (i = 0;i<=MAXBAND;i++) + { + for (p = ion[i].igp;pt0.time==0) continue; + if (p->lat==latp[0] && p->lon==lonp[0] && p->give>0) igp[0] = p; + else if (p->lat==latp[1] && p->lon==lonp[1] && p->give>0) igp[1] = p; + else if (p->lat==latp[0] && p->lon==lonp[2] && p->give>0) igp[2] = p; + else if (p->lat==latp[1] && p->lon==lonp[3] && p->give>0) igp[3] = p; + if (igp[0] && igp[1] && igp[2] && igp[3]) return; + } } - else if (lat<-85.0) { - for (i=0;i<4;i++) lonp[i]=(int)floor((lon-50.0)/90.0)*90+40; - } - } - for (i=0;i<4;i++) if (lonp[i]==180) lonp[i]=-180; - for (i=0;i<=MAXBAND;i++) { - for (p=ion[i].igp;pt0.time==0) continue; - if (p->lat==latp[0]&&p->lon==lonp[0]&&p->give>0) igp[0]=p; - else if (p->lat==latp[1]&&p->lon==lonp[1]&&p->give>0) igp[1]=p; - else if (p->lat==latp[0]&&p->lon==lonp[2]&&p->give>0) igp[2]=p; - else if (p->lat==latp[1]&&p->lon==lonp[3]&&p->give>0) igp[3]=p; - if (igp[0]&&igp[1]&&igp[2]&&igp[3]) return; - } - } } + + /* sbas ionospheric delay correction ------------------------------------------- -* compute sbas ionosphric delay correction -* args : gtime_t time I time -* nav_t *nav I navigation data -* double *pos I receiver position {lat,lon,height} (rad/m) -* double *azel I satellite azimuth/elavation angle (rad) -* double *delay O slant ionospheric delay (L1) (m) -* double *var O variance of ionospheric delay (m^2) -* return : status (1:ok, 0:no correction) -* notes : before calling the function, sbas ionosphere correction parameters -* in navigation data (nav->sbsion) must be set by callig -* sbsupdatecorr() -*-----------------------------------------------------------------------------*/ + * 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 *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} */ - + 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; + 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); - + 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; - + 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} + 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]; - } + 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) -*-----------------------------------------------------------------------------*/ + * 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) + 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; + const double k1 = 77.604,k2 = 382000.0,rd = 287.054,gm = 9.784,g = 9.80665; + static double pos_[3] = {}, zh = 0.0, zw = 0.0; int i; - double c,met[10],sinel=sin(azel[1]),h=pos[2],m; - - trace(4,"sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, - azel[0]*R2D,azel[1]*R2D); - - if (pos[2]<-100.0||10000.01E-7||fabs(pos[1]-pos_[1])>1E-7|| - fabs(pos[2]-pos_[2])>1.0) { - getmet(pos[0]*R2D,met); - c=cos(2.0*PI*(time2doy(time)-(pos[0]>=0.0?28.0:211.0))/365.25); - for (i=0;i<5;i++) met[i]-=met[i+5]*c; - zh=1E-6*k1*rd*met[0]/gm; - zw=1E-6*k2*rd/(gm*(met[4]+1.0)-met[3]*rd)*met[2]/met[1]; - zh*=pow(1.0-met[3]*h/met[1],g/(rd*met[3])); - zw*=pow(1.0-met[3]*h/met[1],(met[4]+1.0)*g/(rd*met[3])-1.0); - for (i=0;i<3;i++) pos_[i]=pos[i]; - } - m=1.001/sqrt(0.002001+sinel*sinel); - *var=0.12*0.12*m*m; + 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) + double *drs, double *ddts) { const sbssatp_t *p; double t; int i; - + trace(3,"sbslongcorr: sat=%2d\n",sat); - - for (p=sbssat->sat;psat+sbssat->nsat;p++) { - if (p->sat!=sat||p->lcorr.t0.time==0) continue; - t=timediff(time,p->lcorr.t0); - if (fabs(t)>MAXSBSAGEL) { - trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", - time_str(time,0),sat,t); - return 0; + + for (p = sbssat->sat;psat+sbssat->nsat;p++) + { + if (p->sat != sat || p->lcorr.t0.time == 0) continue; + t = timediff(time, p->lcorr.t0); + if (fabs(t) > MAXSBSAGEL) + { + trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", + time_str(time,0), sat, t); + return 0; + } + for (i = 0; i < 3;i++) drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t; + *ddts = p->lcorr.daf0 + p->lcorr.daf1 * t; + + trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", + sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT); + + return 1; } - 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) + double *prc, double *var) { const sbssatp_t *p; double t; - + trace(3,"sbsfastcorr: sat=%2d\n",sat); - - for (p=sbssat->sat;psat+sbssat->nsat;p++) { - if (p->sat!=sat) continue; - if (p->fcorr.t0.time==0) break; - t=timediff(time,p->fcorr.t0)+sbssat->tlat; - - /* expire age of correction or UDRE==14 (not monitored) */ - if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue; - *prc=p->fcorr.prc; + + for (p = sbssat->sat;psat+sbssat->nsat;p++) + { + if (p->sat!=sat) continue; + if (p->fcorr.t0.time == 0) break; + t = timediff(time, p->fcorr.t0) + sbssat->tlat; + + /* expire age of correction or UDRE==14 (not monitored) */ + if (fabs(t) > MAXSBSAGEF || p->fcorr.udre >= 15) continue; + *prc = p->fcorr.prc; #ifdef RRCENA - if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) { - *prc+=p->fcorr.rrc*t; - } + 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); + *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 -*-----------------------------------------------------------------------------*/ + * 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 *dts, double *var) { - double drs[3]={},dclk=0.0,prc=0.0; + 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; - } + 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; - + if (!sbsfastcorr(time, sat, &nav->sbssat, &prc, var)) + { + return 0; + } + for (i = 0; i < 3; i++) rs[i] += drs[i]; + + dts[0] += dclk + prc / SPEED_OF_LIGHT; + 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); - + sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT, *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) -*-----------------------------------------------------------------------------*/ + * 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) + 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 */ + + if (time.time == 0) return 0; + tow = time2gpst(time,&sbsmsg->week); + sbsmsg->tow = (int)(tow + DTTOL); + sbsmsg->prn = prn; + for (i = 0; i < 7; i++) for (j = 0; j < 4; j++) + { + sbsmsg->msg[i*4+j] = (unsigned char)(words[i] >> ((3-j)*8)); + } + sbsmsg->msg[28] = (unsigned char)(words[7] >> 18 ) & 0xC0; + for (i = 28; i > 0; i--) f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i-1] << 2); + f[0] = sbsmsg->msg[0] >> 6; + + return rtk_crc24q(f, 29) == (words[7] & 0xFFFFFF); /* check crc */ } diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.h b/src/algorithms/libs/rtklib/rtklib_sbas.h index f3ec8ee51..9f55bfc04 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.h +++ b/src/algorithms/libs/rtklib/rtklib_sbas.h @@ -48,43 +48,21 @@ * (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() -*-----------------------------------------------------------------------------*/ + * + * 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 + * + *-----------------------------------------------------------------------------*/ -#ifndef RTKLIB_SBAS_H_ -#define RTKLIB_SBAS_H_ +#ifndef GNSS_SDR_RTKLIB_SBAS_H_ +#define GNSS_SDR_RTKLIB_SBAS_H_ #include "rtklib.h" #include "rtklib_rtkcmn.h" @@ -94,51 +72,51 @@ /* 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, +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, +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, +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, +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, +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, +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}; +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 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}} +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}} }; @@ -146,6 +124,7 @@ 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); @@ -158,6 +137,7 @@ 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); @@ -184,4 +164,4 @@ int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, sbsmsg_t *sbsmsg); -#endif /* RTKLIB_SBAS_H_ */ +#endif /* GNSS_SDR_RTKLIB_SBAS_H_ */ diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index ec6b79fe2..6c50153a8 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -35,19 +35,20 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants -const double GPS_C_m_s = 299792458.0; //!< The speed of light, [m/s] +const double GPS_C_m_s = SPEED_OF_LIGHT; //!< The speed of light, [m/s] const double GPS_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E const double GPS_TWO_PI = 6.283185307179586;//!< 2Pi as defined in IS-GPS-200E -const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s] +const double OMEGA_EARTH_DOT = DEFAULT_OMEGA_EARTH_DOT; //!< Earth rotation rate, [rad/s] const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)] // carrier and code frequencies -const double GPS_L1_FREQ_HZ = 1.57542e9; //!< L1 [Hz] +const double GPS_L1_FREQ_HZ = FREQ1; //!< L1 [Hz] const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] diff --git a/src/core/system_parameters/GPS_L2C.h b/src/core/system_parameters/GPS_L2C.h index c6c703952..689cc524f 100644 --- a/src/core/system_parameters/GPS_L2C.h +++ b/src/core/system_parameters/GPS_L2C.h @@ -36,6 +36,7 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants const double GPS_L2_C_m_s = 299792458.0; //!< The speed of light, [m/s] @@ -48,7 +49,7 @@ const double GPS_L2_F = -4.442807633e-10; //!< Constant, [s/(m)^(1 // carrier and code frequencies -const double GPS_L2_FREQ_HZ = 1.2276e9; //!< L2 [Hz] +const double GPS_L2_FREQ_HZ = FREQ2; //!< L2 [Hz] const double GPS_L2_M_CODE_RATE_HZ = 0.5115e6; //!< GPS L2 M code rate [chips/s] const int GPS_L2_M_CODE_LENGTH_CHIPS = 10230; //!< GPS L2 M code length [chips] diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index 258a9e954..2790cd11c 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -37,6 +37,7 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD @@ -48,7 +49,7 @@ const double GALILEO_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] const double GALILEO_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)] // carrier and code frequencies -const double Galileo_E1_FREQ_HZ = 1.57542e9; //!< Galileo E1 carrier frequency [Hz] +const double Galileo_E1_FREQ_HZ = FREQ1; //!< Galileo E1 carrier frequency [Hz] const double Galileo_E1_CODE_CHIP_RATE_HZ = 1.023e6; //!< Galileo E1 code rate [chips/s] const double Galileo_E1_CODE_PERIOD = 0.004; //!< Galileo E1 code period [s] const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-carrier 'a' rate [Hz] diff --git a/src/core/system_parameters/Galileo_E5a.h b/src/core/system_parameters/Galileo_E5a.h index 70ec19a1e..cbd16c752 100644 --- a/src/core/system_parameters/Galileo_E5a.h +++ b/src/core/system_parameters/Galileo_E5a.h @@ -35,11 +35,11 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" -// Physical constants already defined in E1 // Carrier and code frequencies -const double Galileo_E5a_FREQ_HZ = 1.176450e9; //!< Galileo E5a carrier frequency [Hz] +const double Galileo_E5a_FREQ_HZ = FREQ5; //!< Galileo E5a carrier frequency [Hz] const double Galileo_E5a_CODE_CHIP_RATE_HZ = 1.023e7; //!< Galileo E5a code rate [chips/s] const double Galileo_E5a_I_TIERED_CODE_PERIOD = 0.020; //!< Galileo E5a-I tiered code period [s] const double Galileo_E5a_Q_TIERED_CODE_PERIOD = 0.100; //!< Galileo E5a-Q tiered code period [s] diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 1ed316ed5..160f0bb4d 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -40,6 +40,9 @@ PI_TWO_PX ==> Pi*2^X ONE_PI_TWO_PX = (1/Pi)*2^X */ + +const double PI = 3.1415926535897932; //!< pi + const double TWO_P4 = (16); //!< 2^4 const double TWO_P11 = (2048); //!< 2^11 const double TWO_P12 = (4096); //!< 2^12 @@ -53,6 +56,7 @@ const double TWO_P57 = (1.441151880758559e+017); //!< 2^57 const double TWO_N2 = (0.25); //!< 2^-2 const double TWO_N5 = (0.03125); //!< 2^-5 +const double TWO_N6 = (0.015625); //!< 2^-6 const double TWO_N8 = (0.00390625); //!< 2^-8 const double TWO_N9 = (0.001953125); //!< 2^-9 const double TWO_N10 = (0.0009765625); //!< 2^-10 @@ -60,9 +64,11 @@ const double TWO_N11 = (4.882812500000000e-004); //!< 2^-11 const double TWO_N14 = (0.00006103515625); //!< 2^-14 const double TWO_N15 = (0.00003051757813); //!< 2^-15 const double TWO_N16 = (0.0000152587890625); //!< 2^-16 +const double TWO_N17 = (7.629394531250000e-006); //!< 2^-17 const double TWO_N19 = (1.907348632812500e-006); //!< 2^-19 const double TWO_N20 = (9.536743164062500e-007); //!< 2^-20 const double TWO_N21 = (4.768371582031250e-007); //!< 2^-21 +const double TWO_N23 = (1.192092895507810e-007); //!< 2^-23 const double TWO_N24 = (5.960464477539063e-008); //!< 2^-24 const double TWO_N25 = (2.980232238769531e-008); //!< 2^-25 const double TWO_N27 = (7.450580596923828e-009); //!< 2^-27 @@ -73,8 +79,9 @@ const double TWO_N32 = (2.328306436538696e-010); //!< 2^-32 const double TWO_N33 = (1.164153218269348e-010); //!< 2^-33 const double TWO_N34 = (5.82076609134674e-011); //!< 2^-34 const double TWO_N35 = (2.91038304567337e-011); //!< 2^-35 - const double TWO_N38 = (3.637978807091713e-012); //!< 2^-38 +const double TWO_N39 = (1.818989403545856e-012); //!< 2^-39 +const double TWO_N40 = (9.094947017729280e-013); //!< 2^-40 const double TWO_N43 = (1.136868377216160e-013); //!< 2^-43 const double TWO_N44 = (5.684341886080802e-14); //!< 2^-44 const double TWO_N46 = (1.4210854715202e-014); //!< 2^-46 @@ -95,4 +102,13 @@ const double PI_TWO_N31 = (1.462918079267160e-009); //!< Pi*2^-31 const double PI_TWO_N38 = (1.142904749427469e-011); //!< Pi*2^-38 const double PI_TWO_N23 = (3.745070282923929e-007); //!< Pi*2^-23 +const double D2R = (PI/180.0); //!< deg to rad */ +const double R2D = (180.0/PI); //!< rad to deg */ +const double SC2RAD = 3.1415926535898; //!< semi-circle to radian (IS-GPS) +const double AS2R = (D2R / 3600.0); //!< arc sec to radian + +const double DEFAULT_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Default Earth rotation rate, [rad/s] +const double SPEED_OF_LIGHT = 299792458.0; //!< [m/s] +const double AU = 149597870691.0; //!< 1 Astronomical Unit AU (m) distance from Earth to the Sun. + #endif /* GNSS_SDR_MATH_CONSTANTS_H_ */ diff --git a/src/core/system_parameters/gnss_frequencies.h b/src/core/system_parameters/gnss_frequencies.h new file mode 100644 index 000000000..bd85aa414 --- /dev/null +++ b/src/core/system_parameters/gnss_frequencies.h @@ -0,0 +1,53 @@ +/*! + * \file gnss_frequencies.h + * \brief GNSS Frequencies + * \author Carles Fernandez, 2017. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GNSS_FREQUENCIES_H_ +#define GNSS_SDR_GNSS_FREQUENCIES_H_ + +const double FREQ1 = 1.57542e9; //!< L1/E1 frequency (Hz) +const double FREQ2 = 1.22760e9; //!< L2 frequency (Hz) +const double FREQ5 = 1.17645e9; //!< L5/E5a frequency (Hz) +const double FREQ6 = 1.27875e9; //!< E6/LEX frequency (Hz) +const double FREQ7 = 1.20714e9; //!< E5b frequency (Hz) +const double FREQ8 = 1.191795e9; //!< E5a+b frequency (Hz) +const double FREQ9 = 2.492028e9; //!< S frequency (Hz) +const double FREQ1_GLO = 1.60200e9; //!< GLONASS G1 base frequency (Hz) +const double DFRQ1_GLO = 0.56250e6; //!< GLONASS G1 bias frequency (Hz/n) +const double FREQ2_GLO = 1.24600e9; //!< GLONASS G2 base frequency (Hz) +const double DFRQ2_GLO = 0.43750e6; //!< GLONASS G2 bias frequency (Hz/n) +const double FREQ3_GLO = 1.202025e9; //!< GLONASS G3 frequency (Hz) +const double FREQ1_BDS = 1.561098e9; //!< BeiDou B1 frequency (Hz) +const double FREQ2_BDS = 1.20714e9; //!< BeiDou B2 frequency (Hz) +const double FREQ3_BDS = 1.26852e9; //!< BeiDou B3 frequency (Hz) + +#endif +