From ecd1612680dea4366ca81ac9528eb276de98bde4 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 21 Apr 2017 16:15:38 +0200 Subject: [PATCH] Revert "Refactor of constants, remove defines" This reverts commit 27ab390944d31dc268350f4303a401a544d3d8b8. --- 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 | 1285 +++-- src/algorithms/libs/rtklib/rtklib_ephemeris.h | 36 +- src/algorithms/libs/rtklib/rtklib_ionex.cc | 834 ++-- src/algorithms/libs/rtklib/rtklib_ionex.h | 39 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 1190 +++-- src/algorithms/libs/rtklib/rtklib_pntpos.h | 20 +- src/algorithms/libs/rtklib/rtklib_preceph.cc | 1283 +++-- 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 | 1453 +++--- 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, 5521 insertions(+), 6220 deletions(-) delete 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 437acd35c..893c4c01e 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]=SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ - nav_data.lam[i][1]=SPEED_OF_LIGHT/FREQ2; /* L2 */ + nav_data.lam[i][0]=CLIGHT/FREQ1; /* L1/E1 */ + nav_data.lam[i][1]=CLIGHT/FREQ2; /* L2 */ } result=pntpos(obs_data, valid_obs, &nav_data, &rtklib_opt, &old_pvt_sol, NULL, NULL,rtklib_msg); diff --git a/src/algorithms/libs/rtklib/CMakeLists.txt b/src/algorithms/libs/rtklib/CMakeLists.txt index 9a172fb53..fd582ea9d 100644 --- a/src/algorithms/libs/rtklib/CMakeLists.txt +++ b/src/algorithms/libs/rtklib/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) +# Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors) # # This file is part of GNSS-SDR. # @@ -38,13 +38,14 @@ 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} @@ -54,4 +55,6 @@ 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 6d6cc26ac..e8bb7bd76 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -48,300 +48,356 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - -#ifndef GNSS_SDR_RTKLIB_H_ -#define GNSS_SDR_RTKLIB_H_ +#ifndef RTKLIB_H_ +#define 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 '/' -const int 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 double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m) -const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84) +#define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */ -const double HION = 350000.0; //!< ionosphere height (m) +#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */ +#define FE_WGS84 (1.0/298.257223563) /* earth flattening (WGS84) */ -const unsigned int POLYCRC32 = 0xEDB88320u; //!< CRC32 polynomial -const unsigned int POLYCRC24Q = 0x1864CFBu; //!< CRC24Q polynomial +#define HION 350000.0 /* ionosphere height (m) */ -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 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 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 POLYCRC32 0xEDB88320u /* CRC32 polynomial */ +#define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */ -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 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 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 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 MAXFREQ = 7; //!< max NFREQ +#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 MAXLEAPS = 64; //!< max number of leap seconds table */ -const double DTTOL = 0.005; //!< tolerance of time difference (s) */ +#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 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 MAXFREQ 7 /* max NFREQ */ -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 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 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 MAXLEAPS 64 /* max number of leap seconds table */ +#define DTTOL 0.005 /* tolerance of time difference (s) */ -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 */ +#define NFREQ 3 +#define NEXOBS 0 /* number of extended obs codes */ +#define MAXANT 64 /* max length of station name/antenna type */ + +#define MINPRNGPS 1 /* min satellite PRN number of GPS */ +#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ +#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */ +#define NSYSGPS 1 + +#define SYS_NONE 0x00 /* navigation system: none */ +#define SYS_GPS 0x01 /* navigation system: GPS */ +#define SYS_SBS 0x02 /* navigation system: SBAS */ +#define SYS_GLO 0x04 /* navigation system: GLONASS */ +#define SYS_GAL 0x08 /* navigation system: Galileo */ +#define SYS_QZS 0x10 /* navigation system: QZSS */ +#define SYS_CMP 0x20 /* navigation system: BeiDou */ +#define SYS_IRN 0x40 /* navigation system: IRNS */ +#define SYS_LEO 0x80 /* navigation system: LEO */ +#define SYS_ALL 0xFF /* navigation system: all */ + +#define CODE_NONE 0 /* obs code: none or unknown */ +#define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ +#define CODE_L1P 2 /* obs code: L1P,G1P (GPS,GLO) */ +#define CODE_L1W 3 /* obs code: L1 Z-track (GPS) */ +#define CODE_L1Y 4 /* obs code: L1Y (GPS) */ +#define CODE_L1M 5 /* obs code: L1M (GPS) */ +#define CODE_L1N 6 /* obs code: L1codeless (GPS) */ +#define CODE_L1S 7 /* obs code: L1C(D) (GPS,QZS) */ +#define CODE_L1L 8 /* obs code: L1C(P) (GPS,QZS) */ +#define CODE_L1E 9 /* (not used) */ +#define CODE_L1A 10 /* obs code: E1A (GAL) */ +#define CODE_L1B 11 /* obs code: E1B (GAL) */ +#define CODE_L1X 12 /* obs code: E1B+C,L1C(D+P) (GAL,QZS) */ +#define CODE_L1Z 13 /* obs code: E1A+B+C,L1SAIF (GAL,QZS) */ +#define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */ +#define CODE_L2D 15 /* obs code: L2 L1C/A-(P2-P1) (GPS) */ +#define CODE_L2S 16 /* obs code: L2C(M) (GPS,QZS) */ +#define CODE_L2L 17 /* obs code: L2C(L) (GPS,QZS) */ +#define CODE_L2X 18 /* obs code: L2C(M+L),B1I+Q (GPS,QZS,CMP) */ +#define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */ +#define CODE_L2W 20 /* obs code: L2 Z-track (GPS) */ +#define CODE_L2Y 21 /* obs code: L2Y (GPS) */ +#define CODE_L2M 22 /* obs code: L2M (GPS) */ +#define CODE_L2N 23 /* obs code: L2codeless (GPS) */ +#define CODE_L5I 24 /* obs code: L5/E5aI (GPS,GAL,QZS,SBS) */ +#define CODE_L5Q 25 /* obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ +#define CODE_L5X 26 /* obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) */ +#define CODE_L7I 27 /* obs code: E5bI,B2I (GAL,CMP) */ +#define CODE_L7Q 28 /* obs code: E5bQ,B2Q (GAL,CMP) */ +#define CODE_L7X 29 /* obs code: E5bI+Q,B2I+Q (GAL,CMP) */ +#define CODE_L6A 30 /* obs code: E6A (GAL) */ +#define CODE_L6B 31 /* obs code: E6B (GAL) */ +#define CODE_L6C 32 /* obs code: E6C (GAL) */ +#define CODE_L6X 33 /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,CMP) */ +#define CODE_L6Z 34 /* obs code: E6A+B+C (GAL) */ +#define CODE_L6S 35 /* obs code: LEXS (QZS) */ +#define CODE_L6L 36 /* obs code: LEXL (QZS) */ +#define CODE_L8I 37 /* obs code: E5(a+b)I (GAL) */ +#define CODE_L8Q 38 /* obs code: E5(a+b)Q (GAL) */ +#define CODE_L8X 39 /* obs code: E5(a+b)I+Q (GAL) */ +#define CODE_L2I 40 /* obs code: B1I (BDS) */ +#define CODE_L2Q 41 /* obs code: B1Q (BDS) */ +#define CODE_L6I 42 /* obs code: B3I (BDS) */ +#define CODE_L6Q 43 /* obs code: B3Q (BDS) */ +#define CODE_L3I 44 /* obs code: G3I (GLO) */ +#define CODE_L3Q 45 /* obs code: G3Q (GLO) */ +#define CODE_L3X 46 /* obs code: G3I+Q (GLO) */ +#define CODE_L1I 47 /* obs code: B1I (BDS) */ +#define CODE_L1Q 48 /* obs code: B1Q (BDS) */ +#define CODE_L5A 49 /* obs code: L5A SPS (IRN) */ +#define CODE_L5B 50 /* obs code: L5B RS(D) (IRN) */ +#define CODE_L5C 51 /* obs code: L5C RS(P) (IRN) */ +#define CODE_L9A 52 /* obs code: SA SPS (IRN) */ +#define CODE_L9B 53 /* obs code: SB RS(D) (IRN) */ +#define CODE_L9C 54 /* obs code: SC RS(P) (IRN) */ +#define CODE_L9X 55 /* obs code: SB+C (IRN) */ +#define MAXCODE 55 /* max number of obs code */ #ifdef ENAGLO -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; +#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ +#define MAXPRNGLO 27 /* max satellite slot number of GLONASS */ +#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */ +#define NSYSGLO 1 #else -const unsigned int MINPRNGLO = 0; -const unsigned int MAXPRNGLO = 0; -const unsigned int NSATGLO = 0; -const unsigned int NSYSGLO = 0; +#define MINPRNGLO 0 +#define MAXPRNGLO 0 +#define NSATGLO 0 +#define NSYSGLO 0 #endif - #ifdef ENAGAL -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; +#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ +#define MAXPRNGAL 30 /* max satellite PRN number of Galileo */ +#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */ +#define NSYSGAL 1 #else -const unsigned int MINPRNGAL = 0; -const unsigned int MAXPRNGAL = 0; -const unsigned int NSATGAL = 0; -const unsigned int NSYSGAL = 0; +#define MINPRNGAL 0 +#define MAXPRNGAL 0 +#define NSATGAL 0 +#define NSYSGAL 0 #endif - #ifdef ENAQZS -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; +#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ +#define MAXPRNQZS 199 /* max satellite PRN number of QZSS */ +#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */ +#define MAXPRNQZS_S 189 /* max satellite PRN number of QZSS SAIF */ +#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */ +#define NSYSQZS 1 #else -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; +#define MINPRNQZS 0 +#define MAXPRNQZS 0 +#define MINPRNQZS_S 0 +#define MAXPRNQZS_S 0 +#define NSATQZS 0 +#define NSYSQZS 0 #endif - -#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; +#ifdef ENACMP +#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ +#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */ +#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */ +#define NSYSCMP 1 #else -const unsigned int MINPRNBDS = 0; -const unsigned int MAXPRNBDS = 0; -const unsigned int NSATBDS = 0; -const unsigned int NSYSBDS = 0; +#define MINPRNCMP 0 +#define MAXPRNCMP 0 +#define NSATCMP 0 +#define NSYSCMP 0 #endif - #ifdef ENAIRN -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; +#define MINPRNIRN 1 /* min satellite sat number of IRNSS */ +#define MAXPRNIRN 7 /* max satellite sat number of IRNSS */ +#define NSATIRN (MAXPRNIRN-MINPRNIRN+1) /* number of IRNSS satellites */ +#define NSYSIRN 1 #else -const unsigned int MINPRNIRN = 0; -const unsigned int MAXPRNIRN = 0; -const unsigned int NSATIRN = 0; -const unsigned int NSYSIRN = 0; +#define MINPRNIRN 0 +#define MAXPRNIRN 0 +#define NSATIRN 0 +#define NSYSIRN 0 #endif #ifdef ENALEO -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; +#define MINPRNLEO 1 /* min satellite sat number of LEO */ +#define MAXPRNLEO 10 /* max satellite sat number of LEO */ +#define NSATLEO (MAXPRNLEO-MINPRNLEO+1) /* number of LEO satellites */ +#define NSYSLEO 1 #else -const unsigned int MINPRNLEO = 0; -const unsigned int MAXPRNLEO = 0; -const unsigned int NSATLEO = 0; -const unsigned int NSYSLEO = 0; +#define MINPRNLEO 0 +#define MAXPRNLEO 0 +#define NSATLEO 0 +#define NSYSLEO 0 #endif -const unsigned int NSYS = (NSYSGPS + NSYSGLO + NSYSGAL + NSYSQZS + NSYSBDS + NSYSIRN + NSYSLEO); /* number of systems */ +#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP+NSYSIRN+NSYSLEO) /* number of systems */ -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 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 MAXSAT = (NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATBDS + NSATIRN + NSATSBS + NSATLEO); +#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATIRN+NSATSBS+NSATLEO) -const unsigned int MAXSTA = 255; +#define MAXNIGP 201 /* max number of IGP in SBAS band */ + + +#define MAXCODE 55 /* max number of obs code */ + +#define MAXSTA 255 #ifndef MAXOBS -const unsigned int MAXOBS = 64; //!< max number of obs in an epoch */ +#define MAXOBS 64 /* max number of obs in an epoch */ #endif -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 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 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 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 MAXSOLMSG = 8191; //!< max length of solution message */ -const unsigned int MAXERRMSG = 4096; //!< max length of error/warning message */ +#define MAXSOLMSG 8191 /* max length of solution message */ +#define MAXERRMSG 4096 /* max length of error/warning message */ -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 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 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 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 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 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 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 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 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) */ - +#define MAXEXFILE 1024 /* max number of expanded files */ +#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ +#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ +#define MAXSBSURA 8 /* max URA of SBAS satellite */ +#define MAXBAND 10 /* max SBAS band of IGP */ +#define MAXNIGP 201 /* max number of IGP in SBAS band */ typedef void fatalfunc_t(const char *); /* fatal callback function type */ - typedef struct { /* time struct */ time_t time; /* time (s) expressed by standard time_t */ double sec; /* fraction of second under 1 s */ } gtime_t; - typedef struct { /* observation data record */ gtime_t time; /* receiver sampling time (GPST) */ unsigned char sat,rcv; /* satellite/receiver number */ @@ -353,13 +409,11 @@ 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) */ @@ -368,13 +422,11 @@ 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 */ @@ -385,13 +437,11 @@ 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) */ @@ -404,15 +454,14 @@ 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/BDS: data sources */ - int flag; /* GPS/QZS: L2 P data flag, BDS: nav type */ + int code; /* GPS/QZS: code on L2, GAL/CMP: data sources */ + int flag; /* GPS/QZS: L2 P data flag, CMP: nav type */ gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */ /* SV orbit parameters */ double A,e,i0,OMG0,omg,M0,deln,OMGd,idot; @@ -423,11 +472,10 @@ 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 */ - /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ + /* CMP :tgd[0]=BGD1,tgd[1]=BGD2 */ double Adot,ndot; /* Adot,ndot for CNAV */ } eph_t; - typedef struct { /* GLONASS broadcast ephemeris type */ int sat; /* satellite number */ int iode; /* IODE (0-6 bit of tb field) */ @@ -453,7 +501,6 @@ 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 */ @@ -461,7 +508,6 @@ 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) */ @@ -474,7 +520,6 @@ 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 */ @@ -496,13 +541,11 @@ 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} */ @@ -514,27 +557,23 @@ 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) */ @@ -545,7 +584,6 @@ 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) */ @@ -554,14 +592,12 @@ 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 */ @@ -569,7 +605,6 @@ 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) */ @@ -577,7 +612,6 @@ 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) */ @@ -585,14 +619,12 @@ 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) */ @@ -601,7 +633,6 @@ 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) */ @@ -621,7 +652,6 @@ 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 */ @@ -632,13 +662,11 @@ 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) */ @@ -654,7 +682,6 @@ 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) */ @@ -662,7 +689,6 @@ 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 */ @@ -672,14 +698,12 @@ 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 */ @@ -690,7 +714,6 @@ 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 */ @@ -738,7 +761,6 @@ 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 */ @@ -755,7 +777,6 @@ 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) */ @@ -772,7 +793,6 @@ 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 */ @@ -784,7 +804,6 @@ 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 */ @@ -800,13 +819,11 @@ 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 */ @@ -838,7 +855,6 @@ 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 */ @@ -846,7 +862,6 @@ 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) */ @@ -854,7 +869,6 @@ 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) */ @@ -863,13 +877,11 @@ 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) */ @@ -932,7 +944,6 @@ 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_???) */ @@ -955,6 +966,7 @@ typedef struct { /* solution options type */ } solopt_t; + typedef struct { /* satellite status type */ unsigned char sys; /* navigation system */ unsigned char vs; /* valid satellite flag single */ @@ -978,7 +990,6 @@ 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 */ @@ -988,7 +999,6 @@ 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) */ @@ -1004,7 +1014,6 @@ 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) */ @@ -1015,23 +1024,21 @@ 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) */ - 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 +const double lam_carr[MAXFREQ]={ /* carrier wave length (m) */ + CLIGHT/FREQ1,CLIGHT/FREQ2,CLIGHT/FREQ5,CLIGHT/FREQ6,CLIGHT/FREQ7, + CLIGHT/FREQ8,CLIGHT/FREQ9 }; #endif diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index b84c0ea4d..12d3108f2 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -53,162 +53,158 @@ 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 617557a62..4a9fd2374 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.h +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -47,10 +47,8 @@ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ - - -#ifndef GNSS_SDR_RTKLIB_CONVERSIONS_H_ -#define GNSS_SDR_RTKLIB_CONVERSIONS_H_ +#ifndef SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ +#define SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ #include "rtklib_rtkcmn.h" #include "gnss_synchro.h" @@ -64,4 +62,4 @@ eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph); obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week); -#endif /* GNSS_SDR_RTKLIB_CONVERSIONS_H_ */ +#endif /* SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 14befc2a3..02e671fb2 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -47,824 +47,751 @@ * 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 ------------------------------------------------------*/ +/* constants and macros ------------------------------------------------------*/ -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 SQR(x) ((x)*(x)) -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 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 SIN_5 = -0.0871557427476582; /* sin(-5.0 deg) */ -const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */ +#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 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 SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */ +#define COS_5 0.9961946980917456 /* cos(-5.0 deg) */ -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 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 unsigned int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */ +#define DEFURASSR 0.15 /* default accurary of ssr corr (m) */ +#define MAXECORSSR 10.0 /* max orbit correction of ssr (m) */ +#define MAXCCORSSR (1E-6*CLIGHT) /* max clock correction of ssr (m) */ +#define MAXAGESSR 90.0 /* max age of ssr orbit and clock (s) */ +#define MAXAGESSR_HRCLK 10.0 /* max age of ssr high-rate clock (s) */ +#define STD_BRDCCLK 30.0 /* error of broadcast clock (m) */ +#define MAX_ITER_KEPLER 30 /* max number of iteration of Kelpler */ /* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/ double var_uraeph(int ura) { - const double ura_value[] = { - 2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0, - 3072.0, 6144.0 + const double ura_value[]={ + 2.4,3.4,4.85,6.85,9.65,13.65,24.0,48.0,96.0,192.0,384.0,768.0,1536.0, + 3072.0,6144.0 }; - return ura < 0 || 15 < ura ? std::pow(2, 6144.0) : std::pow(2, ura_value[ura]); + return ura<0||15= 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); + if (ura<= 0) return SQR(DEFURASSR); + if (ura>=63) return SQR(5.4665); + std=(pow(3.0,(ura>>3)&7)*(1.0+(ura&7)/4.0)-1.0)*1E-3; + return SQR(std); } - - /* almanac to satellite position and clock bias -------------------------------- - * compute satellite position and clock bias with almanac (gps, galileo, qzss) - * args : gtime_t time I time (gpst) - * alm_t *alm I almanac - * double *rs O satellite position (ecef) {x,y,z} (m) - * double *dts O satellite clock bias (s) - * return : none - * notes : see ref [1],[7],[8] - *-----------------------------------------------------------------------------*/ +* 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 && 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; + + trace(4,"alm2pos : time=%s sat=%2d\n",time_str(time,3),alm->sat); + + tk=timediff(time,alm->toa); + + if (alm->A<=0.0) { + rs[0]=rs[1]=rs[2]=*dts=0.0; + return; + } + mu=satsys(alm->sat,NULL)==SYS_GAL?MU_GAL:MU_GPS; + + M=alm->M0+sqrt(mu/(alm->A*alm->A*alm->A))*tk; + for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-alm->e*cos(E)); + } + if (n>=MAX_ITER_KEPLER) { + trace(2,"alm2pos: kepler iteration overflow sat=%2d\n",alm->sat); + return; + } + sinE=sin(E); cosE=cos(E); + u=atan2(sqrt(1.0-alm->e*alm->e)*sinE,cosE-alm->e)+alm->omg; + r=alm->A*(1.0-alm->e*cosE); + i=alm->i0; + O=alm->OMG0+(alm->OMGd-OMGE)*tk-OMGE*alm->toas; + x=r*cos(u); y=r*sin(u); sinO=sin(O); cosO=cos(O); cosi=cos(i); + rs[0]=x*cosO-y*cosi*sinO; + rs[1]=x*sinO+y*cosi*cosO; + rs[2]=y*sin(i); + *dts=alm->f0+alm->f1*tk; } - - /* broadcast ephemeris to satellite clock bias --------------------------------- - * compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss) - * args : gtime_t time I time by satellite clock (gpst) - * eph_t *eph I broadcast ephemeris - * return : satellite clock bias (s) without relativeity correction - * notes : see ref [1],[7],[8] - * satellite clock does not include relativity correction and tdg - *-----------------------------------------------------------------------------*/ +* compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss) +* args : gtime_t time I time by satellite clock (gpst) +* eph_t *eph I broadcast ephemeris +* return : satellite clock bias (s) without relativeity correction +* notes : see ref [1],[7],[8] +* satellite clock does not include relativity correction and tdg +*-----------------------------------------------------------------------------*/ double eph2clk(gtime_t time, const eph_t *eph) { double t; int i; - - trace(4, "eph2clk : time=%s sat=%2d\n", time_str(time, 3), eph->sat); - - t = timediff(time, eph->toc); - - for (i = 0; i < 2; i++) - { - t -= eph->f0 + eph->f1 * t + eph->f2 * t * t; - } - return eph->f0 + eph->f1 * t + eph->f2 * t *t; -} - - -/* broadcast ephemeris to satellite position and clock bias -------------------- - * compute satellite position and clock bias with broadcast ephemeris (gps, - * galileo, qzss) - * args : gtime_t time I time (gpst) - * eph_t *eph I broadcast ephemeris - * double *rs O satellite position (ecef) {x,y,z} (m) - * double *dts O satellite clock bias (s) - * double *var O satellite position and clock variance (m^2) - * return : none - * notes : see ref [1],[7],[8] - * satellite clock includes relativity correction without code bias - * (tgd or bgd) - *-----------------------------------------------------------------------------*/ -void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, - double *var) -{ - double tk, M, E, Ek, sinE, cosE, u, r, i, O, sin2u, cos2u, x, y, sinO, cosO, cosi, mu, omge; - double xg, yg, zg, sino, coso; - int n, sys, prn; - - trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat); - - if (eph->A <= 0.0) - { - rs[0] = rs[1] = rs[2] = *dts = *var = 0.0; - return; - } - tk = timediff(time , eph->toe); - - switch ((sys = satsys(eph->sat, &prn))) - { - case SYS_GAL: mu = MU_GAL; omge = OMGE_GAL; break; - case SYS_BDS: mu = MU_BDS; omge = OMGE_BDS; break; - default: mu = MU_GPS; omge = DEFAULT_OMEGA_EARTH_DOT; break; + + 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; } - 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_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 / std::pow(2, SPEED_OF_LIGHT); - - /* position and clock error variance */ - *var = var_uraeph(eph->sva); + return eph->f0+eph->f1*t+eph->f2*t*t; +} +/* broadcast ephemeris to satellite position and clock bias -------------------- +* compute satellite position and clock bias with broadcast ephemeris (gps, +* galileo, qzss) +* args : gtime_t time I time (gpst) +* eph_t *eph I broadcast ephemeris +* double *rs O satellite position (ecef) {x,y,z} (m) +* double *dts O satellite clock bias (s) +* double *var O satellite position and clock variance (m^2) +* return : none +* notes : see ref [1],[7],[8] +* satellite clock includes relativity correction without code bias +* (tgd or bgd) +*-----------------------------------------------------------------------------*/ +void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, + double *var) +{ + double tk,M,E,Ek,sinE,cosE,u,r,i,O,sin2u,cos2u,x,y,sinO,cosO,cosi,mu,omge; + double xg,yg,zg,sino,coso; + int n,sys,prn; + + trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat); + + if (eph->A<=0.0) { + rs[0]=rs[1]=rs[2]=*dts=*var=0.0; + return; + } + tk=timediff(time,eph->toe); + + switch ((sys=satsys(eph->sat,&prn))) { + case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break; + case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break; + default: mu=MU_GPS; omge=OMGE; break; + } + M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk; + + for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&ne*sin(E)-M)/(1.0-eph->e*cos(E)); + } + if (n>=MAX_ITER_KEPLER) { + trace(2,"eph2pos: kepler iteration overflow sat=%2d\n",eph->sat); + return; + } + sinE=sin(E); cosE=cos(E); + + trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek); + + u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg; + r=eph->A*(1.0-eph->e*cosE); + i=eph->i0+eph->idot*tk; + sin2u=sin(2.0*u); cos2u=cos(2.0*u); + u+=eph->cus*sin2u+eph->cuc*cos2u; + r+=eph->crs*sin2u+eph->crc*cos2u; + i+=eph->cis*sin2u+eph->cic*cos2u; + x=r*cos(u); y=r*sin(u); cosi=cos(i); + + /* beidou geo satellite (ref [9]) */ + if (sys==SYS_CMP&&prn<=5) { + O=eph->OMG0+eph->OMGd*tk-omge*eph->toes; + sinO=sin(O); cosO=cos(O); + xg=x*cosO-y*cosi*sinO; + yg=x*sinO+y*cosi*cosO; + zg=y*sin(i); + sino=sin(omge*tk); coso=cos(omge*tk); + rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5; + rs[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5; + rs[2]=-yg*SIN_5+zg*COS_5; + } + else { + O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes; + sinO=sin(O); cosO=cos(O); + rs[0]=x*cosO-y*cosi*sinO; + rs[1]=x*sinO+y*cosi*cosO; + rs[2]=y*sin(i); + } + tk=timediff(time,eph->toc); + *dts=eph->f0+eph->f1*tk+eph->f2*tk*tk; + + /* relativity correction */ + *dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT); + + /* position and clock error variance */ + *var=var_uraeph(eph->sva); } - - /* glonass orbit differential equations --------------------------------------*/ void deq(const double *x, double *xdot, const double *acc) { - double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = std::pow(2, 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=SQR(OMGE_GLO); + + if (r2<=0.0) { + xdot[0]=xdot[1]=xdot[2]=xdot[3]=xdot[4]=xdot[5]=0.0; + return; + } /* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */ - a = 1.5 * J2_GLO * MU_GLO * 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]; + a=1.5*J2_GLO*MU_GLO*SQR(RE_GLO)/r2/r3; /* 3/2*J2*mu*Ae^2/r^5 */ + b=5.0*x[2]*x[2]/r2; /* 5*z^2/r^2 */ + c=-MU_GLO/r3-a*(1.0-b); /* -mu/r^3-a(1-b) */ + xdot[0]=x[3]; xdot[1]=x[4]; xdot[2]=x[5]; + xdot[3]=(c+omg2)*x[0]+2.0*OMGE_GLO*x[4]+acc[0]; + xdot[4]=(c+omg2)*x[1]-2.0*OMGE_GLO*x[3]+acc[1]; + xdot[5]=(c-2.0*a)*x[2]+acc[2]; } - - /* glonass position and velocity by numerical integration --------------------*/ void glorbit(double t, double *x, const double *acc) { - double k1[6], k2[6], k3[6], k4[6], w[6]; + 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 = std::pow(2, ERREPH_GLO); + for (i=0;i<3;i++) rs[i]=x[i]; + + *var=SQR(ERREPH_GLO); } - - /* sbas ephemeris to satellite clock bias -------------------------------------- - * compute satellite clock bias with sbas ephemeris - * args : gtime_t time I time by satellite clock (gpst) - * seph_t *seph I sbas ephemeris - * return : satellite clock bias (s) - * notes : see ref [3] - *-----------------------------------------------------------------------------*/ +* 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_BDS: tmax = MAXDTOE_BDS + 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_CMP: tmax=MAXDTOE_CMP+1.0; break; + default: tmax=MAXDTOE+1.0; break; } - tmin = tmax + 1.0; - - for (i = 0; i < nav->n; i++) - { - if (nav->eph[i].sat != sat) continue; - if (iode >= 0 && nav->eph[i].iode != iode) continue; - if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; - if (iode >= 0) return nav->eph + i; - if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */ - } - if (iode >= 0 || j<0) - { - trace(3, "no broadcast ephemeris: %s sat=%2d iode=%3d\n", time_str(time, 0), - sat, iode); - return NULL; - } - return nav->eph + j; + tmin=tmax+1.0; + + for (i=0;in;i++) { + if (nav->eph[i].sat!=sat) continue; + if (iode>=0&&nav->eph[i].iode!=iode) continue; + if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue; + if (iode>=0) return nav->eph+i; + if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ + } + if (iode>=0||j<0) { + trace(3,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0), + sat,iode); + return NULL; + } + return nav->eph+j; } - - /* select glonass ephememeris ------------------------------------------------*/ geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav) { - double t, tmax = MAXDTOE_GLO, tmin = tmax + 1.0; - int i, j = -1; - - trace(4, "selgeph : time=%s sat=%2d iode=%2d\n", time_str(time, 3), sat, iode); - - for (i = 0; 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; + double t,tmax=MAXDTOE_GLO,tmin=tmax+1.0; + int i,j=-1; + + trace(4,"selgeph : time=%s sat=%2d iode=%2d\n",time_str(time,3),sat,iode); + + for (i=0;ing;i++) { + if (nav->geph[i].sat!=sat) continue; + if (iode>=0&&nav->geph[i].iode!=iode) continue; + if ((t=fabs(timediff(nav->geph[i].toe,time)))>tmax) continue; + if (iode>=0) return nav->geph+i; + if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ + } + if (iode>=0||j<0) { + trace(3,"no glonass ephemeris : %s sat=%2d iode=%2d\n",time_str(time,0), + sat,iode); + return NULL; + } + return nav->geph+j; } - - /* select sbas ephememeris ---------------------------------------------------*/ seph_t *selseph(gtime_t time, int sat, const nav_t *nav) { - double t, tmax = MAXDTOE_SBS, tmin = tmax + 1.0; - int i, j = -1; - - trace(4, "selseph : time=%s sat=%2d\n", time_str(time, 3), sat); - - for (i = 0; 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; + double t,tmax=MAXDTOE_SBS,tmin=tmax+1.0; + int i,j=-1; + + trace(4,"selseph : time=%s sat=%2d\n",time_str(time,3),sat); + + for (i=0;ins;i++) { + if (nav->seph[i].sat!=sat) continue; + if ((t=fabs(timediff(nav->seph[i].t0,time)))>tmax) continue; + if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ + } + if (j<0) { + trace(3,"no sbas ephemeris : %s sat=%2d\n",time_str(time,0),sat); + return NULL; + } + return nav->seph+j; } - - /* satellite clock with broadcast ephemeris ----------------------------------*/ int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, - double *dts) + 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_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); - } + + trace(4,"ephclk : time=%s sat=%2d\n",time_str(time,3),sat); + + sys=satsys(sat,NULL); + + if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { + if (!(eph=seleph(teph,sat,-1,nav))) return 0; + *dts=eph2clk(time,eph); + } + else if (sys==SYS_GLO) { + if (!(geph=selgeph(teph,sat,-1,nav))) return 0; + *dts=geph2clk(time,geph); + } + else if (sys==SYS_SBS) { + if (!(seph=selseph(teph,sat,nav))) return 0; + *dts=seph2clk(time,seph); + } else return 0; - + return 1; } - - /* satellite position and clock by broadcast ephemeris -----------------------*/ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, - int iode, double *rs, double *dts, double *var, int *svh) + 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_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; - } + double rst[3],dtst[1],tt=1E-3; + int i,sys; + + trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode); + + sys=satsys(sat,NULL); + + *svh=-1; + + if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { + if (!(eph=seleph(teph,sat,iode,nav))) return 0; + + eph2pos(time,eph,rs,dts,var); + time=timeadd(time,tt); + eph2pos(time,eph,rst,dtst,var); + *svh=eph->svh; + } + else if (sys==SYS_GLO) { + if (!(geph=selgeph(teph,sat,iode,nav))) return 0; + geph2pos(time,geph,rs,dts,var); + time=timeadd(time,tt); + geph2pos(time,geph,rst,dtst,var); + *svh=geph->svh; + } + else if (sys==SYS_SBS) { + if (!(seph=selseph(teph,sat,nav))) return 0; + + seph2pos(time,seph,rs,dts,var); + time=timeadd(time,tt); + seph2pos(time,seph,rst,dtst,var); + *svh=seph->svh; + } else return 0; - + /* satellite velocity and clock drift by differential approx */ - for (i = 0; i < 3; i++) rs[i + 3] = (rst[i] - rs[i]) / tt; - dts[1] = (dtst[0] - dts[0]) / tt; - + 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; 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; - } + for (i=0;isbssat.nsat;i++) { + sbs=nav->sbssat.sat+i; + if (sbs->sat==sat) break; + } + if (i>=nav->sbssat.nsat) { + trace(2,"no sbas correction for orbit: %s sat=%2d\n",time_str(time,0),sat); + ephpos(time,teph,sat,nav,-1,rs,dts,var,svh); + *svh=-1; + return 0; + } /* satellite postion and clock by broadcast ephemeris */ - if (!ephpos(time, teph, sat, nav, sbs->lcorr.iode, rs, dts, var, svh)) return 0; - + 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) < 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; - } + if (ssr->iod[0]==ssr->iod[2]&&ssr->t0[2].time&&fabs(t3)hrclk; + } + if (norm(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) { + trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n", + time_str(time,0),norm(deph,3),dclk); + *svh=-1; + return 0; + } /* satellite postion and clock by broadcast ephemeris */ - if (!ephpos(time, teph, sat, nav, ssr->iode, rs, dts, var, svh)) return 0; - + 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_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; - } + sys=satsys(sat,NULL); + if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) { + if (!(eph=seleph(teph,sat,ssr->iode,nav))) return 0; + + /* satellite clock by clock parameters */ + tk=timediff(time,eph->toc); + dts[0]=eph->f0+eph->f1*tk+eph->f2*tk*tk; + dts[1]=eph->f1+2.0*eph->f2*tk; + + /* relativity correction */ + dts[0]-=2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT; + } /* radial-along-cross directions in ecef */ - if (!normv3(rs + 3, ea)) return 0; - cross3(rs, rs + 3, rc); - if (!normv3(rc, ec)) - { - *svh = -1; - return 0; - } - cross3(ea, ec, er); - + 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) / SPEED_OF_LIGHT) (ref [10] eq.3.12-7) */ - dts[0] += dclk / SPEED_OF_LIGHT; - + if (opt) { + satantoff(time,rs,sat,nav,dant); + } + for (i=0;i<3;i++) { + rs[i]+=-(er[i]*deph[0]+ea[i]*deph[1]+ec[i]*deph[2])+dant[i]; + } + /* t_corr = t_sv - (dts(brdc) + dclk(ssr) / CLIGHT) (ref [10] eq.3.12-7) */ + dts[0]+=dclk/CLIGHT; + /* variance by ssr ura */ - *var = var_urassr(ssr->ura); - - trace(5, "satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n", - time_str(time, 2), sat, deph[0], deph[1], deph[2], er[0], er[1], er[2], dclk, *var); - + *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 < 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); - } + 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; } - 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]); + /* transmission time by satellite clock */ + time[i]=timeadd(obs[i].time,-pr/CLIGHT); + + /* satellite clock bias by broadcast ephemeris */ + if (!ephclk(time[i],teph,obs[i].sat,nav,&dt)) { + trace(3,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat); + continue; } + time[i]=timeadd(time[i],-dt); + + /* satellite position and clock at transmission time */ + if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i, + svh+i)) { + trace(3,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat); + continue; + } + /* if no precise clock available, use broadcast clock instead */ + if (dts[i*2]==0.0) { + if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue; + dts[1+i*2]=0.0; + *var=SQR(STD_BRDCCLK); + } + } + for (i=0;i 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 GNSS_SDR_RTKLIB_EPHEMERIS_H_ -#define GNSS_SDR_RTKLIB_EPHEMERIS_H_ +#ifndef RTKLIB_EPHEMERIS_H_ +#define RTKLIB_EPHEMERIS_H_ #include "rtklib.h" #include "rtklib_rtkcmn.h" @@ -96,4 +122,4 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, -#endif /* GNSS_SDR_RTKLIB_EPHEMERIS_H_ */ +#endif /* RTKLIB_EPHEMERIS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc index 602ea3491..0fd99cb44 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.cc +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -55,536 +55,462 @@ * Maps based on GPS Carrier Phase Data Routinely producted by CODE * Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 * +* version : $Revision:$ $Date:$ +* history : 2011/03/29 1.0 new +* 2013/03/05 1.1 change api readtec() +* fix problem in case of lat>85deg or lat<-85deg +* 2014/02/22 1.2 fix problem on compiled as C++ *-----------------------------------------------------------------------------*/ - #include "rtklib_ionex.h" - /* get index -----------------------------------------------------------------*/ int getindex(double value, const double *range) { - if (range[2] == 0.0) return 0; - if (range[1] > 0.0 && (value < range[0] || range[1]0.0&&(valuent >= nav->ntmax) - { - nav->ntmax+=256; - if (!(nav_tec = (tec_t *)realloc(nav->tec, sizeof(tec_t)*nav->ntmax))) - { - trace(1, "readionex malloc error ntmax=%d\n", nav->ntmax); - free(nav->tec); nav->tec = NULL; nav->nt = nav->ntmax = 0; - return NULL; - } - nav->tec = nav_tec; - } - p = nav->tec+nav->nt; - p->time = time0; - p->rb = rb; - for (i = 0; i < 3; i++) - { - p->ndata[i] = ndata[i]; - p->lats[i] = lats[i]; - p->lons[i] = lons[i]; - p->hgts[i] = hgts[i]; - } - n = ndata[0] * ndata[1] * ndata[2]; - - if (!(p->data = (double *)malloc(sizeof(double) * n)) || - !(p->rms = (float *)malloc(sizeof(float ) * n))) - { + 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; } - for (i = 0; i < n; i++) - { - p->data[i] = 0.0; - p->rms [i] = 0.0f; - } + nav->tec=nav_tec; + } + p=nav->tec+nav->nt; + p->time=time0; + p->rb=rb; + for (i=0;i<3;i++) { + p->ndata[i]=ndata[i]; + p->lats[i]=lats[i]; + p->lons[i]=lons[i]; + p->hgts[i]=hgts[i]; + } + n=ndata[0]*ndata[1]*ndata[2]; + + if (!(p->data=(double *)malloc(sizeof(double)*n))|| + !(p->rms =(float *)malloc(sizeof(float )*n))) { + return NULL; + } + for (i=0;idata[i]=0.0; + p->rms [i]=0.0f; + } nav->nt++; return p; } - - /* read ionex dcb aux data ----------------------------------------------------*/ void readionexdcb(FILE *fp, double *dcb, double *rms) { - int i, sat; - char buff[1024], id[32], *label; - - trace(3, "readionexdcb:\n"); - - for (i = 0; i < MAXSAT; i++) dcb[i] = rms[i] = 0.0; - - while (fgets(buff, sizeof(buff), fp)) - { - if (strlen(buff) < 60) continue; - label = buff + 60; - - if (strstr(label, "PRN / BIAS / RMS") == label) - { - strncpy(id, buff+3, 3); id[3] = '\0'; - - if (!(sat = satid2no(id))) - { - trace(2, "ionex invalid satellite: %s\n", id); - continue; - } - dcb[sat-1] = str2num(buff, 6, 10); - rms[sat-1] = str2num(buff, 16, 10); - } - else if (strstr(label, "END OF AUX DATA") == label) break; + int i,sat; + char buff[1024],id[32],*label; + + trace(3,"readionexdcb:\n"); + + for (i=0;int-1; i >= 0; i--) - { - if (fabs(timediff(time, nav->tec[i].time)) >= 1.0) continue; - p = nav->tec + i; - break; - } - } - else if (p) p->time = time; - } - else if (strstr(label, "LAT/LON1/LON2/DLON/H") == label && p) - { - lat = str2num(buff, 2, 6); - lon[0] = str2num(buff, 8, 6); - lon[1] = str2num(buff, 14, 6); - lon[2] = str2num(buff, 20, 6); - hgt = str2num(buff, 26, 6); - - i = getindex(lat, p->lats); - k = getindex(hgt, p->hgts); - n = nitem(lon); - - for (m = 0; 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)); - } - } + 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;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; 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; - } - } + int i,j,n=0; + + trace(3,"combtec : nav->nt=%d\n",nav->nt); + + for (i=0;int-1;i++) { + for (j=i+1;jnt;j++) { + if (timediff(nav->tec[j].time,nav->tec[i].time)<0.0) { + tmp=nav->tec[i]; + nav->tec[i]=nav->tec[j]; + nav->tec[j]=tmp; + } } - for (i = 0; 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]; + } + for (i=0;int;i++) { + if (i>0&&timediff(nav->tec[i].time,nav->tec[n-1].time)==0.0) { + free(nav->tec[n-1].data); + free(nav->tec[n-1].rms ); + nav->tec[n-1]=nav->tec[i]; + continue; } - nav->nt = n; - - trace(4, "combtec : nav->nt=%d\n", nav->nt); + nav->tec[n++]=nav->tec[i]; + } + nav->nt=n; + + trace(4,"combtec : nav->nt=%d\n",nav->nt); } - - /* read ionex tec grid file ---------------------------------------------------- - * read ionex ionospheric tec grid file - * args : char *file I ionex tec grid file - * (wind-card * is expanded) - * nav_t *nav IO navigation data - * nav->nt, nav->ntmax and nav->tec are modified - * int opt I read option (1: no clear of tec data,0:clear) - * return : none - * notes : see ref [1] - *-----------------------------------------------------------------------------*/ +* 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 < MAXEXFILE; i++) - { - if (!(efiles[i] = (char *)malloc(1024))) - { - for (i--; i >= 0; i--) free(efiles[i]); - return; - } + if (!opt) { + free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0; + } + for (i=0;i=0;i--) free(efiles[i]); + return; } + } /* expand wild card in file path */ - n = expath(file, efiles, MAXEXFILE); - - for (i = 0; i < n; i++) - { - if (!(fp = fopen(efiles[i], "r"))) - { - trace(2, "ionex file open error %s\n", efiles[i]); - continue; - } - /* read ionex header */ - if (readionexh(fp, lats, lons, hgts, &rb, &nexp, dcb, rms) <= 0.0) - { - trace(2, "ionex file format error %s\n", efiles[i]); - continue; - } - /* read ionex body */ - readionexb(fp, lats, lons, hgts, rb, nexp, nav); - - fclose(fp); + n=expath(file,efiles,MAXEXFILE); + + for (i=0;int > 0) combtec(nav); - + if (nav->nt>0) combtec(nav); + /* P1-P2 dcb */ - for (i = 0; i < MAXSAT; i++) - { - nav->cbias[i][0] = SPEED_OF_LIGHT * dcb[i] * 1e-9; /* ns->m */ - } + for (i=0;icbias[i][0]=CLIGHT*dcb[i]*1E-9; /* ns->m */ + } } - - /* interpolate tec grid data -------------------------------------------------*/ int interptec(const tec_t *tec, int k, const double *posp, double *value, - double *rms) + double *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;i < tec->ndata[2]; i++) - { /* for a layer */ - hion = tec->hgts[0] + tec->hgts[2] * i; - - /* ionospheric pierce point position */ - fs = ionppp(pos, azel, tec->rb, hion, posp); - - if (opt&2) - { - /* modified single layer mapping function (M-SLM) ref [2] */ - rp = tec->rb / (tec->rb + hion) * sin(0.9782 * (PI / 2.0 - azel[1])); - fs = 1.0 / sqrt(1.0 - rp * rp); - } - if (opt&1) - { - /* earth rotation correction (sun-fixed coordinate) */ - posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0; - } - /* interpolate tec grid data */ - if (!interptec(tec, i, posp, &vtec, &rms)) return 0; - - *delay += fact * fs * vtec; - *var += fact * fact * fs * fs * rms * rms; + + trace(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(4, "iondelay: delay=%7.2f std=%6.2f\n", *delay, sqrt(*var)); - + if (opt&1) { + /* earth rotation correction (sun-fixed coordinate) */ + posp[1]+=2.0*PI*timediff(time,tec->time)/86400.0; + } + /* interpolate tec grid data */ + if (!interptec(tec,i,posp,&vtec,&rms)) return 0; + + *delay+=fact*fs*vtec; + *var+=fact*fact*fs*fs*rms*rms; + } + trace(4,"iondelay: delay=%7.2f std=%6.2f\n",*delay,sqrt(*var)); + return 1; } - - /* ionosphere model by tec grid data ------------------------------------------- - * compute ionospheric delay by tec grid data - * args : gtime_t time I time (gpst) - * nav_t *nav I navigation data - * double *pos I receiver position {lat,lon,h} (rad,m) - * double *azel I azimuth/elevation angle {az,el} (rad) - * int opt I model option - * bit0: 0:earth-fixed,1:sun-fixed - * bit1: 0:single-layer,1:modified single-layer - * double *delay O ionospheric delay (L1) (m) - * double *var O ionospheric dealy (L1) variance (m^2) - * return : status (1:ok,0:error) - * notes : before calling the function, read tec grid data by calling readtec() - * return ok with delay=0 and var=VAR_NOTEC if elnt; i++) - { - if (timediff(nav->tec[i].time, time) > 0.0) break; - } - if (i == 0 || i >= nav->nt) - { - trace(2, "%s: tec grid out of period\n", time_str(time, 0)); - return 0; - } - if ((tt = timediff(nav->tec[i].time, nav->tec[i-1].time)) == 0.0) - { - trace(2, "tec grid time interval error\n"); - return 0; - } + 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]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 a30ba5b10..155b92ff9 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.h +++ b/src/algorithms/libs/rtklib/rtklib_ionex.h @@ -1,5 +1,5 @@ /*! - * \file rtklib_ionex.h + * \file rtklib_ionex.cc * \brief ionex functions * \authors
    *
  • 2007-2013, T. Takasu @@ -47,24 +47,29 @@ * 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 - * - *-----------------------------------------------------------------------------*/ - -#ifndef GNSS_SDR_RTKLIB_IONEX_H_ -#define GNSS_SDR_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 +* +* version : $Revision:$ $Date:$ +* history : 2011/03/29 1.0 new +* 2013/03/05 1.1 change api readtec() +* fix problem in case of lat>85deg or lat<-85deg +* 2014/02/22 1.2 fix problem on compiled as C++ +*-----------------------------------------------------------------------------*/ +#ifndef RTKLIB_IONEX_H_ +#define RTKLIB_IONEX_H_ #include "rtklib_rtkcmn.h" -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) */ +#define SQR(x) ((x)*(x)) +#define VAR_NOTEC SQR(30.0) /* variance of no tec */ +#define MIN_EL 0.0 /* min elevation angle (rad) */ +#define MIN_HGT -1000.0 /* min user height (m) */ int getindex(double value, const double *range); @@ -87,4 +92,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 /* GNSS_SDR_RTKLIB_IONEX_H_ */ +#endif /* SRC_ALGORITHMS_PVT_LIBS_RTKLIB_IONEX_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 7d47085ed..1e45c07bc 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -47,9 +47,19 @@ * 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" @@ -57,732 +67,638 @@ 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 = 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; + fact=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); + varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el)); + if (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */ + return SQR(fact)*varr; } - - /* get tgd parameter (m) -----------------------------------------------------*/ double gettgd(int sat, const nav_t *nav) { int i; - for (i = 0; i < nav->n; i++) - { - if (nav->eph[i].sat != sat) continue; - return SPEED_OF_LIGHT * nav->eph[i].tgd[0]; - } + for (i=0;in;i++) { + if (nav->eph[i].sat!=sat) continue; + return CLIGHT*nav->eph[i].tgd[0]; + } return 0.0; } - - /* psendorange with code bias correction -------------------------------------*/ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, - int iter, const prcopt_t *opt, double *var) + 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; + + *var=0.0; + + if (!(sys=satsys(obs->sat,NULL))) + { + trace(4,"prange: satsys NULL\n"); + return 0.0; + } + /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if (NFREQ>=3&&(sys&(SYS_GAL|SYS_SBS))) j = 2; - - if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) + 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]); + 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)) + if (iter>0) { + if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) { + trace(4,"snr mask: %s sat=%2d el=%.1f snr=%.1f\n", + time_str(obs->time,0),obs->sat,azel[1]*R2D,obs->SNR[i]*0.25); + return 0.0; + } + if (opt->ionoopt==IONOOPT_IFLC) { + if (testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) { - trace(4, "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); + trace(4,"prange: testsnr error\n"); 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 = 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]; - + } + gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */ + P1=obs->P[i]; + P2=obs->P[j]; + P1_P2=nav->cbias[obs->sat-1][0]; + P1_C1=nav->cbias[obs->sat-1][1]; + P2_C2=nav->cbias[obs->sat-1][2]; + /* if no P1-P2 DCB, use TGD instead */ - if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) - { - P1_P2 = (1.0 - gamma) * gettgd(obs->sat, nav); - } - if (opt->ionoopt == IONOOPT_IFLC) - { /* dual-frequency */ - - if (P1 == 0.0 || P2 == 0.0) return 0.0; - if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ - if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ - - /* iono-free combination */ - PC = (gamma * P1 - P2) / (gamma - 1.0); - } - else - { /* single-frequency */ - - if (P1 == 0.0) return 0.0; - if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ - PC = P1 - P1_P2 / (1.0 - gamma); - } - if (opt->sateph == EPHOPT_SBAS) PC -= P1_C1; /* sbas clock based C1 */ - - *var = std::pow(2, 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=SQR(ERR_CBIAS); + return PC; } - - /* ionospheric correction ------------------------------------------------------ - * compute ionospheric correction - * args : gtime_t time I time - * nav_t *nav I navigation data - * int sat I satellite number - * double *pos I receiver position {lat,lon,h} (rad|m) - * double *azel I azimuth/elevation angle {az,el} (rad) - * int ionoopt I ionospheric correction option (IONOOPT_???) - * double *ion O ionospheric delay (L1) (m) - * double *var O ionospheric delay (L1) variance (m^2) - * return : status(1:ok,0:error) - *-----------------------------------------------------------------------------*/ +* 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 = std::pow(2, *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=SQR(*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 ? std::pow(2, ERR_ION) : 0.0; + *ion=0.0; + *var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0; return 1; } - - /* tropospheric correction ----------------------------------------------------- - * compute tropospheric correction - * args : gtime_t time I time - * nav_t *nav I navigation data - * double *pos I receiver position {lat,lon,h} (rad|m) - * double *azel I azimuth/elevation angle {az,el} (rad) - * int tropopt I tropospheric correction option (TROPOPT_???) - * double *trp O tropospheric delay (m) - * double *var O tropospheric delay variance (m^2) - * return : status(1:ok,0:error) - *-----------------------------------------------------------------------------*/ +* 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 ? std::pow(2, ERR_TROP) : 0.0; + *trp=0.0; + *var=tropopt==TROPOPT_OFF?SQR(ERR_TROP):0.0; return 1; } - - /* pseudorange residuals -----------------------------------------------------*/ int rescode(int iter, const obsd_t *obs, int n, const double *rs, - const double *dts, const double *vare, const int *svh, - const nav_t *nav, const double *x, const prcopt_t *opt, - double *v, double *H, double *var, double *azel, int *vsat, - double *resp, int *ns) + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, const prcopt_t *opt, + double *v, double *H, double *var, double *azel, int *vsat, + double *resp, int *ns) { - double r, dion, dtrp, vmeas, vion, vtrp, rr[3], pos[3], dtr, e[3], P, lam_L1; - int i, j, nv = 0, sys, mask[4] = {0}; - - trace(3, "resprng : n=%d\n", n); - - for (i = 0; i < 3; i++) rr[i] = x[i]; - dtr = x[3]; - - ecef2pos(rr, pos); - - for (i = *ns = 0; i < n && i < MAXOBS; i++) - { - vsat[i] = 0; - azel[i*2] = azel[1+i*2] = resp[i] = 0.0; - - if (!(sys = satsys(obs[i].sat, NULL))) continue; - - /* reject duplicated observation data */ - if (i < n - 1 && i < MAXOBS - 1 && obs[i].sat == obs[i+1].sat) - { - trace(2, "duplicated observation data %s sat=%2d\n", - time_str(obs[i].time, 3), obs[i].sat); - i++; - continue; - } - /* geometric distance/azimuth/elevation angle */ - if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || satazel(pos, e, azel + i * 2) < opt->elmin) - { - trace(4, "geodist / satazel error\n"); - continue; - } - - /* psudorange with code bias correction */ - if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0) - { - trace(4, "prange error\n"); - continue; - } - - /* excluded satellite? */ - if (satexclude(obs[i].sat, svh[i], opt)) - { - trace(4, "satexclude error\n"); - continue; - } - - /* ionospheric corrections */ - if (!ionocorr(obs[i].time, nav, obs[i].sat, pos, azel+i*2, - iter>0 ? opt->ionoopt : IONOOPT_BRDC, &dion, &vion)) - { - trace(4, "ionocorr error\n"); - continue; - } - - /* GPS-L1 -> L1/B1 */ - if ((lam_L1 = nav->lam[obs[i].sat - 1][0]) > 0.0) - { - dion *= 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])); + double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1; + int i,j,nv=0,sys,mask[4]={0}; + + trace(3,"resprng : n=%d\n",n); + + for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3]; + + ecef2pos(rr,pos); + + for (i=*ns=0;ielmin) + { + trace(4,"geodist / satazel error\n"); + continue; + } + + /* psudorange with code bias correction */ + if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) + { + trace(4,"prange error\n"); + continue; + } + + /* excluded satellite? */ + if (satexclude(obs[i].sat,svh[i],opt)) + { + trace(4,"satexclude error\n"); + continue; + } + + /* ionospheric corrections */ + if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2, + iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) + { + trace(4,"ionocorr error\n"); + continue; + } + + /* GPS-L1 -> L1/B1 */ + if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) { + dion*=SQR(lam_L1/lam_carr[0]); + } + /* tropospheric corrections */ + if (!tropcorr(obs[i].time,nav,pos,azel+i*2, + iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) { + trace(4,"tropocorr error\n"); + continue; + } + /* pseudorange residual */ + v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp); + + /* design matrix */ + for (j=0;j 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; - } + vv=dot(v,v,nv); + if (nv>nx&&vv>chisqr[nv-nx-1]) { + sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]); + return 0; + } /* large gdop check */ - for (i = ns = 0; i < n; i++) - { - if (!vsat[i]) continue; - azels[ ns*2] = azel[ i*2]; - azels[1+ns*2] = azel[1+i*2]; - ns++; - } - dops(ns, azels, opt->elmin, dop); - if (dop[0] <= 0.0 || dop[0] > opt->maxgdop) - { - sprintf(msg, "gdop error nv=%d gdop=%.1f", nv, dop[0]); - return 0; - } + for (i=ns=0;ielmin,dop); + if (dop[0]<=0.0||dop[0]>opt->maxgdop) { + sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]); + return 0; + } return 1; } - - /* estimate receiver position ------------------------------------------------*/ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, - const double *vare, const int *svh, const nav_t *nav, - const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, - double *resp, char *msg) + const double *vare, const int *svh, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, + double *resp, char *msg) { - double x[NX] = {0}, dx[NX], Q[NX*NX], *v, *H, *var, sig; - int i, j, k, info, stat, nv, ns; - - trace(3, "estpos : n=%d\n", n); - - v = mat(n + 4, 1); - H = mat(NX, n + 4); - var = mat(n + 4, 1); - - for (i = 0; i < 3; i++) x[i] = sol->rr[i]; - - for (i = 0; i < MAXITR; i++) - { - /* pseudorange residuals */ - nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, &ns); - - if (nv < NX) - { - sprintf(msg, "lack of valid sats ns=%d", nv); - break; - } - /* weight by variance */ - for (j = 0;j < nv; j++) - { - sig = sqrt(var[j]); - v[j] /= sig; - for (k = 0; k < NX; k++) H[k + j * NX] /= sig; - } - /* least square estimation */ - if ((info = lsq(H, v, NX, nv, dx, Q))) - { - sprintf(msg, "lsq error info=%d", info); - break; - } - for (j = 0; j < NX; j++) x[j] += dx[j]; - - if (norm(dx, NX) < 1e-4) - { - sol->type = 0; - sol->time = timeadd(obs[0].time, -x[3] / 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; - } + double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig; + int i,j,k,info,stat,nv,ns; + + trace(3,"estpos : n=%d\n",n); + + v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); + + for (i=0;i<3;i++) x[i]=sol->rr[i]; + + for (i=0;i= MAXITR) sprintf(msg, "iteration divergent i=%d", i); - - free(v); - free(H); - free(var); - + /* 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); + 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 < n; i++) - { - /* satellite exclution */ - for (j = k = 0; j < n; j++) - { - if (j == i) continue; - obs_e[k] = obs[j]; - matcpy(rs_e + 6 * k, rs + 6 * j, 6, 1); - matcpy(dts_e + 2 * k, dts + 2 * j, 2, 1); - vare_e[k] = vare[j]; - svh_e[k++] = svh[j]; - } - /* estimate receiver position without a satellite */ - if (!estpos(obs_e, n-1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, - vsat_e, resp_e, msg_e)) - { - trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); - continue; - } - for (j = nvsat = 0, rms_e = 0.0; j < n - 1; j++) - { - if (!vsat_e[j]) continue; - rms_e += std::pow(2, resp_e[j]); - nvsat++; - } - if (nvsat < 5) - { - trace(3, "raim_fde: exsat=%2d lack of satellites nvsat=%2d\n", - obs[i].sat, nvsat); - continue; - } - rms_e = sqrt(rms_e / nvsat); - - trace(3, "raim_fde: exsat=%2d rms=%8.3f\n", obs[i].sat, rms_e); - - if (rms_e > rms) continue; - - /* save result */ - for (j = k = 0; j < n; j++) - { - if (j == i) continue; - matcpy(azel + 2 * j, azel_e + 2 * k, 2, 1); - vsat[j] = vsat_e[k]; - resp[j] = resp_e[k++]; - } - stat = 1; - *sol = sol_e; - sat = obs[i].sat; - rms = rms_e; - vsat[i] = 0; - strcpy(msg, msg_e); + sol_t sol_e={}; + char tstr[32],name[16],msg_e[128]; + double *rs_e,*dts_e,*vare_e,*azel_e,*resp_e,rms_e,rms=100.0; + int i,j,k,nvsat,stat=0,*svh_e,*vsat_e,sat=0; + + trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n); + + if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0; + rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n); + svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n); + + for (i=0;irms) continue; + + /* save result */ + for (j=k=0;jlam[obs[i].sat-1][0]; - - if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm(rs + 3 + i * 6, 3) <= 0.0) - { - continue; - } - /* line-of-sight vector in ecef */ - cosel = cos(azel[1+i*2]); - a[0] = sin(azel[i*2]) * cosel; - a[1] = cos(azel[i*2]) * cosel; - a[2] = sin(azel[1+i*2]); - matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e); - - /* satellite velocity relative to receiver in ecef */ - for (j = 0; j < 3; j++) vs[j] = rs[j+3+i*6] - x[j]; - - /* range rate with earth rotation correction */ - rate = dot(vs, e, 3) + 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++; + 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;ilam[obs[i].sat-1][0]; + + if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) { + continue; } + /* line-of-sight vector in ecef */ + cosel=cos(azel[1+i*2]); + a[0]=sin(azel[i*2])*cosel; + a[1]=cos(azel[i*2])*cosel; + a[2]=sin(azel[1+i*2]); + matmul("TN",3,1,3,1.0,E,a,0.0,e); + + /* satellite velocity relative to receiver in ecef */ + for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j]; + + /* range rate with earth rotation correction */ + rate=dot(vs,e,3)+OMGE/CLIGHT*(rs[4+i*6]*rr[0]+rs[1+i*6]*x[0]- + rs[3+i*6]*rr[1]-rs[ i*6]*x[1]); + + /* doppler residual */ + v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]); + + /* design matrix */ + for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0; + + nv++; + } return nv; } - - /* estimate receiver velocity ------------------------------------------------*/ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, - const nav_t *nav, const prcopt_t *opt, sol_t *sol, - const double *azel, const int *vsat) + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + const double *azel, const int *vsat) { - double x[4] = {0}, dx[4], Q[16], *v, *H; - int i, j, nv; - - trace(3, "estvel : n=%d\n", n); - - v = mat(n, 1); - H = mat(4, n); - - for (i = 0; i < MAXITR; i++) - { - /* doppler residuals */ - if ((nv = resdop(obs, n, rs, dts, nav, sol->rr, x, azel, vsat, v, H)) < 4) - { - break; - } - /* least square estimation */ - if (lsq(H, v, 4, nv, dx, Q)) break; - - for (j = 0; j < 4; j++) x[j] += dx[j]; - - if (norm(dx, 4) < 1e-6) - { - for (i = 0; i < 3; i++) sol->rr[i+3] = x[i]; - break; - } + 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; } - free(v); - free(H); + /* 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) - *-----------------------------------------------------------------------------*/ +* 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) + 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); - // } +// 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 */ + 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; i < n * 2; i++) azel[i] = azel_[i]; + 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; - } - nav->peph = nav_peph; + + if (nav->ne>=nav->nemax) { + nav->nemax+=256; + if (!(nav_peph=(peph_t *)realloc(nav->peph,sizeof(peph_t)*nav->nemax))) { + trace(1,"readsp3b malloc error n=%d\n",nav->nemax); + free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0; + return 0; } - nav->peph[nav->ne++] = *peph; + nav->peph=nav_peph; + } + nav->peph[nav->ne++]=*peph; return 1; } - - /* read sp3 body -------------------------------------------------------------*/ void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, - char *tsys, int index, int opt, nav_t *nav) + 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 < 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; - } + + trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt); + + while (fgets(buff,sizeof(buff),fp)) { + + if (!strncmp(buff,"EOF",3)) break; + + if (buff[0]!='*'||str2time(buff,3,28,&time)) { + trace(2,"sp3 invalid epoch %31.31s\n",buff); + continue; } + if (!strcmp(tsys,"UTC")) time=utc2gpst(time); /* utc->gpst */ + peph.time =time; + peph.index=index; + + for (i=0;i=76&&buff[75]=='P'; + pred_o=strlen(buff)>=80&&buff[79]=='P'; + } + for (j=0;j<4;j++) { + + /* read option for predicted value */ + if (j< 3&&(opt&1)&& pred_o) continue; + if (j< 3&&(opt&2)&&!pred_o) continue; + if (j==3&&(opt&1)&& pred_c) continue; + if (j==3&&(opt&2)&&!pred_c) continue; + + val=str2num(buff, 4+j*14,14); + std=str2num(buff,61+j* 3,j<3?2:3); + + if (buff[0]=='P') { /* position */ + if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { + peph.pos[sat-1][j]=val*(j<3?1000.0:1E-6); + v=1; /* valid epoch */ + } + if ((base=bfact[j<3?0:1])>0.0&&std>0.0) { + peph.std[sat-1][j]=(float)(pow(base,std)*(j<3?1E-3:1E-12)); + } + } + else if (v) { /* velocity */ + if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { + peph.vel[sat-1][j]=val*(j<3?0.1:1E-10); + } + if ((base=bfact[j<3?0:1])>0.0&&std>0.0) { + peph.vst[sat-1][j]=(float)(pow(base,std)*(j<3?1E-7:1E-16)); + } + } + } + } + if (v) { + if (!addpeph(nav,&peph)) return; + } + } } - - /* compare precise ephemeris -------------------------------------------------*/ -int cmppeph(const void *p1, const void *p2) +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; 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]; + + 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]; + } } - nav->ne = i+1; - - trace(4, "combpeph: ne=%d\n", nav->ne); + else if (++ipeph[i]=nav->peph[j]; + } + nav->ne=i+1; + + trace(4,"combpeph: ne=%d\n",nav->ne); } - - /* read sp3 precise ephemeris file --------------------------------------------- - * read sp3 precise ephemeris/clock files and set them to navigation data - * args : char *file I sp3-c precise ephemeris file - * (wind-card * is expanded) - * nav_t *nav IO navigation data - * int opt I options (1: only observed + 2: only predicted + - * 4: not combined) - * return : none - * notes : see ref [1] - * precise ephemeris is appended and combined - * nav->peph and nav->ne must by properly initialized before calling the - * function - * only files with extensions of .sp3, .SP3, .eph* and .EPH* are read - *-----------------------------------------------------------------------------*/ +* read sp3 precise ephemeris/clock files and set them to navigation data +* args : char *file I sp3-c precise ephemeris file +* (wind-card * is expanded) +* nav_t *nav IO navigation data +* int opt I options (1: only observed + 2: only predicted + +* 4: not combined) +* return : none +* notes : see ref [1] +* precise ephemeris is appended and combined +* nav->peph and nav->ne must by properly initialized before calling the +* function +* only files with extensions of .sp3, .SP3, .eph* and .EPH* are read +*-----------------------------------------------------------------------------*/ void readsp3(const char *file, nav_t *nav, int opt) { FILE *fp; - gtime_t time = {}; - double bfact[2] = {}; - int i, j, n, ns, sats[MAXSAT] = {}; - char *efiles[MAXEXFILE], *ext, type = ' ', tsys[4] = ""; - - trace(3, "readpephs: file=%s\n", file); - - for (i = 0; i < MAXEXFILE; i++) - { - if (!(efiles[i] = (char *)malloc(1024))) - { - for (i--; i >= 0; i--) free(efiles[i]); - return; - } + gtime_t time={}; + double bfact[2]={}; + int i,j,n,ns,sats[MAXSAT]={}; + char *efiles[MAXEXFILE],*ext,type=' ',tsys[4]=""; + + trace(3,"readpephs: file=%s\n",file); + + for (i=0;i=0;i--) free(efiles[i]); + return; } + } /* expand wild card in file path */ - n = expath(file, efiles, MAXEXFILE); - - for (i = j = 0; i < n; i++) - { - if (!(ext = strrchr(efiles[i], '.'))) continue; - - if (!strstr(ext + 1, "sp3") && !strstr(ext + 1, ".SP3") && - !strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH")) continue; - - if (!(fp = fopen(efiles[i], "r"))) - { - trace(2, "sp3 file open error %s\n", efiles[i]); - continue; - } - /* read sp3 header */ - ns = readsp3h(fp, &time, &type, sats, bfact, tsys); - - /* read sp3 body */ - readsp3b(fp, type, sats, ns, bfact, tsys, j++, opt, nav); - - fclose(fp); + 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; i < MAXSAT; i++) - { - pcv = searchpcv(i + 1, "", time, &pcvs); - nav->pcvs[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;ipcvs[i]=pcv?*pcv:pcv0; + } free(pcvs.pcv); return 1; } - - /* read dcb parameters file --------------------------------------------------*/ int readdcbf(const char *file, nav_t *nav, const sta_t *sta) { FILE *fp; double cbias; - char buff[256], str1[32], str2[32] = ""; - int i, j, sat, type = 0; - - trace(3, "readdcbf: file=%s\n", file); - - if (!(fp = fopen(file, "r"))) - { - trace(2, "dcb parameters file open error: %s\n", file); - return 0; + 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 */ + } } - 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 */ - } + else if ((sat=satid2no(str1))) { /* satellite dcb */ + nav->cbias[sat-1][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ } + } fclose(fp); - + return 1; } - - /* read dcb parameters --------------------------------------------------------- - * read differential code bias (dcb) parameters - * args : char *file I dcb parameters file (wild-card * expanded) - * nav_t *nav IO navigation data - * sta_t *sta I station info data to inport receiver dcb - * (NULL: no use) - * return : status (1:ok,0:error) - * notes : currently only p1-c1 bias of code *.dcb file - *-----------------------------------------------------------------------------*/ +* read differential code bias (dcb) parameters +* args : char *file I dcb parameters file (wild-card * expanded) +* nav_t *nav IO navigation data +* sta_t *sta I station info data to inport receiver dcb +* (NULL: no use) +* return : status (1:ok,0:error) +* notes : currently only p1-c1 bias of code *.dcb file +*-----------------------------------------------------------------------------*/ int readdcb(const char *file, nav_t *nav, const sta_t *sta) { - int i, j, n; - char *efiles[MAXEXFILE] = {}; - - trace(3, "readdcb : file=%s\n", file); - - for (i = 0;i < MAXSAT; i++) for (j = 0; j < 3; j++) - { - nav->cbias[i][j] = 0.0; + 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; } - for (i = 0; i < MAXEXFILE; i++) - { - if (!(efiles[i] = (char *)malloc(1024))) - { - for (i--; i >= 0; i--) free(efiles[i]); - return 0; - } - } - n = expath(file, efiles, MAXEXFILE); - - for (i = 0; i < n; i++) - { - readdcbf(efiles[i], nav, sta); - } - for (i = 0; i < MAXEXFILE;i ++) free(efiles[i]); - + } + n=expath(file,efiles,MAXEXFILE); + + for (i=0;inf > 0 && fabs(timediff(ts, nav->fcb[nav->nf-1].ts)) <= 1e-3) - { - for (i = 0; i < 3; i++) - { - nav->fcb[nav->nf-1].bias[sat-1][i] = bias[i]; - nav->fcb[nav->nf-1].std [sat-1][i] = std [i]; - } - return 1; + 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]; } - 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; + 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; } - 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; + nav->fcb=nav_fcb; + } + for (i=0;ifcb[nav->nf].bias[i][j]=nav->fcb[nav->nf].std[i][j]=0.0; + } + for (i=0;i<3;i++) { + nav->fcb[nav->nf].bias[sat-1][i]=bias[i]; + nav->fcb[nav->nf].std [sat-1][i]=std [i]; + } + nav->fcb[nav->nf ].ts=ts; + nav->fcb[nav->nf++].te=te; return 1; } - - /* read satellite fcb file ---------------------------------------------------*/ int readfcbf(const char *file, nav_t *nav) { FILE *fp; - gtime_t ts, te; - double ep1[6], ep2[6], bias[3] = {}, std[3] = {}; - char buff[1024], str[32], *p; + 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 < 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); + char *efiles[MAXEXFILE]={}; + int i,n; + + trace(3,"readfcb : file=%s\n",file); + + for (i=0;i=0;i--) free(efiles[i]); + return 0; } + } + n=expath(file,efiles,MAXEXFILE); + + for (i=0;inf>1) { + qsort(nav->fcb,nav->nf,sizeof(fcbd_t),cmpfcb); + } return 1; } - - /* polynomial interpolation by Neville's algorithm ---------------------------*/ double interppol(const double *x, double *y, int n) { - int i, j; - - for (j = 1; j < n; j++) - { - for (i = 0; i < n - j; i++) - { - y[i] = (x[i+j] * y[i] - x[i] * y[i+1]) / (x[i+j] - x[i]); - } + int i,j; + + for (j=1;jne < 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); + 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->nepeph[0].time)<-MAXDTE|| + timediff(time,nav->peph[nav->ne-1].time)>MAXDTE) { + trace(3,"no prec ephem %s sat=%2d\n",time_str(time,0),sat); + return 0; + } + /* binary search */ + for (i=0,j=nav->ne-1;ipeph[k].time,time)<0.0) i=k+1; else j=k; + } + index=i<=0?0:i-1; + + /* polynomial interpolation for orbit */ + i=index-(NMAX+1)/2; + if (i<0) i=0; else if (i+NMAX>=nav->ne) i=nav->ne-NMAX-1; + + for (j=0;j<=NMAX;j++) { + t[j]=timediff(nav->peph[i+j].time,time); + if (norm(nav->peph[i+j].pos[sat-1],3)<=0.0) { + trace(3,"prec ephem outage %s sat=%2d\n",time_str(time,0),sat); return 0; } - /* 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]; + } + 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(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]; + /* correciton for earh rotation ver.2.4.0 */ + sinl=sin(OMGE*t[j]); + cosl=cos(OMGE*t[j]); + p[0][j]=cosl*pos[0]-sinl*pos[1]; + p[1][j]=sinl*pos[0]+cosl*pos[1]; #endif - p[2][j] = pos[2]; - } - for (i = 0; i < 3; i++) - { - rs[i] = interppol(t, p[i], NMAX + 1); - } - if (vare) - { - for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat-1][i]; - std = norm(s, 3); - - /* extrapolation error for orbit */ - if (t[0 ] > 0.0) std += EXTERR_EPH * 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); - } + p[2][j]=pos[2]; + } + for (i=0;i<3;i++) { + rs[i]=interppol(t,p[i],NMAX+1); + } + if (vare) { + for (i=0;i<3;i++) s[i]=nav->peph[index].std[sat-1][i]; + std=norm(s,3); + + /* extrapolation error for orbit */ + if (t[0 ]>0.0) std+=EXTERR_EPH*SQR(t[0 ])/2.0; + else if (t[NMAX]<0.0) std+=EXTERR_EPH*SQR(t[NMAX])/2.0; + *vare=SQR(std); + } /* linear interpolation for clock */ - t[0] = timediff(time, nav->peph[index ].time); - t[1] = timediff(time, nav->peph[index+1].time); - c[0] = nav->peph[index ].pos[sat-1][3]; - c[1] = nav->peph[index+1].pos[sat-1][3]; - - if (t[0] <= 0.0) - { - if ((dts[0] = c[0]) != 0.0) - { - std = nav->peph[index].std[sat-1][3] * SPEED_OF_LIGHT - 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]*CLIGHT-EXTERR_CLK*t[0]; } - else if (t[1] >= 0.0) - { - if ((dts[0] = c[1]) != 0.0) - { - std = nav->peph[index+1].std[sat-1][3] * SPEED_OF_LIGHT + 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]*CLIGHT+EXTERR_CLK*t[1]; } - else if (c[0] != 0.0 && c[1] != 0.0) - { - dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]); - i = t[0] < -t[1] ? 0: 1; - std = nav->peph[index+i].std[sat-1][3] + EXTERR_CLK * fabs(t[i]); - } - else - { - dts[0] = 0.0; - } - if (varc) *varc = std::pow(2, 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=SQR(std); return 1; } - - /* satellite clock by precise clock ------------------------------------------*/ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, - double *varc) + double *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; 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; - + for (i=0,j=nav->nc-1;ipclk[k].time,time)<0.0) i=k+1; else j=k; + } + index=i<=0?0:i-1; + /* linear interpolation for clock */ - t[0] = timediff(time, nav->pclk[index ].time); - t[1] = timediff(time, nav->pclk[index+1].time); - c[0] = nav->pclk[index ].clk[sat-1][0]; - c[1] = nav->pclk[index+1].clk[sat-1][0]; - - if (t[0] <= 0.0) - { - if ((dts[0] = c[0]) == 0.0) return 0; - std = nav->pclk[index].std[sat-1][0] * 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); + t[0]=timediff(time,nav->pclk[index ].time); + t[1]=timediff(time,nav->pclk[index+1].time); + c[0]=nav->pclk[index ].clk[sat-1][0]; + c[1]=nav->pclk[index+1].clk[sat-1][0]; + + if (t[0]<=0.0) { + if ((dts[0]=c[0])==0.0) return 0; + std=nav->pclk[index].std[sat-1][0]*CLIGHT-EXTERR_CLK*t[0]; + } + else if (t[1]>=0.0) { + if ((dts[0]=c[1])==0.0) return 0; + std=nav->pclk[index+1].std[sat-1][0]*CLIGHT+EXTERR_CLK*t[1]; + } + else if (c[0]!=0.0&&c[1]!=0.0) { + dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); + i=t[0]<-t[1]?0:1; + std=nav->pclk[index+i].std[sat-1][0]*CLIGHT+EXTERR_CLK*fabs(t[i]); + } + else { + trace(3,"prec clock outage %s sat=%2d\n",time_str(time,0),sat); + return 0; + } + if (varc) *varc=SQR(std); return 1; } - - /* satellite antenna phase center offset --------------------------------------- - * compute satellite antenna phase center offset in ecef - * args : gtime_t time I time (gpst) - * double *rs I satellite position and velocity (ecef) - * {x,y,z,vx,vy,vz} (m|m/s) - * int sat I satellite number - * nav_t *nav I navigation data - * double *dant I satellite antenna phase center offset (ecef) - * {dx,dy,dz} (m) (iono-free LC value) - * return : none - *-----------------------------------------------------------------------------*/ +* 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 = std::pow(2, lam[k]) / std::pow(2, 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=SQR(lam[k])/SQR(lam[j]); + C1=gamma/(gamma-1.0); + C2=-1.0 /(gamma-1.0); + /* iono-free LC */ - for (i = 0; i < 3; i++) - { - dant1 = pcv->off[j][0] * ex[i] + pcv->off[j][1] * ey[i] + pcv->off[j][2] * ez[i]; - dant2 = pcv->off[k][0] * ex[i] + pcv->off[k][1] * ey[i] + pcv->off[k][2] * ez[i]; - dant[i] = C1 * dant1 + C2 * dant2; - } + 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 || MAXSAT < sat) return 0; - + + 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; @@ -998,79 +902,72 @@ 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); @@ -1083,22 +980,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) @@ -1276,22 +1154,20 @@ 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]); @@ -1299,346 +1175,305 @@ 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) { @@ -1649,85 +1484,75 @@ 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; @@ -1735,58 +1560,52 @@ 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); @@ -1794,151 +1613,132 @@ 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); @@ -1949,164 +1749,158 @@ 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}; @@ -2116,25 +1910,24 @@ 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; @@ -2143,70 +1936,61 @@ 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; - } - pcvs->pcv=pcvs_pcv; + + 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->n++]=*pcv; } - - /* read ngs antenna parameter file -------------------------------------------*/ int readngspcv(const char *file, pcvs_t *pcvs) { @@ -2216,51 +2000,44 @@ 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; + + 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'; } - 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==2) { + if (decodef(buff,3,neu)<3) continue; + pcv.off[0][0]=neu[1]; + pcv.off[0][1]=neu[0]; + pcv.off[0][2]=neu[2]; } + else if (n==3) decodef(buff,10,pcv.var[0]); + else if (n==4) decodef(buff,9,pcv.var[0]+10); + else if (n==5) { + if (decodef(buff,3,neu)<3) continue;; + pcv.off[1][0]=neu[1]; + pcv.off[1][1]=neu[0]; + pcv.off[1][2]=neu[2]; + } + else if (n==6) decodef(buff,10,pcv.var[1]); + else if (n==7) { + decodef(buff,9,pcv.var[1]+10); + addpcv(&pcv,pcvs); + } + } fclose(fp); - + return 1; } - - /* read antex file ----------------------------------------------------------*/ int readantex(const char *file, pcvs_t *pcvs) { @@ -2270,183 +2047,159 @@ 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; + + 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; } - 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; - } - } - 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; - } + for (i=0;in;i++) { + pcv=pcvs->pcv+i; + if (pcv->sat!=sat) continue; + if (pcv->ts.time!=0&&timediff(pcv->ts,time)>0.0) continue; + if (pcv->te.time!=0&&timediff(pcv->te,time)<0.0) continue; + return pcv; } + } + else { + strcpy(buff,type); + for (p=strtok(buff," ");p&&n<2;p=strtok(NULL," ")) types[n++]=p; + if (n<=0) return NULL; + + /* search receiver antenna with radome at first */ + for (i=0;in;i++) { + pcv=pcvs->pcv+i; + for (j=0;jtype,types[j])) break; + if (j>=n) return pcv; + } + /* search receiver antenna without radome */ + for (i=0;in;i++) { + pcv=pcvs->pcv+i; + if (strstr(pcv->type,types[0])!=pcv->type) continue; + + trace(2,"pcv without radome is used type=%s\n",type); + return pcv; + } + } return NULL; } - - /* read station positions ------------------------------------------------------ - * read positions from station position file - * args : char *file I station position file containing - * lat(deg) lon(deg) height(m) name in a line - * char *rcvs I station name - * double *pos O station position {lat,lon,h} (rad/m) - * (all 0 if search error) - * return : none - *-----------------------------------------------------------------------------*/ +* 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]; @@ -2454,364 +2207,314 @@ 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; } - 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_data; } + erp->data[erp->n].mjd=v[0]; + erp->data[erp->n].xp=v[1]*1E-6*AS2R; + erp->data[erp->n].yp=v[2]*1E-6*AS2R; + erp->data[erp->n].ut1_utc=v[3]*1E-7; + erp->data[erp->n].lod=v[4]*1E-7; + erp->data[erp->n].xpr=v[12]*1E-6*AS2R; + erp->data[erp->n++].ypr=v[13]*1E-6*AS2R; + } fclose(fp); return 1; } - - - /* get earth rotation parameter values ----------------------------------------- - * get earth rotation parameter values - * args : erp_t *erp I earth rotation parameters - * gtime_t time I time (gpst) - * double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) - * return : status (1:ok,0:error) - *-----------------------------------------------------------------------------*/ +* 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) { @@ -2821,68 +2524,58 @@ 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; @@ -2891,146 +2584,134 @@ 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; - } - 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; - } + + while (fgets(buff,sizeof(buff),fp)) { + if (!strncmp(buff,"IONUTC",6)) { + for (i=0;i<8;i++) nav->ion_gps[i]=0.0; + for (i=0;i<4;i++) nav->utc_gps[i]=0.0; + nav->leaps=0; + sscanf(buff,"IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d", + &nav->ion_gps[0],&nav->ion_gps[1],&nav->ion_gps[2],&nav->ion_gps[3], + &nav->ion_gps[4],&nav->ion_gps[5],&nav->ion_gps[6],&nav->ion_gps[7], + &nav->utc_gps[0],&nav->utc_gps[1],&nav->utc_gps[2],&nav->utc_gps[3], + &nav->leaps); + continue; } + if ((p=strchr(buff,','))) *p='\0'; else continue; + if (!(sat=satid2no(buff))) continue; + if (satsys(sat,&prn)==SYS_GLO) { + nav->geph[prn-1]=geph0; + nav->geph[prn-1].sat=sat; + toe_time=tof_time=0; + sscanf(p+1,"%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf", + &nav->geph[prn-1].iode,&nav->geph[prn-1].frq,&nav->geph[prn-1].svh, + &nav->geph[prn-1].sva,&nav->geph[prn-1].age, + &toe_time,&tof_time, + &nav->geph[prn-1].pos[0],&nav->geph[prn-1].pos[1],&nav->geph[prn-1].pos[2], + &nav->geph[prn-1].vel[0],&nav->geph[prn-1].vel[1],&nav->geph[prn-1].vel[2], + &nav->geph[prn-1].acc[0],&nav->geph[prn-1].acc[1],&nav->geph[prn-1].acc[2], + &nav->geph[prn-1].taun ,&nav->geph[prn-1].gamn ,&nav->geph[prn-1].dtaun); + nav->geph[prn-1].toe.time=toe_time; + nav->geph[prn-1].tof.time=tof_time; + } + else { + nav->eph[sat-1]=eph0; + nav->eph[sat-1].sat=sat; + toe_time=toc_time=ttr_time=0; + sscanf(p+1,"%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d", + &nav->eph[sat-1].iode,&nav->eph[sat-1].iodc,&nav->eph[sat-1].sva , + &nav->eph[sat-1].svh , + &toe_time,&toc_time,&ttr_time, + &nav->eph[sat-1].A ,&nav->eph[sat-1].e ,&nav->eph[sat-1].i0 , + &nav->eph[sat-1].OMG0,&nav->eph[sat-1].omg ,&nav->eph[sat-1].M0 , + &nav->eph[sat-1].deln,&nav->eph[sat-1].OMGd,&nav->eph[sat-1].idot, + &nav->eph[sat-1].crc ,&nav->eph[sat-1].crs ,&nav->eph[sat-1].cuc , + &nav->eph[sat-1].cus ,&nav->eph[sat-1].cic ,&nav->eph[sat-1].cis , + &nav->eph[sat-1].toes,&nav->eph[sat-1].fit ,&nav->eph[sat-1].f0 , + &nav->eph[sat-1].f1 ,&nav->eph[sat-1].f2 ,&nav->eph[sat-1].tgd[0], + &nav->eph[sat-1].code, &nav->eph[sat-1].flag); + nav->eph[sat-1].toe.time=toe_time; + nav->eph[sat-1].toc.time=toc_time; + nav->eph[sat-1].ttr.time=ttr_time; + } + } fclose(fp); return 1; } - - int savenav(const char *file, const nav_t *nav) { FILE *fp; int i; char id[32]; - + trace(3,"savenav: file=%s\n",file); - + if (!(fp=fopen(file,"w"))) return 0; - - for (i=0;ieph[i].ttr.time==0) continue; - satno2id(nav->eph[i].sat,id); - fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", - id,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].sva , - nav->eph[i].svh ,(int)nav->eph[i].toe.time, - (int)nav->eph[i].toc.time,(int)nav->eph[i].ttr.time, - nav->eph[i].A ,nav->eph[i].e ,nav->eph[i].i0 ,nav->eph[i].OMG0, - nav->eph[i].omg ,nav->eph[i].M0 ,nav->eph[i].deln,nav->eph[i].OMGd, - nav->eph[i].idot,nav->eph[i].crc,nav->eph[i].crs ,nav->eph[i].cuc , - nav->eph[i].cus ,nav->eph[i].cic,nav->eph[i].cis ,nav->eph[i].toes, - nav->eph[i].fit ,nav->eph[i].f0 ,nav->eph[i].f1 ,nav->eph[i].f2 , - nav->eph[i].tgd[0],nav->eph[i].code,nav->eph[i].flag); - } - for (i=0;igeph[i].tof.time==0) continue; - satno2id(nav->geph[i].sat,id); - fprintf(fp,"%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," - "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", - id,nav->geph[i].iode,nav->geph[i].frq,nav->geph[i].svh, - nav->geph[i].sva,nav->geph[i].age,(int)nav->geph[i].toe.time, - (int)nav->geph[i].tof.time, - nav->geph[i].pos[0],nav->geph[i].pos[1],nav->geph[i].pos[2], - nav->geph[i].vel[0],nav->geph[i].vel[1],nav->geph[i].vel[2], - nav->geph[i].acc[0],nav->geph[i].acc[1],nav->geph[i].acc[2], - nav->geph[i].taun,nav->geph[i].gamn,nav->geph[i].dtaun); - } + + 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;} @@ -3042,8 +2723,6 @@ 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 // @@ -3052,7 +2731,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 pthread_mutex_t lock_trace; /* lock for trace */ +//static lock_t lock_trace; /* lock for trace */ // //static void traceswap(void) //{ @@ -3177,7 +2856,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) @@ -3208,9 +2887,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); // } // } //} @@ -3227,7 +2906,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); // } // } //} @@ -3239,12 +2918,10 @@ 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); @@ -3263,547 +2940,501 @@ 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 SPEED_OF_LIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); - } - } - else if (frq==2) - { /* L3 */ - return SPEED_OF_LIGHT/FREQ3_GLO; - } + + if (sys==SYS_GLO) { + if (0<=frq&&frq<=1) { + for (i=0;ing;i++) { + if (nav->geph[i].sat!=sat) continue; + return CLIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); + } } - 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 (frq==2) { /* L3 */ + return CLIGHT/FREQ3_GLO; } + } + else if (sys==SYS_CMP) { + if (frq==0) return CLIGHT/FREQ1_CMP; /* B1 */ + else if (frq==1) return CLIGHT/FREQ2_CMP; /* B2 */ + else if (frq==2) return CLIGHT/FREQ3_CMP; /* B3 */ + } + else { + if (frq==0) return CLIGHT/FREQ1; /* L1/E1 */ + else if (frq==1) return CLIGHT/FREQ2; /* L2 */ + else if (frq==2) return CLIGHT/FREQ5; /* L5/E5a */ + else if (frq==3) return CLIGHT/FREQ6; /* L6/LEX */ + else if (frq==4) return CLIGHT/FREQ7; /* E5b */ + else if (frq==5) return CLIGHT/FREQ8; /* E5a+b */ + else if (frq==6) return CLIGHT/FREQ9; /* S */ + } return 0.0; } - - /* geometric distance ---------------------------------------------------------- - * compute geometric distance and receiver-to-satellite unit vector - * args : double *rs I satellilte position (ecef at transmission) (m) - * double *rr I receiver position (ecef at reception) (m) - * double *e O line-of-sight vector (ecef) - * return : geometric distance (m) (0>:error/no satellite position) - * notes : distance includes sagnac effect correction - *-----------------------------------------------------------------------------*/ +* 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 SPEED_OF_LIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9); + + return CLIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9); } - - /* ionosphere mapping function ------------------------------------------------- - * compute ionospheric delay mapping function by single layer model - * args : double *pos I receiver position {lat,lon,h} (rad,m) - * double *azel I azimuth/elevation angle {az,el} (rad) - * return : ionospheric mapping function - *-----------------------------------------------------------------------------*/ +* 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) { @@ -3812,260 +3443,235 @@ 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; - } - strcpy(tmpfile,uncfile); - stat=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; + } /* extract tar file */ - if ((p=strrchr(tmpfile,'.'))&&!strcmp(p,".tar")) - { - strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; - strcpy(buff,tmpfile); - fname=buff; - if ((p=strrchr(buff,'/'))) - { - *p='\0'; dir=fname; fname=p+1; - } - sprintf(cmd,"tar -C \"%s\" -xf \"%s\"",dir,tmpfile); - if (execcmd(cmd)) - { - if (stat) remove(tmpfile); - return -1; - } - if (stat) remove(tmpfile); - stat=1; + if ((p=strrchr(tmpfile,'.'))&&!strcmp(p,".tar")) { + + strcpy(uncfile,tmpfile); uncfile[p-tmpfile]='\0'; + strcpy(buff,tmpfile); + fname=buff; + if ((p=strrchr(buff,'/'))) { + *p='\0'; dir=fname; fname=p+1; } + sprintf(cmd,"tar -C \"%s\" -xf \"%s\"",dir,tmpfile); + if (execcmd(cmd)) { + if (stat) remove(tmpfile); + return -1; + } + if (stat) remove(tmpfile); + stat=1; + } /* extract hatanaka-compressed file by cnx2rnx */ - else if ((p=strrchr(tmpfile,'.'))&&strlen(p)>3&&(*(p+3)=='d'||*(p+3)=='D')) - { - - strcpy(uncfile,tmpfile); - uncfile[p-tmpfile+3]=*(p+3)=='D'?'O':'o'; - sprintf(cmd,"crx2rnx < \"%s\" > \"%s\"",tmpfile,uncfile); - - if (execcmd(cmd)) - { - remove(uncfile); - if (stat) remove(tmpfile); - return -1; - } + 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); - stat=1; + return -1; } + if (stat) remove(tmpfile); + stat=1; + } trace(3,"rtk_uncompress: stat=%d\n",stat); return stat; } - /* expand file path ------------------------------------------------------------ - * expand file path with wild-card (*) in file - * args : char *path I file path to expand (captal insensitive) - * char *paths O expanded file paths - * int nmax I max number of expanded file paths - * return : number of expanded file paths - * notes : the order of expanded files is alphabetical order - *-----------------------------------------------------------------------------*/ +* 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; @@ -4082,34 +3688,29 @@ 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; - } - if (p&&nd_name); + for (p=s1,q=strtok_r(s2,"*",&r);q;q=strtok_r(NULL,"*",&r)) { + if ((p=strstr(p,q))) p+=strlen(q); else break; } + if (p&&nd_name); + } closedir(dp); /* sort paths in alphabetical order */ - for (i=0;i0) - { - strcpy(tmp,paths[i]); - strcpy(paths[i],paths[j]); - strcpy(paths[j],tmp); - } - } + for (i=0;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_ #include "rtklib.h" @@ -272,4 +360,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 /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ +#endif /* RTKLIB_RTKCMN_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.cc b/src/algorithms/libs/rtklib/rtklib_sbas.cc index 3b4013723..623bd9cf0 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.cc +++ b/src/algorithms/libs/rtklib/rtklib_sbas.cc @@ -48,960 +48,863 @@ * (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 - * - *-----------------------------------------------------------------------------*/ +* +* option : -DRRCENA enable rrc correction +* +* references : +* [1] RTCA/DO-229C, Minimum operational performanc standards for global +* positioning system/wide area augmentation system airborne equipment, +* RTCA inc, November 28, 2001 +* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service +* Interface Specification for QZSS, Japan Aerospace Exploration Agency, +* July 31, 2009 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2007/10/14 1.0 new +* 2009/01/24 1.1 modify sbspntpos() api +* improve fast/ion correction update +* 2009/04/08 1.2 move function crc24q() to rcvlog.c +* support glonass, galileo and qzss +* 2009/06/08 1.3 modify sbsupdatestat() +* delete sbssatpos() +* 2009/12/12 1.4 support glonass +* 2010/01/22 1.5 support ems (egnos message service) format +* 2010/06/10 1.6 added api: +* sbssatcorr(),sbstropcorr(),sbsioncorr(), +* sbsupdatecorr() +* changed api: +* sbsreadmsgt(),sbsreadmsg() +* deleted api: +* sbspntpos(),sbsupdatestat() +* 2010/08/16 1.7 not reject udre==14 or give==15 correction message +* (2.4.0_p4) +* 2011/01/15 1.8 use api ionppp() +* add prn mask of qzss for qzss L1SAIF +* 2016/07/29 1.9 crc24q() -> rtk_crc24q() +*-----------------------------------------------------------------------------*/ #include "rtklib_sbas.h" /* extract field from line ---------------------------------------------------*/ char *getfield(char *p, int pos) { - for (pos--; pos > 0 ; pos--, p++) if (!(p = strchr(p, ','))) return NULL; + 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; - } - 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(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(5, "decode_sbstype2: type=%d iodf=%d\n", type, 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); 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)*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); + seph.sat=sat; + seph.t0 =gpst2time(msg->week,msg->tow+t); + seph.tof=gpst2time(msg->week,msg->tow); + seph.sva=getbitu(msg->msg,35,4); + seph.svh=seph.sva==15?1:0; /* unhealthy if ura==15 */ + + seph.pos[0]=getbits(msg->msg, 39,30)*0.08; + seph.pos[1]=getbits(msg->msg, 69,30)*0.08; + seph.pos[2]=getbits(msg->msg, 99,25)*0.4; + seph.vel[0]=getbits(msg->msg,124,17)*0.000625; + seph.vel[1]=getbits(msg->msg,141,17)*0.000625; + seph.vel[2]=getbits(msg->msg,158,18)*0.004; + seph.acc[0]=getbits(msg->msg,176,10)*0.0000125; + seph.acc[1]=getbits(msg->msg,186,10)*0.0000125; + seph.acc[2]=getbits(msg->msg,196,10)*0.0000625; + + seph.af0=getbits(msg->msg,206,12)*P2_31; + seph.af1=getbits(msg->msg,218, 8)*P2_39/2.0; + + i=msg->prn-MINPRNSBS; + if (!nav->seph||fabs(timediff(nav->seph[i].t0,seph.t0))<1E-3) { /* not change */ + return 0; + } + nav->seph[NSATSBS+i]=nav->seph[i]; /* previous */ + nav->seph[i]=seph; /* current */ + + trace(5,"decode_sbstype9: prn=%d\n",msg->prn); return 1; } - - /* decode type 18: ionospheric grid point masks ------------------------------*/ int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion) { const sbsigpband_t *p; - int i, j, n, m, band = getbitu(msg->msg, 18, 4); - - trace(4, "decode_sbstype18:\n"); - - if (0<=band && band<= 8) {p = igpband1[band ]; m = 8;} - else if (9<=band && band<=10) {p = igpband2[band-9]; m = 5;} + int i,j,n,m,band=getbitu(msg->msg,18,4); + + trace(4,"decode_sbstype18:\n"); + + if (0<=band&&band<= 8) {p=igpband1[band ]; m=8;} + else if (9<=band&&band<=10) {p=igpband2[band-9]; m=5;} else return 0; - - sbsion[band].iodi = (short)getbitu(msg->msg, 22, 2); - - for (i = 1, n = 0; i<= 201;i++) - { - if (!getbitu(msg->msg , 23 + i, 1)) continue; - for (j = 0; j < m; j++) - { - if (i < p[j].bits || p[j].bite < i) continue; - sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x; - sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits]; - break; - } + + sbsion[band].iodi=(short)getbitu(msg->msg,22,2); + + for (i=1,n=0;i<=201;i++) { + if (!getbitu(msg->msg,23+i,1)) continue; + for (j=0;jmsg, p, 6); - - trace(4, "decode_longcorr0:\n"); - + 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) * 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); + + sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); + + for (i=0;i<3;i++) { + sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+9*i,9)*0.125; + sbssat->sat[n-1].lcorr.dvel[i]=0.0; + } + sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+41,10)*P2_31; + sbssat->sat[n-1].lcorr.daf1=0.0; + sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow); + + trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat); return 1; } - - /* decode half long term correction (vel code=1) -----------------------------*/ int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat) { - int i, n = getbitu(msg->msg, p, 6), t; - - trace(4, "decode_longcorr1:\n"); - + 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) * 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; + + sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); + + for (i=0;i<3;i++) { + sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+i*11,11)*0.125; + sbssat->sat[n-1].lcorr.dvel[i]=getbits(msg->msg,p+58+i* 8, 8)*P2_11; + } + sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+47,11)*P2_31; + sbssat->sat[n-1].lcorr.daf1=getbits(msg->msg,p+82, 8)*P2_39; + t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400; if (t<=-43200) t+=86400; else if (t> 43200) t-=86400; - sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow+t); - - trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n-1].sat); + sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t); + + trace(5,"decode_longcorr1: sat=%2d\n",sbssat->sat[n-1].sat); return 1; } - - /* decode half long term correction ------------------------------------------*/ int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat) { - trace(4, "decode_longcorrh:\n"); - - if (getbitu(msg->msg, p, 1)==0) { /* vel code=0 */ - if (sbssat->iodp==(int)getbitu(msg->msg, p+103, 2)) - { - return decode_longcorr0(msg, p+ 1, sbssat) && - decode_longcorr0(msg, p+52, sbssat); - } - } - else if (sbssat->iodp==(int)getbitu(msg->msg, p+104, 2)) - { - return decode_longcorr1(msg, p+1, sbssat); + trace(4,"decode_longcorrh:\n"); + + if (getbitu(msg->msg,p,1)==0) { /* vel code=0 */ + if (sbssat->iodp==(int)getbitu(msg->msg,p+103,2)) { + return decode_longcorr0(msg,p+ 1,sbssat)&& + decode_longcorr0(msg,p+52,sbssat); } + } + else if (sbssat->iodp==(int)getbitu(msg->msg,p+104,2)) { + return decode_longcorr1(msg,p+1,sbssat); + } return 0; } - - /* decode type 24: mixed fast/long term correction ---------------------------*/ int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) { - int i, j, iodf, blk, udre; - - trace(4, "decode_sbstype24:\n"); - - if (sbssat->iodp!=(int)getbitu(msg->msg, 110, 2)) return 0; /* check IODP */ - - blk = getbitu(msg->msg, 112, 2); - iodf = getbitu(msg->msg, 114, 2); - - for (i = 0;i<6;i++) - { - if ((j = 13*blk+i)>=sbssat->nsat) break; - udre = getbitu(msg->msg, 86+4*i, 4); - - sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); - sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14+i*12, 12)*0.125f; - sbssat->sat[j].fcorr.udre = udre+1; - sbssat->sat[j].fcorr.iodf = iodf; - } - return decode_longcorrh(msg, 120, sbssat); + 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; + + 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 */ } - 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 (sscanf(buff,"%d %lf %lf %lf %lf %lf %lf %d", + &prn,ep,ep+1,ep+2,ep+3,ep+4,ep+5,&msg)==8) { + /* ems (EGNOS Message Service) form */ + ep[0]+=ep[0]<70.0?2000.0:1900.0; + tow=time2gpst(epoch2time(ep),&week); + p=buff+(msg>=10?25:24); } + else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) { /* NovAtel OEM4/V */ + if (!(p=getfield(buff,6))) continue; + if (sscanf(p,"%d,%lf",&week,&tow)<2) continue; + if (!(p=strchr(++p,';'))) continue; + if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue; + if (!(p=getfield(p,4))) continue; + } + else if (!strncmp(buff,"$FRMA",5)) { /* NovAtel OEM3 */ + if (!(p=getfield(buff,2))) continue; + if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue; + if (!(p=getfield(p,6))) continue; + if (weekn>=sbs->nmax) { + sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2; + if (!(sbs_msgs=(sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) { + trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax); + free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0; + return; + } + sbs->msgs=sbs_msgs; + } + sbs->msgs[sbs->n].week=week; + sbs->msgs[sbs->n].tow=(int)(tow+0.5); + sbs->msgs[sbs->n].prn=prn; + for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0; + for (i=0;*(p-1)&&*p&&i<29;p+=2,i++) { + if (sscanf(p,"%2X",&b)==1) sbs->msgs[sbs->n].msg[i]=(unsigned char)b; + } + sbs->msgs[sbs->n++].msg[28]&=0xC0; + } fclose(fp); } - - /* compare sbas messages -----------------------------------------------------*/ int cmpmsgs(const void *p1, const void *p2) { - sbsmsg_t *q1 = (sbsmsg_t *)p1,*q2 = (sbsmsg_t *)p2; + 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; - } - } - /* expand wild card in file path */ - n = expath(file,efiles,MAXEXFILE); - - for (i = 0;in>0) - { - qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs); - } - return sbs->n; -} - - -int sbsreadmsg(const char *file, int sel, sbs_t *sbs) -{ - gtime_t ts = {},te = {}; - - trace(3,"sbsreadmsg: file=%s sel=%d\n",file,sel); - - return sbsreadmsgt(file,sel,ts,te,sbs); -} - - -/* output sbas messages -------------------------------------------------------- - * output sbas message record to output file in rtklib sbas log format - * args : FILE *fp I output file pointer - * sbsmsg_t *sbsmsg I sbas messages - * return : none - *-----------------------------------------------------------------------------*/ -void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg) -{ - int i,type = sbsmsg->msg[1]>>2; - - trace(4,"sbsoutmsg:\n"); - - fprintf(fp,"%4d %6d %3d %2d : ",sbsmsg->week,sbsmsg->tow,sbsmsg->prn,type); - for (i = 0;i<29;i++) fprintf(fp,"%02X",sbsmsg->msg[i]); - fprintf(fp,"\n"); -} - - -/* search igps ---------------------------------------------------------------*/ -void searchigp(gtime_t time, const double *pos, const sbsion_t *ion, - const sbsigp_t **igp, double *x, double *y) -{ - int i,latp[2],lonp[4]; - double lat = pos[0]*R2D,lon = pos[1]*R2D; - const sbsigp_t *p; - - trace(4,"searchigp: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); - - if (lon>=180.0) lon-=360.0; - if (-55.0<=lat && lat<55.0) - { - latp[0] = (int)floor(lat/5.0)*5; - latp[1] = latp[0]+5; - lonp[0] = lonp[1] = (int)floor(lon/5.0)*5; - lonp[2] = lonp[3] = lonp[0]+5; - *x = (lon-lonp[0])/5.0; - *y = (lat-latp[0])/5.0; - } - else - { - latp[0] = (int)floor((lat-5.0)/10.0)*10+5; - latp[1] = latp[0]+10; - lonp[0] = lonp[1] = (int)floor(lon/10.0)*10; - lonp[2] = lonp[3] = lonp[0]+10; - *x = (lon-lonp[0])/10.0; - *y = (lat-latp[0])/10.0; - if (75.0<=lat && lat<85.0) - { - lonp[1] = (int)floor(lon/90.0)*90; - lonp[3] = lonp[1]+90; - } - else if (-85.0<=lat && lat<-75.0) - { - lonp[0] = (int)floor((lon-50.0)/90.0)*90+40; - lonp[2] = lonp[0]+90; - } - else if (lat>=85.0) - { - for (i = 0;i<4;i++) lonp[i] = (int)floor(lon/90.0)*90; - } - else if (lat<-85.0) - { - for (i = 0;i<4;i++) lonp[i] = (int)floor((lon-50.0)/90.0)*90+40; - } - } - for (i = 0;i<4;i++) if (lonp[i]==180) lonp[i]=-180; - for (i = 0;i<=MAXBAND;i++) - { - for (p = ion[i].igp;pt0.time==0) continue; - if (p->lat==latp[0] && p->lon==lonp[0] && p->give>0) igp[0] = p; - else if (p->lat==latp[1] && p->lon==lonp[1] && p->give>0) igp[1] = p; - else if (p->lat==latp[0] && p->lon==lonp[2] && p->give>0) igp[2] = p; - else if (p->lat==latp[1] && p->lon==lonp[3] && p->give>0) igp[3] = p; - if (igp[0] && igp[1] && igp[2] && igp[3]) return; - } - } -} - - -/* sbas ionospheric delay correction ------------------------------------------- - * compute sbas ionosphric delay correction - * args : gtime_t time I time - * nav_t *nav I navigation data - * double *pos I receiver position {lat,lon,height} (rad/m) - * double *azel I satellite azimuth/elavation angle (rad) - * double *delay O slant ionospheric delay (L1) (m) - * double *var O variance of ionospheric delay (m^2) - * return : status (1:ok, 0:no correction) - * notes : before calling the function, sbas ionosphere correction parameters - * in navigation data (nav->sbsion) must be set by callig - * sbsupdatecorr() - *-----------------------------------------------------------------------------*/ -int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, - const double *azel, double *delay, double *var) -{ - const double re = 6378.1363,hion = 350.0; - int i,err = 0; - double fp,posp[2],x = 0.0,y = 0.0,t,w[4] = {}; - const sbsigp_t *igp[4] = {}; /* {ws,wn,es,en} */ - - trace(4,"sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, - azel[0]*R2D,azel[1]*R2D); - - *delay = *var = 0.0; - if (pos[2]<-100.0||azel[1]<=0) return 1; - - /* ipp (ionospheric pierce point) position */ - fp = ionppp(pos,azel,re,hion,posp); - - /* search igps around ipp */ - searchigp(time,posp,nav->sbsion,igp,&x,&y); - - /* weight of igps */ - if (igp[0] && igp[1] && igp[2] && igp[3]) - { - w[0] = (1.0 - x) * (1.0 - y); - w[1] = (1.0 - x) * y; - w[2] = x * (1.0 - y); - w[3] = x * y; - } - else if (igp[0] && igp[1] && igp[2]) - { - w[1] = y; w[2] = x; - if ((w[0] = 1.0- w[1] - w[2]) < 0.0) err = 1; - } - else if (igp[0] && igp[2] && igp[3]) - { - w[0] = 1.0 - x; w[3] = y; - if ((w[2] = 1.0 - w[0] - w[3]) < 0.0) err = 1; - } - else if (igp[0] && igp[1] && igp[3]) - { - w[0] = 1.0 - y; w[3] = x; - if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1; - } - else if (igp[1] && igp[2] && igp[3]) - { - w[1] = 1.0-x; w[2] = 1.0-y; - if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1; - } - else err = 1; - - if (err) - { - trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f\n", posp[0] * R2D, - posp[1] * R2D); + + for (i=0;i=0;i--) free(efiles[i]); 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); + } + /* expand wild card in file path */ + n=expath(file,efiles,MAXEXFILE); + + for (i=0;in>0) { + qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs); + } + return sbs->n; +} +int sbsreadmsg(const char *file, int sel, sbs_t *sbs) +{ + gtime_t ts={},te={}; + + trace(3,"sbsreadmsg: file=%s sel=%d\n",file,sel); + + return sbsreadmsgt(file,sel,ts,te,sbs); +} +/* output sbas messages -------------------------------------------------------- +* output sbas message record to output file in rtklib sbas log format +* args : FILE *fp I output file pointer +* sbsmsg_t *sbsmsg I sbas messages +* return : none +*-----------------------------------------------------------------------------*/ +void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg) +{ + int i,type=sbsmsg->msg[1]>>2; + + trace(4,"sbsoutmsg:\n"); + + fprintf(fp,"%4d %6d %3d %2d : ",sbsmsg->week,sbsmsg->tow,sbsmsg->prn,type); + for (i=0;i<29;i++) fprintf(fp,"%02X",sbsmsg->msg[i]); + fprintf(fp,"\n"); +} +/* search igps ---------------------------------------------------------------*/ +void searchigp(gtime_t time, const double *pos, const sbsion_t *ion, + const sbsigp_t **igp, double *x, double *y) +{ + int i,latp[2],lonp[4]; + double lat=pos[0]*R2D,lon=pos[1]*R2D; + const sbsigp_t *p; + + trace(4,"searchigp: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); + + if (lon>=180.0) lon-=360.0; + if (-55.0<=lat&&lat<55.0) { + latp[0]=(int)floor(lat/5.0)*5; + latp[1]=latp[0]+5; + lonp[0]=lonp[1]=(int)floor(lon/5.0)*5; + lonp[2]=lonp[3]=lonp[0]+5; + *x=(lon-lonp[0])/5.0; + *y=(lat-latp[0])/5.0; + } + else { + latp[0]=(int)floor((lat-5.0)/10.0)*10+5; + latp[1]=latp[0]+10; + lonp[0]=lonp[1]=(int)floor(lon/10.0)*10; + lonp[2]=lonp[3]=lonp[0]+10; + *x=(lon-lonp[0])/10.0; + *y=(lat-latp[0])/10.0; + if (75.0<=lat&&lat<85.0) { + lonp[1]=(int)floor(lon/90.0)*90; + lonp[3]=lonp[1]+90; } - *delay *= fp; *var *= fp * fp; - + else if (-85.0<=lat&&lat<-75.0) { + lonp[0]=(int)floor((lon-50.0)/90.0)*90+40; + lonp[2]=lonp[0]+90; + } + else if (lat>=85.0) { + for (i=0;i<4;i++) lonp[i]=(int)floor(lon/90.0)*90; + } + else if (lat<-85.0) { + for (i=0;i<4;i++) lonp[i]=(int)floor((lon-50.0)/90.0)*90+40; + } + } + for (i=0;i<4;i++) if (lonp[i]==180) lonp[i]=-180; + for (i=0;i<=MAXBAND;i++) { + for (p=ion[i].igp;pt0.time==0) continue; + if (p->lat==latp[0]&&p->lon==lonp[0]&&p->give>0) igp[0]=p; + else if (p->lat==latp[1]&&p->lon==lonp[1]&&p->give>0) igp[1]=p; + else if (p->lat==latp[0]&&p->lon==lonp[2]&&p->give>0) igp[2]=p; + else if (p->lat==latp[1]&&p->lon==lonp[3]&&p->give>0) igp[3]=p; + if (igp[0]&&igp[1]&&igp[2]&&igp[3]) return; + } + } +} +/* sbas ionospheric delay correction ------------------------------------------- +* compute sbas ionosphric delay correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* double *pos I receiver position {lat,lon,height} (rad/m) +* double *azel I satellite azimuth/elavation angle (rad) +* double *delay O slant ionospheric delay (L1) (m) +* double *var O variance of ionospheric delay (m^2) +* return : status (1:ok, 0:no correction) +* notes : before calling the function, sbas ionosphere correction parameters +* in navigation data (nav->sbsion) must be set by callig +* sbsupdatecorr() +*-----------------------------------------------------------------------------*/ +int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, double *delay, double *var) +{ + const double re=6378.1363,hion=350.0; + int i,err=0; + double fp,posp[2],x=0.0,y=0.0,t,w[4]={}; + const sbsigp_t *igp[4]={}; /* {ws,wn,es,en} */ + + trace(4,"sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, + azel[0]*R2D,azel[1]*R2D); + + *delay=*var=0.0; + if (pos[2]<-100.0||azel[1]<=0) return 1; + + /* ipp (ionospheric pierce point) position */ + fp=ionppp(pos,azel,re,hion,posp); + + /* search igps around ipp */ + searchigp(time,posp,nav->sbsion,igp,&x,&y); + + /* weight of igps */ + if (igp[0]&&igp[1]&&igp[2]&&igp[3]) { + w[0]=(1.0-x)*(1.0-y); w[1]=(1.0-x)*y; w[2]=x*(1.0-y); w[3]=x*y; + } + else if (igp[0]&&igp[1]&&igp[2]) { + w[1]=y; w[2]=x; + if ((w[0]=1.0-w[1]-w[2])<0.0) err=1; + } + else if (igp[0]&&igp[2]&&igp[3]) { + w[0]=1.0-x; w[3]=y; + if ((w[2]=1.0-w[0]-w[3])<0.0) err=1; + } + else if (igp[0]&&igp[1]&&igp[3]) { + w[0]=1.0-y; w[3]=x; + if ((w[1]=1.0-w[0]-w[3])<0.0) err=1; + } + else if (igp[1]&&igp[2]&&igp[3]) { + w[1]=1.0-x; w[2]=1.0-y; + if ((w[3]=1.0-w[1]-w[2])<0.0) err=1; + } + else err=1; + + if (err) { + trace(2,"no sbas iono correction: lat=%3.0f lon=%4.0f\n",posp[0]*R2D, + posp[1]*R2D); + return 0; + } + for (i=0;i<4;i++) { + if (!igp[i]) continue; + t=timediff(time,igp[i]->t0); + *delay+=w[i]*igp[i]->delay; + *var+=w[i]*varicorr(igp[i]->give)*9E-8*fabs(t); + } + *delay*=fp; *var*=fp*fp; + trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var)); return 1; } - - /* get meterological parameters ----------------------------------------------*/ void getmet(double lat, double *met) { - static const double metprm[][10] = { /* lat=15,30,45,60,75 */ - {1013.25,299.65,26.31,6.30E-3,2.77, 0.00, 0.00,0.00,0.00E-3,0.00}, - {1017.25,294.15,21.79,6.05E-3,3.15, -3.75, 7.00,8.85,0.25E-3,0.33}, - {1015.75,283.15,11.66,5.58E-3,2.57, -2.25,11.00,7.24,0.32E-3,0.46}, - {1011.75,272.15, 6.78,5.39E-3,1.81, -1.75,15.00,5.36,0.81E-3,0.74}, - {1013.00,263.65, 4.11,4.53E-3,1.55, -0.50,14.50,3.39,0.62E-3,0.30} + 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.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; + double c,met[10],sinel=sin(azel[1]),h=pos[2],m; + + trace(4,"sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, + azel[0]*R2D,azel[1]*R2D); + + if (pos[2]<-100.0||10000.01E-7||fabs(pos[1]-pos_[1])>1E-7|| + fabs(pos[2]-pos_[2])>1.0) { + getmet(pos[0]*R2D,met); + c=cos(2.0*PI*(time2doy(time)-(pos[0]>=0.0?28.0:211.0))/365.25); + for (i=0;i<5;i++) met[i]-=met[i+5]*c; + zh=1E-6*k1*rd*met[0]/gm; + zw=1E-6*k2*rd/(gm*(met[4]+1.0)-met[3]*rd)*met[2]/met[1]; + zh*=pow(1.0-met[3]*h/met[1],g/(rd*met[3])); + zw*=pow(1.0-met[3]*h/met[1],(met[4]+1.0)*g/(rd*met[3])-1.0); + for (i=0;i<3;i++) pos_[i]=pos[i]; + } + m=1.001/sqrt(0.002001+sinel*sinel); + *var=0.12*0.12*m*m; return (zh+zw)*m; } - - /* long term correction ------------------------------------------------------*/ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat, - double *drs, double *ddts) + double *drs, double *ddts) { const sbssatp_t *p; double t; int i; - + trace(3,"sbslongcorr: sat=%2d\n",sat); - - for (p = sbssat->sat;psat+sbssat->nsat;p++) - { - if (p->sat != sat || p->lcorr.t0.time == 0) continue; - t = timediff(time, p->lcorr.t0); - if (fabs(t) > MAXSBSAGEL) - { - trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", - time_str(time,0), sat, t); - return 0; - } - for (i = 0; i < 3;i++) drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t; - *ddts = p->lcorr.daf0 + p->lcorr.daf1 * t; - - trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", - sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT); - - return 1; + + for (p=sbssat->sat;psat+sbssat->nsat;p++) { + if (p->sat!=sat||p->lcorr.t0.time==0) continue; + t=timediff(time,p->lcorr.t0); + if (fabs(t)>MAXSBSAGEL) { + trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", + time_str(time,0),sat,t); + return 0; } + for (i=0;i<3;i++) drs[i]=p->lcorr.dpos[i]+p->lcorr.dvel[i]*t; + *ddts=p->lcorr.daf0+p->lcorr.daf1*t; + + trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", + sat,drs[0],drs[1],drs[2],*ddts*CLIGHT); + + return 1; + } /* if sbas satellite without correction, no correction applied */ if (satsys(sat,NULL)==SYS_SBS) return 1; - + trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); return 0; } - - /* fast correction -----------------------------------------------------------*/ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat, - double *prc, double *var) + 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; - } -#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; + if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) { + *prc+=p->fcorr.rrc*t; } - trace(2,"no sbas fast correction: %s sat=%2d\n", time_str(time, 0), sat); +#endif + *var=varfcorr(p->fcorr.udre)+degfcorr(p->fcorr.ai)*t*t/2.0; + + trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat, + *prc,sqrt(*var),t); + return 1; + } + trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat); return 0; } - - /* sbas satellite ephemeris and clock correction ------------------------------- - * correct satellite position and clock bias with sbas satellite corrections - * args : gtime_t time I reception time - * int sat I satellite - * nav_t *nav I navigation data - * double *rs IO sat position and corrected {x,y,z} (ecef) (m) - * double *dts IO sat clock bias and corrected (s) - * double *var O sat position and clock variance (m^2) - * return : status (1:ok,0:no correction) - * notes : before calling the function, sbas satellite correction parameters - * in navigation data (nav->sbssat) must be set by callig - * sbsupdatecorr(). - * satellite clock correction include long-term correction and fast - * correction. - * sbas clock correction is usually based on L1C/A code. TGD or DCB has - * to be considered for other codes - *-----------------------------------------------------------------------------*/ +* 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 / SPEED_OF_LIGHT; - + if (!sbsfastcorr(time,sat,&nav->sbssat,&prc,var)) { + return 0; + } + for (i=0;i<3;i++) rs[i]+=drs[i]; + + dts[0]+=dclk+prc/CLIGHT; + trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n", - sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT, *var); - + sat,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var); + return 1; } - - /* decode sbas message --------------------------------------------------------- - * decode sbas message frame words and check crc - * args : gtime_t time I reception time - * int prn I sbas satellite prn number - * unsigned int *word I message frame words (24bit x 10) - * sbsmsg_t *sbsmsg O sbas message - * return : status (1:ok,0:crc error) - *-----------------------------------------------------------------------------*/ +* 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 9f55bfc04..f3ec8ee51 100644 --- a/src/algorithms/libs/rtklib/rtklib_sbas.h +++ b/src/algorithms/libs/rtklib/rtklib_sbas.h @@ -48,21 +48,43 @@ * (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 - * - *-----------------------------------------------------------------------------*/ +* +* option : -DRRCENA enable rrc correction +* +* references : +* [1] RTCA/DO-229C, Minimum operational performanc standards for global +* positioning system/wide area augmentation system airborne equipment, +* RTCA inc, November 28, 2001 +* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service +* Interface Specification for QZSS, Japan Aerospace Exploration Agency, +* July 31, 2009 +* +* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $ +* history : 2007/10/14 1.0 new +* 2009/01/24 1.1 modify sbspntpos() api +* improve fast/ion correction update +* 2009/04/08 1.2 move function crc24q() to rcvlog.c +* support glonass, galileo and qzss +* 2009/06/08 1.3 modify sbsupdatestat() +* delete sbssatpos() +* 2009/12/12 1.4 support glonass +* 2010/01/22 1.5 support ems (egnos message service) format +* 2010/06/10 1.6 added api: +* sbssatcorr(),sbstropcorr(),sbsioncorr(), +* sbsupdatecorr() +* changed api: +* sbsreadmsgt(),sbsreadmsg() +* deleted api: +* sbspntpos(),sbsupdatestat() +* 2010/08/16 1.7 not reject udre==14 or give==15 correction message +* (2.4.0_p4) +* 2011/01/15 1.8 use api ionppp() +* add prn mask of qzss for qzss L1SAIF +* 2016/07/29 1.9 crc24q() -> rtk_crc24q() +*-----------------------------------------------------------------------------*/ -#ifndef GNSS_SDR_RTKLIB_SBAS_H_ -#define GNSS_SDR_RTKLIB_SBAS_H_ +#ifndef RTKLIB_SBAS_H_ +#define RTKLIB_SBAS_H_ #include "rtklib.h" #include "rtklib_rtkcmn.h" @@ -72,51 +94,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}} }; @@ -124,7 +146,6 @@ 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); @@ -137,7 +158,6 @@ 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); @@ -164,4 +184,4 @@ int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, sbsmsg_t *sbsmsg); -#endif /* GNSS_SDR_RTKLIB_SBAS_H_ */ +#endif /* RTKLIB_SBAS_H_ */ diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 6c50153a8..ec6b79fe2 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -35,20 +35,19 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" -#include "gnss_frequencies.h" // Physical constants -const double GPS_C_m_s = SPEED_OF_LIGHT; //!< The speed of light, [m/s] +const double GPS_C_m_s = 299792458.0; //!< 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 = DEFAULT_OMEGA_EARTH_DOT; //!< Earth rotation rate, [rad/s] +const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< 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 = FREQ1; //!< L1 [Hz] +const double GPS_L1_FREQ_HZ = 1.57542e9; //!< 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 689cc524f..c6c703952 100644 --- a/src/core/system_parameters/GPS_L2C.h +++ b/src/core/system_parameters/GPS_L2C.h @@ -36,7 +36,6 @@ #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] @@ -49,7 +48,7 @@ const double GPS_L2_F = -4.442807633e-10; //!< Constant, [s/(m)^(1 // carrier and code frequencies -const double GPS_L2_FREQ_HZ = FREQ2; //!< L2 [Hz] +const double GPS_L2_FREQ_HZ = 1.2276e9; //!< 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 2790cd11c..258a9e954 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -37,7 +37,6 @@ #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 @@ -49,7 +48,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 = FREQ1; //!< Galileo E1 carrier frequency [Hz] +const double Galileo_E1_FREQ_HZ = 1.57542e9; //!< 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 cbd16c752..70ec19a1e 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 = FREQ5; //!< Galileo E5a carrier frequency [Hz] +const double Galileo_E5a_FREQ_HZ = 1.176450e9; //!< 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 160f0bb4d..1ed316ed5 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -40,9 +40,6 @@ 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 @@ -56,7 +53,6 @@ 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 @@ -64,11 +60,9 @@ 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 @@ -79,9 +73,8 @@ 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 @@ -102,13 +95,4 @@ 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 deleted file mode 100644 index bd85aa414..000000000 --- a/src/core/system_parameters/gnss_frequencies.h +++ /dev/null @@ -1,53 +0,0 @@ -/*! - * \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 -