mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-07-07 04:04:20 +00:00
Code Issues Releases Wiki Activity

Revert "Refactor of constants, remove defines"

Browse Source 
This reverts commit 27ab390944.
This commit is contained in:
Carles Fernandez 2017-04-21 16:15:38 +02:00
parent a1c3188c44
commit ecd1612680
23 changed files with 5521 additions and 6220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -217,8 +217,8 @@ bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
nav_data.n=valid_obs; nav_data.n=valid_obs;
for (int i=0; i< MAXSAT;i++) for (int i=0; i< MAXSAT;i++)
{ {
nav_data.lam[i][0]=SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ nav_data.lam[i][0]=CLIGHT/FREQ1; /* L1/E1 */
nav_data.lam[i][1]=SPEED_OF_LIGHT/FREQ2; /* L2 */ 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); result=pntpos(obs_data, valid_obs, &nav_data, &rtklib_opt, &old_pvt_sol, NULL, NULL,rtklib_msg);

7
src/algorithms/libs/rtklib/CMakeLists.txt
View File

@ -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. # This file is part of GNSS-SDR.
# #
@ -38,13 +38,14 @@ include_directories(
${GFlags_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS}
) )
file(GLOB RTKLIB_LIB_HEADERS "*.h") file(GLOB RTKLIB_LIB_HEADERS "*.h")
list(SORT RTKLIB_LIB_HEADERS) list(SORT RTKLIB_LIB_HEADERS)
add_library(rtklib_lib ${RTKLIB_LIB_SOURCES} ${RTKLIB_LIB_HEADERS}) add_library(rtklib_lib ${RTKLIB_LIB_SOURCES} ${RTKLIB_LIB_HEADERS})
source_group(Headers FILES ${RTKLIB_LIB_HEADERS}) source_group(Headers FILES ${RTKLIB_LIB_HEADERS})
add_dependencies(rtklib_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE}) 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( target_link_libraries(
rtklib_lib rtklib_lib
${Boost_LIBRARIES} ${Boost_LIBRARIES}
@ -55,3 +56,5 @@ target_link_libraries(
${LAPACK} ${LAPACK}
) )
#MESSAGE( STATUS "*****************BLAS: " ${BLAS} )

577
src/algorithms/libs/rtklib/rtklib.h
View File

@ -48,300 +48,356 @@
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#ifndef RTKLIB_H_
#ifndef GNSS_SDR_RTKLIB_H_ #define RTKLIB_H_
#define GNSS_SDR_RTKLIB_H_
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <stdarg.h> #include <stdarg.h>
#include <cstring> #include <cstring>
#include <cmath> #include <math.h>
#include <time.h> #include <time.h>
#include <ctype.h> #include <ctype.h>
#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) #define OMGE 7.2921151467E-5 /* earth angular velocity (IS-GPS) (rad/s) */
const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84)
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 #define HION 350000.0 /* ionosphere height (m) */
const unsigned int POLYCRC24Q = 0x1864CFBu; //!< CRC24Q polynomial
const unsigned int PMODE_SINGLE = 0; //!< positioning mode: single #define P2_5 0.03125 /* 2^-5 */
const unsigned int PMODE_DGPS = 1; //!< positioning mode: DGPS/DGNSS #define P2_6 0.015625 /* 2^-6 */
const unsigned int PMODE_KINEMA = 2; //!< positioning mode: kinematic #define P2_11 4.882812500000000E-04 /* 2^-11 */
const unsigned int PMODE_STATIC = 3; //!< positioning mode: static #define P2_15 3.051757812500000E-05 /* 2^-15 */
const unsigned int PMODE_MOVEB = 4; //!< positioning mode: moving-base #define P2_17 7.629394531250000E-06 /* 2^-17 */
const unsigned int PMODE_FIXED = 5; //!< positioning mode: fixed #define P2_19 1.907348632812500E-06 /* 2^-19 */
const unsigned int PMODE_PPP_KINEMA = 6; //!< positioning mode: PPP-kinemaric #define P2_20 9.536743164062500E-07 /* 2^-20 */
const unsigned int PMODE_PPP_STATIC = 7; //!< positioning mode: PPP-static #define P2_21 4.768371582031250E-07 /* 2^-21 */
const unsigned int PMODE_PPP_FIXED = 8; //!< positioning mode: PPP-fixed #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 #define POLYCRC32 0xEDB88320u /* CRC32 polynomial */
const unsigned int SOLF_XYZ = 1; //!< solution format: x/y/z-ecef #define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */
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
const unsigned int SOLQ_NONE = 0; //!< solution status: no solution #define PMODE_SINGLE 0 /* positioning mode: single */
const unsigned int SOLQ_FIX = 1; //!< solution status: fix #define PMODE_DGPS 1 /* positioning mode: DGPS/DGNSS */
const unsigned int SOLQ_FLOAT = 2; //!< solution status: float #define PMODE_KINEMA 2 /* positioning mode: kinematic */
const unsigned int SOLQ_SBAS = 3; //!< solution status: SBAS #define PMODE_STATIC 3 /* positioning mode: static */
const unsigned int SOLQ_DGPS = 4; //!< solution status: DGPS/DGNSS #define PMODE_MOVEB 4 /* positioning mode: moving-base */
const unsigned int SOLQ_SINGLE = 5; //!< solution status: single #define PMODE_FIXED 5 /* positioning mode: fixed */
const unsigned int SOLQ_PPP = 6; //!< solution status: PPP #define PMODE_PPP_KINEMA 6 /* positioning mode: PPP-kinemaric */
const unsigned int SOLQ_DR = 7; //!< solution status: dead reckoning #define PMODE_PPP_STATIC 7 /* positioning mode: PPP-static */
const unsigned int MAXSOLQ = 7; //!< max number of solution status #define PMODE_PPP_FIXED 8 /* positioning mode: PPP-fixed */
const unsigned int TIMES_GPST = 0; //!< time system: gps time #define SOLF_LLH 0 /* solution format: lat/lon/height */
const unsigned int TIMES_UTC = 1; //!< time system: utc #define SOLF_XYZ 1 /* solution format: x/y/z-ecef */
const unsigned int TIMES_JST = 2; //!< time system: jst #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 */ #define TIMES_GPST 0 /* time system: gps time */
const double DTTOL = 0.005; //!< tolerance of time difference (s) */ #define TIMES_UTC 1 /* time system: utc */
#define TIMES_JST 2 /* time system: jst */
const unsigned int NFREQ = 3; #define MAXFREQ 7 /* max NFREQ */
const unsigned int NEXOBS = 0; //!< number of extended obs codes */
const unsigned int MAXANT = 64; //!< max length of station name/antenna type */
const unsigned int MINPRNGPS = 1; //!< min satellite PRN number of GPS */ #define FREQ1 1.57542E9 /* L1/E1 frequency (Hz) */
const unsigned int MAXPRNGPS = 32; //!< max satellite PRN number of GPS */ #define FREQ2 1.22760E9 /* L2 frequency (Hz) */
const unsigned int NSATGPS = (MAXPRNGPS - MINPRNGPS + 1); //!< number of GPS satellites */ #define FREQ5 1.17645E9 /* L5/E5a frequency (Hz) */
const unsigned int NSYSGPS = 1; #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 */ #define MAXLEAPS 64 /* max number of leap seconds table */
const int SYS_GPS = 0x01; //!< navigation system: GPS */ #define DTTOL 0.005 /* tolerance of time difference (s) */
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 */
const unsigned int CODE_NONE = 0; //!< obs code: none or unknown */ #define NFREQ 3
const unsigned int CODE_L1C = 1; //!< obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */ #define NEXOBS 0 /* number of extended obs codes */
const unsigned int CODE_L1P = 2; //!< obs code: L1P,G1P (GPS,GLO) */ #define MAXANT 64 /* max length of station name/antenna type */
const unsigned int CODE_L1W = 3; //!< obs code: L1 Z-track (GPS) */
const unsigned int CODE_L1Y = 4; //!< obs code: L1Y (GPS) */ #define MINPRNGPS 1 /* min satellite PRN number of GPS */
const unsigned int CODE_L1M = 5; //!< obs code: L1M (GPS) */ #define MAXPRNGPS 32 /* max satellite PRN number of GPS */
const unsigned int CODE_L1N = 6; //!< obs code: L1codeless (GPS) */ #define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */
const unsigned int CODE_L1S = 7; //!< obs code: L1C(D) (GPS,QZS) */ #define NSYSGPS 1
const unsigned int CODE_L1L = 8; //!< obs code: L1C(P) (GPS,QZS) */
const unsigned int CODE_L1E = 9; //!< (not used) */ #define SYS_NONE 0x00 /* navigation system: none */
const unsigned int CODE_L1A = 10; //!< obs code: E1A (GAL) */ #define SYS_GPS 0x01 /* navigation system: GPS */
const unsigned int CODE_L1B = 11; //!< obs code: E1B (GAL) */ #define SYS_SBS 0x02 /* navigation system: SBAS */
const unsigned int CODE_L1X = 12; //!< obs code: E1B+C,L1C(D+P) (GAL,QZS) */ #define SYS_GLO 0x04 /* navigation system: GLONASS */
const unsigned int CODE_L1Z = 13; //!< obs code: E1A+B+C,L1SAIF (GAL,QZS) */ #define SYS_GAL 0x08 /* navigation system: Galileo */
const unsigned int CODE_L2C = 14; //!< obs code: L2C/A,G1C/A (GPS,GLO) */ #define SYS_QZS 0x10 /* navigation system: QZSS */
const unsigned int CODE_L2D = 15; //!< obs code: L2 L1C/A-(P2-P1) (GPS) */ #define SYS_CMP 0x20 /* navigation system: BeiDou */
const unsigned int CODE_L2S = 16; //!< obs code: L2C(M) (GPS,QZS) */ #define SYS_IRN 0x40 /* navigation system: IRNS */
const unsigned int CODE_L2L = 17; //!< obs code: L2C(L) (GPS,QZS) */ #define SYS_LEO 0x80 /* navigation system: LEO */
const unsigned int CODE_L2X = 18; //!< obs code: L2C(M+L),B1I+Q (GPS,QZS,BDS) */ #define SYS_ALL 0xFF /* navigation system: all */
const unsigned int CODE_L2P = 19; //!< obs code: L2P,G2P (GPS,GLO) */
const unsigned int CODE_L2W = 20; //!< obs code: L2 Z-track (GPS) */ #define CODE_NONE 0 /* obs code: none or unknown */
const unsigned int CODE_L2Y = 21; //!< obs code: L2Y (GPS) */ #define CODE_L1C 1 /* obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) */
const unsigned int CODE_L2M = 22; //!< obs code: L2M (GPS) */ #define CODE_L1P 2 /* obs code: L1P,G1P (GPS,GLO) */
const unsigned int CODE_L2N = 23; //!< obs code: L2codeless (GPS) */ #define CODE_L1W 3 /* obs code: L1 Z-track (GPS) */
const unsigned int CODE_L5I = 24; //!< obs code: L5/E5aI (GPS,GAL,QZS,SBS) */ #define CODE_L1Y 4 /* obs code: L1Y (GPS) */
const unsigned int CODE_L5Q = 25; //!< obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */ #define CODE_L1M 5 /* obs code: L1M (GPS) */
const unsigned int CODE_L5X = 26; //!< obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) */ #define CODE_L1N 6 /* obs code: L1codeless (GPS) */
const unsigned int CODE_L7I = 27; //!< obs code: E5bI,B2I (GAL,BDS) */ #define CODE_L1S 7 /* obs code: L1C(D) (GPS,QZS) */
const unsigned int CODE_L7Q = 28; //!< obs code: E5bQ,B2Q (GAL,BDS) */ #define CODE_L1L 8 /* obs code: L1C(P) (GPS,QZS) */
const unsigned int CODE_L7X = 29; //!< obs code: E5bI+Q,B2I+Q (GAL,BDS) */ #define CODE_L1E 9 /* (not used) */
const unsigned int CODE_L6A = 30; //!< obs code: E6A (GAL) */ #define CODE_L1A 10 /* obs code: E1A (GAL) */
const unsigned int CODE_L6B = 31; //!< obs code: E6B (GAL) */ #define CODE_L1B 11 /* obs code: E1B (GAL) */
const unsigned int CODE_L6C = 32; //!< obs code: E6C (GAL) */ #define CODE_L1X 12 /* obs code: E1B+C,L1C(D+P) (GAL,QZS) */
const unsigned int CODE_L6X = 33; //!< obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,BDS) */ #define CODE_L1Z 13 /* obs code: E1A+B+C,L1SAIF (GAL,QZS) */
const unsigned int CODE_L6Z = 34; //!< obs code: E6A+B+C (GAL) */ #define CODE_L2C 14 /* obs code: L2C/A,G1C/A (GPS,GLO) */
const unsigned int CODE_L6S = 35; //!< obs code: LEXS (QZS) */ #define CODE_L2D 15 /* obs code: L2 L1C/A-(P2-P1) (GPS) */
const unsigned int CODE_L6L = 36; //!< obs code: LEXL (QZS) */ #define CODE_L2S 16 /* obs code: L2C(M) (GPS,QZS) */
const unsigned int CODE_L8I = 37; //!< obs code: E5(a+b)I (GAL) */ #define CODE_L2L 17 /* obs code: L2C(L) (GPS,QZS) */
const unsigned int CODE_L8Q = 38; //!< obs code: E5(a+b)Q (GAL) */ #define CODE_L2X 18 /* obs code: L2C(M+L),B1I+Q (GPS,QZS,CMP) */
const unsigned int CODE_L8X = 39; //!< obs code: E5(a+b)I+Q (GAL) */ #define CODE_L2P 19 /* obs code: L2P,G2P (GPS,GLO) */
const unsigned int CODE_L2I = 40; //!< obs code: B1I (BDS) */ #define CODE_L2W 20 /* obs code: L2 Z-track (GPS) */
const unsigned int CODE_L2Q = 41; //!< obs code: B1Q (BDS) */ #define CODE_L2Y 21 /* obs code: L2Y (GPS) */
const unsigned int CODE_L6I = 42; //!< obs code: B3I (BDS) */ #define CODE_L2M 22 /* obs code: L2M (GPS) */
const unsigned int CODE_L6Q = 43; //!< obs code: B3Q (BDS) */ #define CODE_L2N 23 /* obs code: L2codeless (GPS) */
const unsigned int CODE_L3I = 44; //!< obs code: G3I (GLO) */ #define CODE_L5I 24 /* obs code: L5/E5aI (GPS,GAL,QZS,SBS) */
const unsigned int CODE_L3Q = 45; //!< obs code: G3Q (GLO) */ #define CODE_L5Q 25 /* obs code: L5/E5aQ (GPS,GAL,QZS,SBS) */
const unsigned int CODE_L3X = 46; //!< obs code: G3I+Q (GLO) */ #define CODE_L5X 26 /* obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) */
const unsigned int CODE_L1I = 47; //!< obs code: B1I (BDS) */ #define CODE_L7I 27 /* obs code: E5bI,B2I (GAL,CMP) */
const unsigned int CODE_L1Q = 48; //!< obs code: B1Q (BDS) */ #define CODE_L7Q 28 /* obs code: E5bQ,B2Q (GAL,CMP) */
const unsigned int CODE_L5A = 49; //!< obs code: L5A SPS (IRN) */ #define CODE_L7X 29 /* obs code: E5bI+Q,B2I+Q (GAL,CMP) */
const unsigned int CODE_L5B = 50; //!< obs code: L5B RS(D) (IRN) */ #define CODE_L6A 30 /* obs code: E6A (GAL) */
const unsigned int CODE_L5C = 51; //!< obs code: L5C RS(P) (IRN) */ #define CODE_L6B 31 /* obs code: E6B (GAL) */
const unsigned int CODE_L9A = 52; //!< obs code: SA SPS (IRN) */ #define CODE_L6C 32 /* obs code: E6C (GAL) */
const unsigned int CODE_L9B = 53; //!< obs code: SB RS(D) (IRN) */ #define CODE_L6X 33 /* obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,CMP) */
const unsigned int CODE_L9C = 54; //!< obs code: SC RS(P) (IRN) */ #define CODE_L6Z 34 /* obs code: E6A+B+C (GAL) */
const unsigned int CODE_L9X = 55; //!< obs code: SB+C (IRN) */ #define CODE_L6S 35 /* obs code: LEXS (QZS) */
const unsigned int MAXCODE = 55; //!< max number of obs code */ #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 #ifdef ENAGLO
const unsigned int MINPRNGLO = 1; //!< min satellite slot number of GLONASS */ #define MINPRNGLO 1 /* min satellite slot number of GLONASS */
const unsigned int MAXPRNGLO = 27; //!< max satellite slot number of GLONASS */ #define MAXPRNGLO 27 /* max satellite slot number of GLONASS */
const unsigned int NSATGLO = (MAXPRNGLO - MINPRNGLO + 1); //!< number of GLONASS satellites */ #define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */
const unsigned int NSYSGLO = 1; #define NSYSGLO 1
#else #else
const unsigned int MINPRNGLO = 0; #define MINPRNGLO 0
const unsigned int MAXPRNGLO = 0; #define MAXPRNGLO 0
const unsigned int NSATGLO = 0; #define NSATGLO 0
const unsigned int NSYSGLO = 0; #define NSYSGLO 0
#endif #endif
#ifdef ENAGAL #ifdef ENAGAL
const unsigned int MINPRNGAL = 1; //!< min satellite PRN number of Galileo */ #define MINPRNGAL 1 /* min satellite PRN number of Galileo */
const unsigned int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo */ #define MAXPRNGAL 30 /* max satellite PRN number of Galileo */
const unsigned int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites */ #define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */
const unsigned int NSYSGAL = 1; #define NSYSGAL 1
#else #else
const unsigned int MINPRNGAL = 0; #define MINPRNGAL 0
const unsigned int MAXPRNGAL = 0; #define MAXPRNGAL 0
const unsigned int NSATGAL = 0; #define NSATGAL 0
const unsigned int NSYSGAL = 0; #define NSYSGAL 0
#endif #endif
#ifdef ENAQZS #ifdef ENAQZS
const unsigned int MINPRNQZS = 193; //!< min satellite PRN number of QZSS */ #define MINPRNQZS 193 /* min satellite PRN number of QZSS */
const unsigned int MAXPRNQZS = 199; //!< max satellite PRN number of QZSS */ #define MAXPRNQZS 199 /* max satellite PRN number of QZSS */
const unsigned int MINPRNQZS_S = 183; //!< min satellite PRN number of QZSS SAIF */ #define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */
const unsigned int MAXPRNQZS_S = 189; //!< max satellite PRN number of QZSS SAIF */ #define MAXPRNQZS_S 189 /* max satellite PRN number of QZSS SAIF */
const unsigned int NSATQZS = (MAXPRNQZS - MINPRNQZS + 1); //!< number of QZSS satellites */ #define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */
const unsigned int NSYSQZS = 1; #define NSYSQZS 1
#else #else
const unsigned int MINPRNQZS = 0; #define MINPRNQZS 0
const unsigned int MAXPRNQZS = 0; #define MAXPRNQZS 0
const unsigned int MINPRNQZS_S = 0; #define MINPRNQZS_S 0
const unsigned int MAXPRNQZS_S = 0; #define MAXPRNQZS_S 0
const unsigned int NSATQZS = 0; #define NSATQZS 0
const unsigned int NSYSQZS = 0; #define NSYSQZS 0
#endif #endif
#ifdef ENACMP
#ifdef ENABDS #define MINPRNCMP 1 /* min satellite sat number of BeiDou */
const unsigned int MINPRNBDS = 1; //!< min satellite sat number of BeiDou */ #define MAXPRNCMP 35 /* max satellite sat number of BeiDou */
const unsigned int MAXPRNBDS = 35; //!< max satellite sat number of BeiDou */ #define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */
const unsigned int NSATBDS = (MAXPRNBDS - MINPRNCM + 1); //!< number of BeiDou satellites */ #define NSYSCMP 1
const unsigned int NSYSBDS = 1;
#else #else
const unsigned int MINPRNBDS = 0; #define MINPRNCMP 0
const unsigned int MAXPRNBDS = 0; #define MAXPRNCMP 0
const unsigned int NSATBDS = 0; #define NSATCMP 0
const unsigned int NSYSBDS = 0; #define NSYSCMP 0
#endif #endif
#ifdef ENAIRN #ifdef ENAIRN
const unsigned int MINPRNIRN = 1; //!< min satellite sat number of IRNSS */ #define MINPRNIRN 1 /* min satellite sat number of IRNSS */
const unsigned int MAXPRNIRN = 7; //!< max satellite sat number of IRNSS */ #define MAXPRNIRN 7 /* max satellite sat number of IRNSS */
const unsigned int NSATIRN = (MAXPRNIRN - MINPRNIRN + 1); //!< number of IRNSS satellites */ #define NSATIRN (MAXPRNIRN-MINPRNIRN+1) /* number of IRNSS satellites */
const unsigned int NSYSIRN = 1; #define NSYSIRN 1
#else #else
const unsigned int MINPRNIRN = 0; #define MINPRNIRN 0
const unsigned int MAXPRNIRN = 0; #define MAXPRNIRN 0
const unsigned int NSATIRN = 0; #define NSATIRN 0
const unsigned int NSYSIRN = 0; #define NSYSIRN 0
#endif #endif
#ifdef ENALEO #ifdef ENALEO
const unsigned int MINPRNLEO = 1; //!< min satellite sat number of LEO */ #define MINPRNLEO 1 /* min satellite sat number of LEO */
const unsigned int NSATLEO = 10; //!< max satellite sat number of LEO */ #define MAXPRNLEO 10 /* max satellite sat number of LEO */
const unsigned int NSATLEO = (MAXPRNLEO - MINPRNLEO + 1); //!< number of LEO satellites */ #define NSATLEO (MAXPRNLEO-MINPRNLEO+1) /* number of LEO satellites */
const unsigned int NSYSLEO = 1; #define NSYSLEO 1
#else #else
const unsigned int MINPRNLEO = 0; #define MINPRNLEO 0
const unsigned int MAXPRNLEO = 0; #define MAXPRNLEO 0
const unsigned int NSATLEO = 0; #define NSATLEO 0
const unsigned int NSYSLEO = 0; #define NSYSLEO 0
#endif #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 */ #define MINPRNSBS 120 /* min satellite PRN number of SBAS */
const unsigned int MAXPRNSBS = 142; //!< max satellite PRN number of SBAS */ #define MAXPRNSBS 142 /* max satellite PRN number of SBAS */
const unsigned int NSATSBS = (MAXPRNSBS - MINPRNSBS + 1); //!< number of SBAS satellites */ #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 #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 #endif
const unsigned int MAXRCV = 64; //!< max receiver number (1 to MAXRCV) */ #define MAXRCV 64 /* max receiver number (1 to MAXRCV) */
const unsigned int MAXOBSTYPE = 64; //!< max number of obs type in RINEX */ #define MAXOBSTYPE 64 /* max number of obs type in RINEX */
const double MAXDTOE = 7200.0; //!< max time difference to GPS Toe (s) */ #define DTTOL 0.005 /* tolerance of time difference (s) */
const double MAXDTOE_QZS = 7200.0; //!< max time difference to QZSS Toe (s) */ #define MAXDTOE 7200.0 /* max time difference to GPS Toe (s) */
const double MAXDTOE_GAL = 10800.0; //!< max time difference to Galileo Toe (s) */ #define MAXDTOE_QZS 7200.0 /* max time difference to QZSS Toe (s) */
const double MAXDTOE_BDS = 21600.0; //!< max time difference to BeiDou Toe (s) */ #define MAXDTOE_GAL 10800.0 /* max time difference to Galileo Toe (s) */
const double MAXDTOE_GLO = 1800.0; //!< max time difference to GLONASS Toe (s) */ #define MAXDTOE_CMP 21600.0 /* max time difference to BeiDou Toe (s) */
const double MAXDTOE_SBS = 360.0; //!< max time difference to SBAS Toe (s) */ #define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */
const double MAXDTOE_S = 86400.0; //!< max time difference to ephem toe (s) for other */ #define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */
const double MAXGDOP = 300.0; //!< max GDOP */ #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 */ #define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
const unsigned int MAXBAND = 10; //!< max SBAS band of IGP */ #define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
const unsigned int MAXNIGP = 201; //!< max number of IGP in SBAS band */ #define MAXSBSURA 8 /* max URA of SBAS satellite */
const unsigned int MAXNGEO = 4; //!< max number of GEO satellites */ #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 */ #define MAXSOLMSG 8191 /* max length of solution message */
const unsigned int MAXERRMSG = 4096; //!< max length of error/warning message */ #define MAXERRMSG 4096 /* max length of error/warning message */
const unsigned int IONOOPT_OFF = 0; //!< ionosphere option: correction off */ #define IONOOPT_OFF 0 /* ionosphere option: correction off */
const unsigned int IONOOPT_BRDC = 1; //!< ionosphere option: broadcast model */ #define IONOOPT_BRDC 1 /* ionosphere option: broadcast model */
const unsigned int IONOOPT_SBAS = 2; //!< ionosphere option: SBAS model */ #define IONOOPT_SBAS 2 /* ionosphere option: SBAS model */
const unsigned int IONOOPT_IFLC = 3; //!< ionosphere option: L1/L2 or L1/L5 iono-free LC */ #define IONOOPT_IFLC 3 /* ionosphere option: L1/L2 or L1/L5 iono-free LC */
const unsigned int IONOOPT_EST = 4; //!< ionosphere option: estimation */ #define IONOOPT_EST 4 /* ionosphere option: estimation */
const unsigned int IONOOPT_TEC = 5; //!< ionosphere option: IONEX TEC model */ #define IONOOPT_TEC 5 /* ionosphere option: IONEX TEC model */
const unsigned int IONOOPT_QZS = 6; //!< ionosphere option: QZSS broadcast model */ #define IONOOPT_QZS 6 /* ionosphere option: QZSS broadcast model */
const unsigned int IONOOPT_LEX = 7; //!< ionosphere option: QZSS LEX ionospehre */ #define IONOOPT_LEX 7 /* ionosphere option: QZSS LEX ionospehre */
const unsigned int IONOOPT_STEC = 8; //!< ionosphere option: SLANT TEC model */ #define IONOOPT_STEC 8 /* ionosphere option: SLANT TEC model */
const unsigned int TROPOPT_OFF = 0; //!< troposphere option: correction off */ #define TROPOPT_OFF 0 /* troposphere option: correction off */
const unsigned int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model */ #define TROPOPT_SAAS 1 /* troposphere option: Saastamoinen model */
const unsigned int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model */ #define TROPOPT_SBAS 2 /* troposphere option: SBAS model */
const unsigned int TROPOPT_EST = 3; //!< troposphere option: ZTD estimation */ #define TROPOPT_EST 3 /* troposphere option: ZTD estimation */
const unsigned int TROPOPT_ESTG = 4; //!< troposphere option: ZTD+grad estimation */ #define TROPOPT_ESTG 4 /* troposphere option: ZTD+grad estimation */
const unsigned int TROPOPT_ZTD = 5; //!< troposphere option: ZTD correction */ #define TROPOPT_ZTD 5 /* troposphere option: ZTD correction */
const unsigned int EPHOPT_BRDC = 0; //!< ephemeris option: broadcast ephemeris */ #define EPHOPT_BRDC 0 /* ephemeris option: broadcast ephemeris */
const unsigned int EPHOPT_PREC = 1; //!< ephemeris option: precise ephemeris */ #define EPHOPT_PREC 1 /* ephemeris option: precise ephemeris */
const unsigned int EPHOPT_SBAS = 2; //!< ephemeris option: broadcast + SBAS */ #define EPHOPT_SBAS 2 /* ephemeris option: broadcast + SBAS */
const unsigned int EPHOPT_SSRAPC = 3; //!< ephemeris option: broadcast + SSR_APC */ #define EPHOPT_SSRAPC 3 /* ephemeris option: broadcast + SSR_APC */
const unsigned int EPHOPT_SSRCOM = 4; //!< ephemeris option: broadcast + SSR_COM */ #define EPHOPT_SSRCOM 4 /* ephemeris option: broadcast + SSR_COM */
const unsigned int EPHOPT_LEX = 5; //!< ephemeris option: QZSS LEX ephemeris */ #define EPHOPT_LEX 5 /* ephemeris option: QZSS LEX ephemeris */
const double EFACT_GPS = 1.0; //!< error factor: GPS */ #define EFACT_GPS 1.0 /* error factor: GPS */
const double EFACT_GLO = 1.5; //!< error factor: GLONASS */ #define EFACT_GLO 1.5 /* error factor: GLONASS */
const double EFACT_GAL = 1.0; //!< error factor: Galileo */ #define EFACT_GAL 1.0 /* error factor: Galileo */
const double EFACT_QZS = 1.0; //!< error factor: QZSS */ #define EFACT_QZS 1.0 /* error factor: QZSS */
const double EFACT_BDS = 1.0; //!< error factor: BeiDou */ #define EFACT_CMP 1.0 /* error factor: BeiDou */
const double EFACT_IRN = 1.5; //!< error factor: IRNSS */ #define EFACT_IRN 1.5 /* error factor: IRNSS */
const double EFACT_SBS = 3.0; //!< error factor: SBAS */ #define EFACT_SBS 3.0 /* error factor: SBAS */
const unsigned int MAXEXFILE = 1024; //!< max number of expanded files */ #define MAXEXFILE 1024 /* max number of expanded files */
const double MAXSBSAGEF = 30.0; //!< max age of SBAS fast correction (s) */ #define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
const double MAXSBSAGEL = 1800.0; //!< max age of SBAS long term corr (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 void fatalfunc_t(const char *); /* fatal callback function type */
typedef struct { /* time struct */ typedef struct { /* time struct */
time_t time; /* time (s) expressed by standard time_t */ time_t time; /* time (s) expressed by standard time_t */
double sec; /* fraction of second under 1 s */ double sec; /* fraction of second under 1 s */
} gtime_t; } gtime_t;
typedef struct { /* observation data record */ typedef struct { /* observation data record */
gtime_t time; /* receiver sampling time (GPST) */ gtime_t time; /* receiver sampling time (GPST) */
unsigned char sat,rcv; /* satellite/receiver number */ 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) */ float D[NFREQ+NEXOBS]; /* observation data doppler frequency (Hz) */
} obsd_t; } obsd_t;
typedef struct { /* observation data */ typedef struct { /* observation data */
int n,nmax; /* number of obervation data/allocated */ int n,nmax; /* number of obervation data/allocated */
obsd_t *data; /* observation data records */ obsd_t *data; /* observation data records */
} obs_t; } obs_t;
typedef struct { /* earth rotation parameter data type */ typedef struct { /* earth rotation parameter data type */
double mjd; /* mjd (days) */ double mjd; /* mjd (days) */
double xp,yp; /* pole offset (rad) */ double xp,yp; /* pole offset (rad) */
@ -368,13 +422,11 @@ typedef struct { /* earth rotation parameter data type */
double lod; /* length of day (s/day) */ double lod; /* length of day (s/day) */
} erpd_t; } erpd_t;
typedef struct { /* earth rotation parameter type */ typedef struct { /* earth rotation parameter type */
int n,nmax; /* number and max number of data */ int n,nmax; /* number and max number of data */
erpd_t *data; /* earth rotation parameter data */ erpd_t *data; /* earth rotation parameter data */
} erp_t; } erp_t;
typedef struct { /* antenna parameter type */ typedef struct { /* antenna parameter type */
int sat; /* satellite number (0:receiver) */ int sat; /* satellite number (0:receiver) */
char type[MAXANT]; /* antenna type */ 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) */ /* el=90,85,...,0 or nadir=0,1,2,3,... (deg) */
} pcv_t; } pcv_t;
typedef struct { /* antenna parameters type */ typedef struct { /* antenna parameters type */
int n,nmax; /* number of data/allocated */ int n,nmax; /* number of data/allocated */
pcv_t *pcv; /* antenna parameters data */ pcv_t *pcv; /* antenna parameters data */
} pcvs_t; } pcvs_t;
typedef struct { /* almanac type */ typedef struct { /* almanac type */
int sat; /* satellite number */ int sat; /* satellite number */
int svh; /* sv health (0:ok) */ int svh; /* sv health (0:ok) */
@ -404,15 +454,14 @@ typedef struct { /* almanac type */
double f0,f1; /* SV clock parameters (af0,af1) */ double f0,f1; /* SV clock parameters (af0,af1) */
} alm_t; } alm_t;
typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */
int sat; /* satellite number */ int sat; /* satellite number */
int iode,iodc; /* IODE,IODC */ int iode,iodc; /* IODE,IODC */
int sva; /* SV accuracy (URA index) */ int sva; /* SV accuracy (URA index) */
int svh; /* SV health (0:ok) */ int svh; /* SV health (0:ok) */
int week; /* GPS/QZS: gps week, GAL: galileo week */ int week; /* GPS/QZS: gps week, GAL: galileo week */
int code; /* GPS/QZS: code on L2, GAL/BDS: data sources */ int code; /* GPS/QZS: code on L2, GAL/CMP: data sources */
int flag; /* GPS/QZS: L2 P data flag, BDS: nav type */ int flag; /* GPS/QZS: L2 P data flag, CMP: nav type */
gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */ gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */
/* SV orbit parameters */ /* SV orbit parameters */
double A,e,i0,OMG0,omg,M0,deln,OMGd,idot; 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 */ double tgd[4]; /* group delay parameters */
/* GPS/QZS:tgd[0]=TGD */ /* GPS/QZS:tgd[0]=TGD */
/* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ /* 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 */ double Adot,ndot; /* Adot,ndot for CNAV */
} eph_t; } eph_t;
typedef struct { /* GLONASS broadcast ephemeris type */ typedef struct { /* GLONASS broadcast ephemeris type */
int sat; /* satellite number */ int sat; /* satellite number */
int iode; /* IODE (0-6 bit of tb field) */ 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) */ float vco[MAXSAT][3]; /* satellite velocity covariance (m^2) */
} peph_t; } peph_t;
typedef struct { /* precise clock type */ typedef struct { /* precise clock type */
gtime_t time; /* time (GPST) */ gtime_t time; /* time (GPST) */
int index; /* clock index for multiple files */ int index; /* clock index for multiple files */
@ -461,7 +508,6 @@ typedef struct { /* precise clock type */
float std[MAXSAT][1]; /* satellite clock std (s) */ float std[MAXSAT][1]; /* satellite clock std (s) */
} pclk_t; } pclk_t;
typedef struct { /* SBAS ephemeris type */ typedef struct { /* SBAS ephemeris type */
int sat; /* satellite number */ int sat; /* satellite number */
gtime_t t0; /* reference epoch time (GPST) */ 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) */ double af0,af1; /* satellite clock-offset/drift (s,s/s) */
} seph_t; } seph_t;
typedef struct { /* norad two line element data type */ typedef struct { /* norad two line element data type */
char name [32]; /* common name */ char name [32]; /* common name */
char alias[32]; /* alias name */ char alias[32]; /* alias name */
@ -496,13 +541,11 @@ typedef struct { /* norad two line element data type */
int rev; /* revolution number at epoch */ int rev; /* revolution number at epoch */
} tled_t; } tled_t;
typedef struct { /* norad two line element type */ typedef struct { /* norad two line element type */
int n,nmax; /* number/max number of two line element data */ int n,nmax; /* number/max number of two line element data */
tled_t *data; /* norad two line element data */ tled_t *data; /* norad two line element data */
} tle_t; } tle_t;
typedef struct { /* TEC grid type */ typedef struct { /* TEC grid type */
gtime_t time; /* epoch time (GPST) */ gtime_t time; /* epoch time (GPST) */
int ndata[3]; /* TEC grid data size {nlat,nlon,nhgt} */ int ndata[3]; /* TEC grid data size {nlat,nlon,nhgt} */
@ -514,27 +557,23 @@ typedef struct { /* TEC grid type */
float *rms; /* RMS values (tecu) */ float *rms; /* RMS values (tecu) */
} tec_t; } tec_t;
typedef struct { /* satellite fcb data type */ typedef struct { /* satellite fcb data type */
gtime_t ts,te; /* start/end time (GPST) */ gtime_t ts,te; /* start/end time (GPST) */
double bias[MAXSAT][3]; /* fcb value (cyc) */ double bias[MAXSAT][3]; /* fcb value (cyc) */
double std [MAXSAT][3]; /* fcb std-dev (cyc) */ double std [MAXSAT][3]; /* fcb std-dev (cyc) */
} fcbd_t; } fcbd_t;
typedef struct { /* SBAS message type */ typedef struct { /* SBAS message type */
int week,tow; /* receiption time */ int week,tow; /* receiption time */
int prn; /* SBAS satellite PRN number */ int prn; /* SBAS satellite PRN number */
unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */
} sbsmsg_t; } sbsmsg_t;
typedef struct { /* SBAS messages type */ typedef struct { /* SBAS messages type */
int n,nmax; /* number of SBAS messages/allocated */ int n,nmax; /* number of SBAS messages/allocated */
sbsmsg_t *msgs; /* SBAS messages */ sbsmsg_t *msgs; /* SBAS messages */
} sbs_t; } sbs_t;
typedef struct { /* SBAS fast correction type */ typedef struct { /* SBAS fast correction type */
gtime_t t0; /* time of applicability (TOF) */ gtime_t t0; /* time of applicability (TOF) */
double prc; /* pseudorange correction (PRC) (m) */ double prc; /* pseudorange correction (PRC) (m) */
@ -545,7 +584,6 @@ typedef struct { /* SBAS fast correction type */
short ai; /* degradation factor indicator */ short ai; /* degradation factor indicator */
} sbsfcorr_t; } sbsfcorr_t;
typedef struct { /* SBAS long term satellite error correction type */ typedef struct { /* SBAS long term satellite error correction type */
gtime_t t0; /* correction time */ gtime_t t0; /* correction time */
int iode; /* IODE (issue of date ephemeris) */ 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) */ double daf0,daf1; /* delta clock-offset/drift (s,s/s) */
} sbslcorr_t; } sbslcorr_t;
typedef struct { /* SBAS satellite correction type */ typedef struct { /* SBAS satellite correction type */
int sat; /* satellite number */ int sat; /* satellite number */
sbsfcorr_t fcorr; /* fast correction */ sbsfcorr_t fcorr; /* fast correction */
sbslcorr_t lcorr; /* long term correction */ sbslcorr_t lcorr; /* long term correction */
} sbssatp_t; } sbssatp_t;
typedef struct { /* SBAS satellite corrections type */ typedef struct { /* SBAS satellite corrections type */
int iodp; /* IODP (issue of date mask) */ int iodp; /* IODP (issue of date mask) */
int nsat; /* number of satellites */ int nsat; /* number of satellites */
@ -569,7 +605,6 @@ typedef struct { /* SBAS satellite corrections type */
sbssatp_t sat[MAXSAT]; /* satellite correction */ sbssatp_t sat[MAXSAT]; /* satellite correction */
} sbssat_t; } sbssat_t;
typedef struct { /* SBAS ionospheric correction type */ typedef struct { /* SBAS ionospheric correction type */
gtime_t t0; /* correction time */ gtime_t t0; /* correction time */
short lat,lon; /* latitude/longitude (deg) */ short lat,lon; /* latitude/longitude (deg) */
@ -577,7 +612,6 @@ typedef struct { /* SBAS ionospheric correction type */
float delay; /* vertical delay estimate (m) */ float delay; /* vertical delay estimate (m) */
} sbsigp_t; } sbsigp_t;
typedef struct { /* IGP band type */ typedef struct { /* IGP band type */
short x; /* longitude/latitude (deg) */ short x; /* longitude/latitude (deg) */
const short *y; /* latitudes/longitudes (deg) */ const short *y; /* latitudes/longitudes (deg) */
@ -585,14 +619,12 @@ typedef struct { /* IGP band type */
unsigned char bite; /* IGP mask end bit */ unsigned char bite; /* IGP mask end bit */
} sbsigpband_t; } sbsigpband_t;
typedef struct { /* SBAS ionospheric corrections type */ typedef struct { /* SBAS ionospheric corrections type */
int iodi; /* IODI (issue of date ionos corr) */ int iodi; /* IODI (issue of date ionos corr) */
int nigp; /* number of igps */ int nigp; /* number of igps */
sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
} sbsion_t; } sbsion_t;
typedef struct { /* DGPS/GNSS correction type */ typedef struct { /* DGPS/GNSS correction type */
gtime_t t0; /* correction time */ gtime_t t0; /* correction time */
double prc; /* pseudorange correction (PRC) (m) */ double prc; /* pseudorange correction (PRC) (m) */
@ -601,7 +633,6 @@ typedef struct { /* DGPS/GNSS correction type */
double udre; /* UDRE */ double udre; /* UDRE */
} dgps_t; } dgps_t;
typedef struct { /* SSR correction type */ typedef struct { /* SSR correction type */
gtime_t t0[6]; /* epoch time (GPST) {eph,clk,hrclk,ura,bias,pbias} */ gtime_t t0[6]; /* epoch time (GPST) {eph,clk,hrclk,ura,bias,pbias} */
double udi[6]; /* SSR update interval (s) */ 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) */ unsigned char update; /* update flag (0:no update,1:update) */
} ssr_t; } ssr_t;
typedef struct { /* QZSS LEX message type */ typedef struct { /* QZSS LEX message type */
int prn; /* satellite PRN number */ int prn; /* satellite PRN number */
int type; /* message type */ int type; /* message type */
@ -632,13 +662,11 @@ typedef struct { /* QZSS LEX message type */
unsigned char msg[212]; /* LEX message data part 1695 bits */ unsigned char msg[212]; /* LEX message data part 1695 bits */
} lexmsg_t; } lexmsg_t;
typedef struct { /* QZSS LEX messages type */ typedef struct { /* QZSS LEX messages type */
int n,nmax; /* number of LEX messages and allocated */ int n,nmax; /* number of LEX messages and allocated */
lexmsg_t *msgs; /* LEX messages */ lexmsg_t *msgs; /* LEX messages */
} lex_t; } lex_t;
typedef struct { /* QZSS LEX ephemeris type */ typedef struct { /* QZSS LEX ephemeris type */
gtime_t toe; /* epoch time (GPST) */ gtime_t toe; /* epoch time (GPST) */
gtime_t tof; /* message frame time (GPST) */ gtime_t tof; /* message frame time (GPST) */
@ -654,7 +682,6 @@ typedef struct { /* QZSS LEX ephemeris type */
double isc[8]; /* ISC */ double isc[8]; /* ISC */
} lexeph_t; } lexeph_t;
typedef struct { /* QZSS LEX ionosphere correction type */ typedef struct { /* QZSS LEX ionosphere correction type */
gtime_t t0; /* epoch time (GPST) */ gtime_t t0; /* epoch time (GPST) */
double tspan; /* valid time span (s) */ 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) */ double coef[3][2]; /* coefficients lat x lon (3 x 2) */
} lexion_t; } lexion_t;
typedef struct { /* stec data type */ typedef struct { /* stec data type */
gtime_t time; /* time (GPST) */ gtime_t time; /* time (GPST) */
unsigned char sat; /* satellite number */ unsigned char sat; /* satellite number */
@ -672,14 +698,12 @@ typedef struct { /* stec data type */
unsigned char flag; /* fix flag */ unsigned char flag; /* fix flag */
} stec_t; } stec_t;
typedef struct { /* trop data type */ typedef struct { /* trop data type */
gtime_t time; /* time (GPST) */ gtime_t time; /* time (GPST) */
double trp[3]; /* zenith tropos delay/gradient (m) */ double trp[3]; /* zenith tropos delay/gradient (m) */
float std[3]; /* std-dev (m) */ float std[3]; /* std-dev (m) */
} trop_t; } trop_t;
typedef struct { /* ppp corrections type */ typedef struct { /* ppp corrections type */
int nsta; /* number of stations */ int nsta; /* number of stations */
char stas[MAXSTA][8]; /* station names */ char stas[MAXSTA][8]; /* station names */
@ -690,7 +714,6 @@ typedef struct { /* ppp corrections type */
trop_t *trop[MAXSTA]; /* trop data */ trop_t *trop[MAXSTA]; /* trop data */
} pppcorr_t; } pppcorr_t;
typedef struct { /* navigation data type */ typedef struct { /* navigation data type */
int n,nmax; /* number of broadcast ephemeris */ int n,nmax; /* number of broadcast ephemeris */
int ng,ngmax; /* number of glonass ephemeris */ int ng,ngmax; /* number of glonass ephemeris */
@ -738,7 +761,6 @@ typedef struct { /* navigation data type */
pppcorr_t pppcorr; /* ppp corrections */ pppcorr_t pppcorr; /* ppp corrections */
} nav_t; } nav_t;
typedef struct { /* station parameter type */ typedef struct { /* station parameter type */
char name [MAXANT]; /* marker name */ char name [MAXANT]; /* marker name */
char marker [MAXANT]; /* marker number */ char marker [MAXANT]; /* marker number */
@ -755,7 +777,6 @@ typedef struct { /* station parameter type */
double hgt; /* antenna height (m) */ double hgt; /* antenna height (m) */
} sta_t; } sta_t;
typedef struct { /* solution type */ typedef struct { /* solution type */
gtime_t time; /* time (GPST) */ gtime_t time; /* time (GPST) */
double rr[6]; /* position/velocity (m|m/s) */ double rr[6]; /* position/velocity (m|m/s) */
@ -772,7 +793,6 @@ typedef struct { /* solution type */
float thres; /* AR ratio threshold for valiation */ float thres; /* AR ratio threshold for valiation */
} sol_t; } sol_t;
typedef struct { /* solution buffer type */ typedef struct { /* solution buffer type */
int n,nmax; /* number of solution/max number of buffer */ int n,nmax; /* number of solution/max number of buffer */
int cyclic; /* cyclic buffer flag */ int cyclic; /* cyclic buffer flag */
@ -784,7 +804,6 @@ typedef struct { /* solution buffer type */
int nb; /* number of byte in message buffer */ int nb; /* number of byte in message buffer */
} solbuf_t; } solbuf_t;
typedef struct { /* solution status type */ typedef struct { /* solution status type */
gtime_t time; /* time (GPST) */ gtime_t time; /* time (GPST) */
unsigned char sat; /* satellite number */ unsigned char sat; /* satellite number */
@ -800,13 +819,11 @@ typedef struct { /* solution status type */
unsigned short rejc; /* reject counter */ unsigned short rejc; /* reject counter */
} solstat_t; } solstat_t;
typedef struct { /* solution status buffer type */ typedef struct { /* solution status buffer type */
int n,nmax; /* number of solution/max number of buffer */ int n,nmax; /* number of solution/max number of buffer */
solstat_t *data; /* solution status data */ solstat_t *data; /* solution status data */
} solstatbuf_t; } solstatbuf_t;
typedef struct { /* RTCM control struct type */ typedef struct { /* RTCM control struct type */
int staid; /* station id */ int staid; /* station id */
int stah; /* station health */ int stah; /* station health */
@ -838,7 +855,6 @@ typedef struct { /* RTCM control struct type */
char opt[256]; /* RTCM dependent options */ char opt[256]; /* RTCM dependent options */
} rtcm_t; } rtcm_t;
typedef struct { /* download url type */ typedef struct { /* download url type */
char type[32]; /* data type */ char type[32]; /* data type */
char path[1024]; /* url path */ char path[1024]; /* url path */
@ -846,7 +862,6 @@ typedef struct { /* download url type */
double tint; /* time interval (s) */ double tint; /* time interval (s) */
} url_t; } url_t;
typedef struct { /* option type */ typedef struct { /* option type */
const char *name; /* option name */ const char *name; /* option name */
int format; /* option format (0:int,1:double,2:string,3:enum) */ 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 */ const char *comment; /* option comment/enum labels/unit */
} opt_t; } opt_t;
typedef struct { /* extended receiver error model */ typedef struct { /* extended receiver error model */
int ena[4]; /* model enabled */ int ena[4]; /* model enabled */
double cerr[4][NFREQ*2]; /* code errors (m) */ 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) */ double gloicb [NFREQ]; /* glonass interchannel bias (m/fn) */
} exterr_t; } exterr_t;
typedef struct { /* SNR mask type */ typedef struct { /* SNR mask type */
int ena[2]; /* enable flag {rover,base} */ int ena[2]; /* enable flag {rover,base} */
double mask[NFREQ][9]; /* mask (dBHz) at 5,10,...85 deg */ double mask[NFREQ][9]; /* mask (dBHz) at 5,10,...85 deg */
} snrmask_t; } snrmask_t;
typedef struct { /* processing options type */ typedef struct { /* processing options type */
int mode; /* positioning mode (PMODE_???) */ int mode; /* positioning mode (PMODE_???) */
int soltype; /* solution type (0:forward,1:backward,2:combined) */ int soltype; /* solution type (0:forward,1:backward,2:combined) */
@ -932,7 +944,6 @@ typedef struct { /* processing options type */
char pppopt[256]; /* ppp option */ char pppopt[256]; /* ppp option */
} prcopt_t; } prcopt_t;
typedef struct { /* solution options type */ typedef struct { /* solution options type */
int posf; /* solution format (SOLF_???) */ int posf; /* solution format (SOLF_???) */
int times; /* time system (TIMES_???) */ int times; /* time system (TIMES_???) */
@ -955,6 +966,7 @@ typedef struct { /* solution options type */
} solopt_t; } solopt_t;
typedef struct { /* satellite status type */ typedef struct { /* satellite status type */
unsigned char sys; /* navigation system */ unsigned char sys; /* navigation system */
unsigned char vs; /* valid satellite flag single */ 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) */ double ph[2][NFREQ]; /* previous carrier-phase observable (cycle) */
} ssat_t; } ssat_t;
typedef struct { /* ambiguity control type */ typedef struct { /* ambiguity control type */
gtime_t epoch[4]; /* last epoch */ gtime_t epoch[4]; /* last epoch */
int n[4]; /* number of epochs */ int n[4]; /* number of epochs */
@ -988,7 +999,6 @@ typedef struct { /* ambiguity control type */
char flags[MAXSAT]; /* fix flags */ char flags[MAXSAT]; /* fix flags */
} ambc_t; } ambc_t;
typedef struct { /* RTK control/result type */ typedef struct { /* RTK control/result type */
sol_t sol; /* RTK solution */ sol_t sol; /* RTK solution */
double rb[6]; /* base position/velocity (ecef) (m|m/s) */ 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 */ prcopt_t opt; /* processing options */
} rtk_t; } rtk_t;
typedef struct half_cyc_tag { /* half-cycle correction list type */ typedef struct half_cyc_tag { /* half-cycle correction list type */
unsigned char sat; /* satellite number */ unsigned char sat; /* satellite number */
unsigned char freq; /* frequency number (0:L1,1:L2,2:L5) */ unsigned char freq; /* frequency number (0:L1,1:L2,2:L5) */
@ -1027,11 +1036,9 @@ const double chisqr[100] = { /* chi-sqr(n) (alpha=0.001) */
126 ,127 ,128 ,129 ,131 ,132 ,133 ,134 ,135 ,137 , 126 ,127 ,128 ,129 ,131 ,132 ,133 ,134 ,135 ,137 ,
138 ,139 ,140 ,142 ,143 ,144 ,145 ,147 ,148 ,149 138 ,139 ,140 ,142 ,143 ,144 ,145 ,147 ,148 ,149
}; };
const double lam_carr[MAXFREQ]={ /* carrier wave length (m) */ 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, CLIGHT/FREQ1,CLIGHT/FREQ2,CLIGHT/FREQ5,CLIGHT/FREQ6,CLIGHT/FREQ7,
SPEED_OF_LIGHT / FREQ8, SPEED_OF_LIGHT / FREQ9 CLIGHT/FREQ8,CLIGHT/FREQ9
}; };
#endif #endif

6
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@ -62,8 +62,6 @@ obsd_t obs_to_rtklib(Gnss_Synchro gnss_synchro, int week)
//printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec); //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; return rtklib_obs;
} }
eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph) eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph)
{ {
eph_t rtklib_sat; eph_t rtklib_sat;
@ -111,7 +109,6 @@ eph_t eph_to_rtklib(Galileo_Ephemeris gal_eph)
return rtklib_sat; return rtklib_sat;
} }
eph_t eph_to_rtklib(Gps_Ephemeris gps_eph) eph_t eph_to_rtklib(Gps_Ephemeris gps_eph)
{ {
eph_t rtklib_sat; eph_t rtklib_sat;
@ -160,8 +157,6 @@ eph_t eph_to_rtklib(Gps_Ephemeris gps_eph)
return rtklib_sat; return rtklib_sat;
} }
eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph) eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph)
{ {
eph_t rtklib_sat; eph_t rtklib_sat;
@ -211,4 +206,5 @@ eph_t eph_to_rtklib(Gps_CNAV_Ephemeris gps_cnav_eph)
rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow); rtklib_sat.ttr=gpst2time(rtklib_sat.week,tow);
return rtklib_sat; return rtklib_sat;
} }

8
src/algorithms/libs/rtklib/rtklib_conversions.h
View File

@ -47,10 +47,8 @@
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/
#ifndef SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_
#define SRC_ALGORITHMS_PVT_LIBS_RTKLIB_CONVERSIONS_H_
#ifndef GNSS_SDR_RTKLIB_CONVERSIONS_H_
#define GNSS_SDR_RTKLIB_CONVERSIONS_H_
#include "rtklib_rtkcmn.h" #include "rtklib_rtkcmn.h"
#include "gnss_synchro.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); 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_ */

337
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
View File

@ -47,41 +47,69 @@
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/ * history : 2010/07/28 1.1 moved from rtkcmn.c
* added api:
* eph2clk(),geph2clk(),seph2clk(),satantoff()
* satposs()
* changed api:
* eph2pos(),geph2pos(),satpos()
* deleted api:
* satposv(),satposiode()
* 2010/08/26 1.2 add ephemeris option EPHOPT_LEX
* 2010/09/09 1.3 fix problem when precise clock outage
* 2011/01/12 1.4 add api alm2pos()
* change api satpos(),satposs()
* enable valid unhealthy satellites and output status
* fix bug on exception by glonass ephem computation
* 2013/01/10 1.5 support beidou (compass)
* use newton's method to solve kepler eq.
* update ssr correction algorithm
* 2013/03/20 1.6 fix problem on ssr clock relativitic correction
* 2013/09/01 1.7 support negative pseudorange
* fix bug on variance in case of ura ssr = 63
* 2013/11/11 1.8 change constant MAXAGESSR 70.0 -> 90.0
* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15<ura
* 2014/12/07 1.10 modify MAXDTOE for qzss,gal and bds
* test max number of iteration for Kepler
* 2015/08/26 1.11 update RTOL_ELPLER 1E-14 -> 1E-13
* set MAX_ITER_KEPLER for alm2pos()
*-----------------------------------------------------------------------------*/
#include "rtklib_ephemeris.h" #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] */ #define SQR(x) ((x)*(x))
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] */
const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */ #define RE_GLO 6378136.0 /* radius of earth (m) ref [2] */
const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */ #define MU_GPS 3.9860050E14 /* gravitational constant ref [1] */
const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */ #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) */ #define OMGE_GLO 7.292115E-5 /* earth angular velocity (rad/s) ref [2] */
const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */ #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) */ #define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */
const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */ #define COS_5 0.9961946980917456 /* cos(-5.0 deg) */
const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */
const double DEFURASSR = 0.15; /* default accurary of ssr corr (m) */ #define ERREPH_GLO 5.0 /* error of glonass ephemeris (m) */
const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */ #define TSTEP 60.0 /* integration step glonass ephemeris (s) */
const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */ #define RTOL_KEPLER 1E-13 /* relative tolerance for Kepler equation */
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) */
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) --------------------------*/ /* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/
double var_uraeph(int ura) double var_uraeph(int ura)
@ -90,21 +118,17 @@ double var_uraeph(int ura)
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, 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 3072.0,6144.0
}; };
return ura < 0 || 15 < ura ? std::pow(2, 6144.0) : std::pow(2, ura_value[ura]); return ura<0||15<ura?SQR(6144.0):SQR(ura_value[ura]);
} }
/* variance by ura ssr (ref [4]) ---------------------------------------------*/ /* variance by ura ssr (ref [4]) ---------------------------------------------*/
double var_urassr(int ura) double var_urassr(int ura)
{ {
double std; double std;
if (ura <= 0) return std::pow(2, DEFURASSR); if (ura<= 0) return SQR(DEFURASSR);
if (ura >= 63) return std::pow(2, 5.4665); if (ura>=63) return SQR(5.4665);
std = (pow(3.0, (ura >> 3) & 7) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1e-3; std=(pow(3.0,(ura>>3)&7)*(1.0+(ura&7)/4.0)-1.0)*1E-3;
return std::pow(2, std); return SQR(std);
} }
/* almanac to satellite position and clock bias -------------------------------- /* almanac to satellite position and clock bias --------------------------------
* compute satellite position and clock bias with almanac (gps, galileo, qzss) * compute satellite position and clock bias with almanac (gps, galileo, qzss)
* args : gtime_t time I time (gpst) * args : gtime_t time I time (gpst)
@ -123,42 +147,31 @@ void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts)
tk=timediff(time,alm->toa); tk=timediff(time,alm->toa);
if (alm->A <= 0.0) if (alm->A<=0.0) {
{
rs[0]=rs[1]=rs[2]=*dts=0.0; rs[0]=rs[1]=rs[2]=*dts=0.0;
return; return;
} }
mu=satsys(alm->sat,NULL)==SYS_GAL?MU_GAL:MU_GPS; mu=satsys(alm->sat,NULL)==SYS_GAL?MU_GAL:MU_GPS;
M=alm->M0+sqrt(mu/(alm->A*alm->A*alm->A))*tk; 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++) 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));
Ek = E;
E -= (E-alm->e * sin(E) - M) / (1.0 - alm->e * cos(E));
} }
if (n >= MAX_ITER_KEPLER) if (n>=MAX_ITER_KEPLER) {
{
trace(2,"alm2pos: kepler iteration overflow sat=%2d\n",alm->sat); trace(2,"alm2pos: kepler iteration overflow sat=%2d\n",alm->sat);
return; return;
} }
sinE = sin(E); sinE=sin(E); cosE=cos(E);
cosE = cos(E);
u=atan2(sqrt(1.0-alm->e*alm->e)*sinE,cosE-alm->e)+alm->omg; u=atan2(sqrt(1.0-alm->e*alm->e)*sinE,cosE-alm->e)+alm->omg;
r=alm->A*(1.0-alm->e*cosE); r=alm->A*(1.0-alm->e*cosE);
i=alm->i0; i=alm->i0;
O = alm->OMG0 + (alm->OMGd - DEFAULT_OMEGA_EARTH_DOT) * tk - DEFAULT_OMEGA_EARTH_DOT * alm->toas; O=alm->OMG0+(alm->OMGd-OMGE)*tk-OMGE*alm->toas;
x = r * cos(u); x=r*cos(u); y=r*sin(u); sinO=sin(O); cosO=cos(O); cosi=cos(i);
y = r * sin(u);
sinO = sin(O);
cosO = cos(O);
cosi = cos(i);
rs[0]=x*cosO-y*cosi*sinO; rs[0]=x*cosO-y*cosi*sinO;
rs[1]=x*sinO+y*cosi*cosO; rs[1]=x*sinO+y*cosi*cosO;
rs[2]=y*sin(i); rs[2]=y*sin(i);
*dts=alm->f0+alm->f1*tk; *dts=alm->f0+alm->f1*tk;
} }
/* broadcast ephemeris to satellite clock bias --------------------------------- /* broadcast ephemeris to satellite clock bias ---------------------------------
* compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss) * compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss)
* args : gtime_t time I time by satellite clock (gpst) * args : gtime_t time I time by satellite clock (gpst)
@ -176,14 +189,11 @@ double eph2clk(gtime_t time, const eph_t *eph)
t=timediff(time,eph->toc); t=timediff(time,eph->toc);
for (i = 0; i < 2; i++) for (i=0;i<2;i++) {
{
t-=eph->f0+eph->f1*t+eph->f2*t*t; t-=eph->f0+eph->f1*t+eph->f2*t*t;
} }
return 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 -------------------- /* broadcast ephemeris to satellite position and clock bias --------------------
* compute satellite position and clock bias with broadcast ephemeris (gps, * compute satellite position and clock bias with broadcast ephemeris (gps,
* galileo, qzss) * galileo, qzss)
@ -206,33 +216,27 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat); trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat);
if (eph->A <= 0.0) if (eph->A<=0.0) {
{
rs[0]=rs[1]=rs[2]=*dts=*var=0.0; rs[0]=rs[1]=rs[2]=*dts=*var=0.0;
return; return;
} }
tk=timediff(time,eph->toe); tk=timediff(time,eph->toe);
switch ((sys = satsys(eph->sat, &prn))) switch ((sys=satsys(eph->sat,&prn))) {
{
case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break; case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break;
case SYS_BDS: mu = MU_BDS; omge = OMGE_BDS; break; case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break;
default: mu = MU_GPS; omge = DEFAULT_OMEGA_EARTH_DOT; break; default: mu=MU_GPS; omge=OMGE; break;
} }
M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk; 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++) 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));
Ek = E;
E -= (E - eph->e * sin(E) - M) / (1.0 - eph->e * cos(E));
} }
if (n >= MAX_ITER_KEPLER) if (n>=MAX_ITER_KEPLER) {
{
trace(2,"eph2pos: kepler iteration overflow sat=%2d\n",eph->sat); trace(2,"eph2pos: kepler iteration overflow sat=%2d\n",eph->sat);
return; return;
} }
sinE = sin(E); sinE=sin(E); cosE=cos(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); trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek);
@ -243,30 +247,23 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
u+=eph->cus*sin2u+eph->cuc*cos2u; u+=eph->cus*sin2u+eph->cuc*cos2u;
r+=eph->crs*sin2u+eph->crc*cos2u; r+=eph->crs*sin2u+eph->crc*cos2u;
i+=eph->cis*sin2u+eph->cic*cos2u; i+=eph->cis*sin2u+eph->cic*cos2u;
x = r * cos(u); x=r*cos(u); y=r*sin(u); cosi=cos(i);
y = r * sin(u);
cosi = cos(i);
/* beidou geo satellite (ref [9]) */ /* beidou geo satellite (ref [9]) */
if (sys == SYS_BDS && prn <= 5) if (sys==SYS_CMP&&prn<=5) {
{
O=eph->OMG0+eph->OMGd*tk-omge*eph->toes; O=eph->OMG0+eph->OMGd*tk-omge*eph->toes;
sinO = sin(O); sinO=sin(O); cosO=cos(O);
cosO = cos(O);
xg=x*cosO-y*cosi*sinO; xg=x*cosO-y*cosi*sinO;
yg=x*sinO+y*cosi*cosO; yg=x*sinO+y*cosi*cosO;
zg=y*sin(i); zg=y*sin(i);
sino = sin(omge * tk); sino=sin(omge*tk); coso=cos(omge*tk);
coso = cos(omge * tk);
rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5; 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[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5;
rs[2]=-yg*SIN_5+zg*COS_5; rs[2]=-yg*SIN_5+zg*COS_5;
} }
else else {
{
O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes; O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes;
sinO = sin(O); sinO=sin(O); cosO=cos(O);
cosO = cos(O);
rs[0]=x*cosO-y*cosi*sinO; rs[0]=x*cosO-y*cosi*sinO;
rs[1]=x*sinO+y*cosi*cosO; rs[1]=x*sinO+y*cosi*cosO;
rs[2]=y*sin(i); rs[2]=y*sin(i);
@ -275,35 +272,29 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
*dts=eph->f0+eph->f1*tk+eph->f2*tk*tk; *dts=eph->f0+eph->f1*tk+eph->f2*tk*tk;
/* relativity correction */ /* relativity correction */
*dts -= 2.0 * sqrt(mu * eph->A) * eph-> e* sinE / std::pow(2, SPEED_OF_LIGHT); *dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT);
/* position and clock error variance */ /* position and clock error variance */
*var=var_uraeph(eph->sva); *var=var_uraeph(eph->sva);
} }
/* glonass orbit differential equations --------------------------------------*/ /* glonass orbit differential equations --------------------------------------*/
void deq(const double *x, double *xdot, const double *acc) 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); double a,b,c,r2=dot(x,x,3),r3=r2*sqrt(r2),omg2=SQR(OMGE_GLO);
if (r2 <= 0.0) if (r2<=0.0) {
{
xdot[0]=xdot[1]=xdot[2]=xdot[3]=xdot[4]=xdot[5]=0.0; xdot[0]=xdot[1]=xdot[2]=xdot[3]=xdot[4]=xdot[5]=0.0;
return; return;
} }
/* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */ /* 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 */ 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 */ 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) */ c=-MU_GLO/r3-a*(1.0-b); /* -mu/r^3-a(1-b) */
xdot[0] = x[3]; xdot[0]=x[3]; xdot[1]=x[4]; xdot[2]=x[5];
xdot[1] = x[4]; xdot[2] = x[5];
xdot[3]=(c+omg2)*x[0]+2.0*OMGE_GLO*x[4]+acc[0]; 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[4]=(c+omg2)*x[1]-2.0*OMGE_GLO*x[3]+acc[1];
xdot[5]=(c-2.0*a)*x[2]+acc[2]; xdot[5]=(c-2.0*a)*x[2]+acc[2];
} }
/* glonass position and velocity by numerical integration --------------------*/ /* glonass position and velocity by numerical integration --------------------*/
void glorbit(double t, double *x, const double *acc) void glorbit(double t, double *x, const double *acc)
{ {
@ -316,8 +307,6 @@ void glorbit(double t, double *x, const double *acc)
deq(w,k4,acc); 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; 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 ----------------------------------- /* glonass ephemeris to satellite clock bias -----------------------------------
* compute satellite clock bias with glonass ephemeris * compute satellite clock bias with glonass ephemeris
* args : gtime_t time I time by satellite clock (gpst) * args : gtime_t time I time by satellite clock (gpst)
@ -334,14 +323,11 @@ double geph2clk(gtime_t time, const geph_t *geph)
t=timediff(time,geph->toe); t=timediff(time,geph->toe);
for (i = 0; i < 2; i++) for (i=0;i<2;i++) {
{
t-=-geph->taun+geph->gamn*t; t-=-geph->taun+geph->gamn*t;
} }
return -geph->taun+geph->gamn*t; return -geph->taun+geph->gamn*t;
} }
/* glonass ephemeris to satellite position and clock bias ---------------------- /* glonass ephemeris to satellite position and clock bias ----------------------
* compute satellite position and clock bias with glonass ephemeris * compute satellite position and clock bias with glonass ephemeris
* args : gtime_t time I time (gpst) * args : gtime_t time I time (gpst)
@ -364,22 +350,18 @@ void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts,
*dts=-geph->taun+geph->gamn*t; *dts=-geph->taun+geph->gamn*t;
for (i = 0;i<3;i++) for (i=0;i<3;i++) {
{
x[i ]=geph->pos[i]; x[i ]=geph->pos[i];
x[i+3]=geph->vel[i]; x[i+3]=geph->vel[i];
} }
for (tt = t<0.0?-TSTEP:TSTEP;fabs(t)>1E-9;t-=tt) for (tt=t<0.0?-TSTEP:TSTEP;fabs(t)>1E-9;t-=tt) {
{
if (fabs(t)<TSTEP) tt=t; if (fabs(t)<TSTEP) tt=t;
glorbit(tt,x,geph->acc); glorbit(tt,x,geph->acc);
} }
for (i=0;i<3;i++) rs[i]=x[i]; for (i=0;i<3;i++) rs[i]=x[i];
*var = std::pow(2, ERREPH_GLO); *var=SQR(ERREPH_GLO);
} }
/* sbas ephemeris to satellite clock bias -------------------------------------- /* sbas ephemeris to satellite clock bias --------------------------------------
* compute satellite clock bias with sbas ephemeris * compute satellite clock bias with sbas ephemeris
* args : gtime_t time I time by satellite clock (gpst) * args : gtime_t time I time by satellite clock (gpst)
@ -396,14 +378,11 @@ double seph2clk(gtime_t time, const seph_t *seph)
t=timediff(time,seph->t0); t=timediff(time,seph->t0);
for (i = 0; i < 2; i++) for (i=0;i<2;i++) {
{
t-=seph->af0+seph->af1*t; t-=seph->af0+seph->af1*t;
} }
return seph->af0+seph->af1*t; return seph->af0+seph->af1*t;
} }
/* sbas ephemeris to satellite position and clock bias ------------------------- /* sbas ephemeris to satellite position and clock bias -------------------------
* compute satellite position and clock bias with sbas ephemeris * compute satellite position and clock bias with sbas ephemeris
* args : gtime_t time I time (gpst) * args : gtime_t time I time (gpst)
@ -424,16 +403,13 @@ void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts,
t=timediff(time,seph->t0); t=timediff(time,seph->t0);
for (i = 0; i < 3; i++) for (i=0;i<3;i++) {
{
rs[i]=seph->pos[i]+seph->vel[i]*t+seph->acc[i]*t*t/2.0; rs[i]=seph->pos[i]+seph->vel[i]*t+seph->acc[i]*t*t/2.0;
} }
*dts=seph->af0+seph->af1*t; *dts=seph->af0+seph->af1*t;
*var=var_uraeph(seph->sva); *var=var_uraeph(seph->sva);
} }
/* select ephememeris --------------------------------------------------------*/ /* select ephememeris --------------------------------------------------------*/
eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
{ {
@ -442,33 +418,28 @@ eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
trace(4,"seleph : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode); trace(4,"seleph : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
switch (satsys(sat, NULL)) switch (satsys(sat,NULL)) {
{
case SYS_QZS: tmax=MAXDTOE_QZS+1.0; break; case SYS_QZS: tmax=MAXDTOE_QZS+1.0; break;
case SYS_GAL: tmax=MAXDTOE_GAL+1.0; break; case SYS_GAL: tmax=MAXDTOE_GAL+1.0; break;
case SYS_BDS: tmax = MAXDTOE_BDS + 1.0; break; case SYS_CMP: tmax=MAXDTOE_CMP+1.0; break;
default: tmax=MAXDTOE+1.0; break; default: tmax=MAXDTOE+1.0; break;
} }
tmin=tmax+1.0; tmin=tmax+1.0;
for (i = 0; i < nav->n; i++) for (i=0;i<nav->n;i++) {
{
if (nav->eph[i].sat!=sat) continue; if (nav->eph[i].sat!=sat) continue;
if (iode>=0&&nav->eph[i].iode!=iode) continue; if (iode>=0&&nav->eph[i].iode!=iode) continue;
if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue; if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue;
if (iode>=0) return nav->eph+i; if (iode>=0) return nav->eph+i;
if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */
} }
if (iode >= 0 || j<0) if (iode>=0||j<0) {
{
trace(3,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0), trace(3,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0),
sat,iode); sat,iode);
return NULL; return NULL;
} }
return nav->eph+j; return nav->eph+j;
} }
/* select glonass ephememeris ------------------------------------------------*/ /* select glonass ephememeris ------------------------------------------------*/
geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav) geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav)
{ {
@ -477,24 +448,20 @@ geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav)
trace(4,"selgeph : time=%s sat=%2d iode=%2d\n",time_str(time,3),sat,iode); trace(4,"selgeph : time=%s sat=%2d iode=%2d\n",time_str(time,3),sat,iode);
for (i = 0; i < nav->ng; i++) for (i=0;i<nav->ng;i++) {
{
if (nav->geph[i].sat!=sat) continue; if (nav->geph[i].sat!=sat) continue;
if (iode>=0&&nav->geph[i].iode!=iode) continue; if (iode>=0&&nav->geph[i].iode!=iode) continue;
if ((t=fabs(timediff(nav->geph[i].toe,time)))>tmax) continue; if ((t=fabs(timediff(nav->geph[i].toe,time)))>tmax) continue;
if (iode>=0) return nav->geph+i; if (iode>=0) return nav->geph+i;
if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */
} }
if (iode >= 0 || j < 0) if (iode>=0||j<0) {
{
trace(3,"no glonass ephemeris : %s sat=%2d iode=%2d\n",time_str(time,0), trace(3,"no glonass ephemeris : %s sat=%2d iode=%2d\n",time_str(time,0),
sat,iode); sat,iode);
return NULL; return NULL;
} }
return nav->geph+j; return nav->geph+j;
} }
/* select sbas ephememeris ---------------------------------------------------*/ /* select sbas ephememeris ---------------------------------------------------*/
seph_t *selseph(gtime_t time, int sat, const nav_t *nav) seph_t *selseph(gtime_t time, int sat, const nav_t *nav)
{ {
@ -503,21 +470,17 @@ seph_t *selseph(gtime_t time, int sat, const nav_t *nav)
trace(4,"selseph : time=%s sat=%2d\n",time_str(time,3),sat); trace(4,"selseph : time=%s sat=%2d\n",time_str(time,3),sat);
for (i = 0; i < nav->ns; i++) for (i=0;i<nav->ns;i++) {
{
if (nav->seph[i].sat!=sat) continue; if (nav->seph[i].sat!=sat) continue;
if ((t=fabs(timediff(nav->seph[i].t0,time)))>tmax) continue; if ((t=fabs(timediff(nav->seph[i].t0,time)))>tmax) continue;
if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */ if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */
} }
if (j < 0) if (j<0) {
{
trace(3,"no sbas ephemeris : %s sat=%2d\n",time_str(time,0),sat); trace(3,"no sbas ephemeris : %s sat=%2d\n",time_str(time,0),sat);
return NULL; return NULL;
} }
return nav->seph+j; return nav->seph+j;
} }
/* satellite clock with broadcast ephemeris ----------------------------------*/ /* satellite clock with broadcast ephemeris ----------------------------------*/
int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
double *dts) double *dts)
@ -531,18 +494,15 @@ int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
sys=satsys(sat,NULL); sys=satsys(sat,NULL);
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
{
if (!(eph=seleph(teph,sat,-1,nav))) return 0; if (!(eph=seleph(teph,sat,-1,nav))) return 0;
*dts=eph2clk(time,eph); *dts=eph2clk(time,eph);
} }
else if (sys == SYS_GLO) else if (sys==SYS_GLO) {
{
if (!(geph=selgeph(teph,sat,-1,nav))) return 0; if (!(geph=selgeph(teph,sat,-1,nav))) return 0;
*dts=geph2clk(time,geph); *dts=geph2clk(time,geph);
} }
else if (sys == SYS_SBS) else if (sys==SYS_SBS) {
{
if (!(seph=selseph(teph,sat,nav))) return 0; if (!(seph=selseph(teph,sat,nav))) return 0;
*dts=seph2clk(time,seph); *dts=seph2clk(time,seph);
} }
@ -550,8 +510,6 @@ int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
return 1; return 1;
} }
/* satellite position and clock by broadcast ephemeris -----------------------*/ /* satellite position and clock by broadcast ephemeris -----------------------*/
int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, 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)
@ -559,7 +517,7 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
eph_t *eph; eph_t *eph;
geph_t *geph; geph_t *geph;
seph_t *seph; seph_t *seph;
double rst[3], dtst[1], tt = 1e-3; double rst[3],dtst[1],tt=1E-3;
int i,sys; int i,sys;
trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode); trace(4,"ephpos : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
@ -568,8 +526,7 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
*svh=-1; *svh=-1;
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
{
if (!(eph=seleph(teph,sat,iode,nav))) return 0; if (!(eph=seleph(teph,sat,iode,nav))) return 0;
eph2pos(time,eph,rs,dts,var); eph2pos(time,eph,rs,dts,var);
@ -577,16 +534,14 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
eph2pos(time,eph,rst,dtst,var); eph2pos(time,eph,rst,dtst,var);
*svh=eph->svh; *svh=eph->svh;
} }
else if (sys == SYS_GLO) else if (sys==SYS_GLO) {
{
if (!(geph=selgeph(teph,sat,iode,nav))) return 0; if (!(geph=selgeph(teph,sat,iode,nav))) return 0;
geph2pos(time,geph,rs,dts,var); geph2pos(time,geph,rs,dts,var);
time=timeadd(time,tt); time=timeadd(time,tt);
geph2pos(time,geph,rst,dtst,var); geph2pos(time,geph,rst,dtst,var);
*svh=geph->svh; *svh=geph->svh;
} }
else if (sys == SYS_SBS) else if (sys==SYS_SBS) {
{
if (!(seph=selseph(teph,sat,nav))) return 0; if (!(seph=selseph(teph,sat,nav))) return 0;
seph2pos(time,seph,rs,dts,var); seph2pos(time,seph,rs,dts,var);
@ -602,8 +557,6 @@ int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
return 1; return 1;
} }
/* satellite position and clock with sbas correction -------------------------*/ /* satellite position and clock with sbas correction -------------------------*/
int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav, 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)
@ -614,13 +567,11 @@ int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
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 */ /* search sbas satellite correciton */
for (i = 0; i < nav->sbssat.nsat; i++) for (i=0;i<nav->sbssat.nsat;i++) {
{
sbs=nav->sbssat.sat+i; sbs=nav->sbssat.sat+i;
if (sbs->sat==sat) break; if (sbs->sat==sat) break;
} }
if (i >= nav->sbssat.nsat) if (i>=nav->sbssat.nsat) {
{
trace(2,"no sbas correction for orbit: %s sat=%2d\n",time_str(time,0),sat); 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); ephpos(time,teph,sat,nav,-1,rs,dts,var,svh);
*svh=-1; *svh=-1;
@ -634,8 +585,6 @@ int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
*svh=-1; *svh=-1;
return 0; return 0;
} }
/* satellite position and clock with ssr correction --------------------------*/ /* satellite position and clock with ssr correction --------------------------*/
int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, 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)
@ -649,19 +598,16 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
ssr=nav->ssr+sat-1; ssr=nav->ssr+sat-1;
if (!ssr->t0[0].time) if (!ssr->t0[0].time) {
{
trace(2,"no ssr orbit correction: %s sat=%2d\n",time_str(time,0),sat); trace(2,"no ssr orbit correction: %s sat=%2d\n",time_str(time,0),sat);
return 0; return 0;
} }
if (!ssr->t0[1].time) if (!ssr->t0[1].time) {
{
trace(2,"no ssr clock correction: %s sat=%2d\n",time_str(time,0),sat); trace(2,"no ssr clock correction: %s sat=%2d\n",time_str(time,0),sat);
return 0; return 0;
} }
/* inconsistency between orbit and clock correction */ /* inconsistency between orbit and clock correction */
if (ssr->iod[0] != ssr->iod[1]) if (ssr->iod[0]!=ssr->iod[1]) {
{
trace(2,"inconsist ssr correction: %s sat=%2d iod=%d %d\n", trace(2,"inconsist ssr correction: %s sat=%2d iod=%d %d\n",
time_str(time,0),sat,ssr->iod[0],ssr->iod[1]); time_str(time,0),sat,ssr->iod[0],ssr->iod[1]);
*svh=-1; *svh=-1;
@ -672,8 +618,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
t3=timediff(time,ssr->t0[2]); t3=timediff(time,ssr->t0[2]);
/* ssr orbit and clock correction (ref [4]) */ /* ssr orbit and clock correction (ref [4]) */
if (fabs(t1) > MAXAGESSR || fabs(t2) > MAXAGESSR) if (fabs(t1)>MAXAGESSR||fabs(t2)>MAXAGESSR) {
{
trace(2,"age of ssr error: %s sat=%2d t=%.0f %.0f\n",time_str(time,0), trace(2,"age of ssr error: %s sat=%2d t=%.0f %.0f\n",time_str(time,0),
sat,t1,t2); sat,t1,t2);
*svh=-1; *svh=-1;
@ -686,12 +631,10 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
dclk=ssr->dclk[0]+ssr->dclk[1]*t2+ssr->dclk[2]*t2*t2; dclk=ssr->dclk[0]+ssr->dclk[1]*t2+ssr->dclk[2]*t2*t2;
/* ssr highrate clock correction (ref [4]) */ /* ssr highrate clock correction (ref [4]) */
if (ssr->iod[0] == ssr->iod[2] && ssr->t0[2].time && fabs(t3) < MAXAGESSR_HRCLK) if (ssr->iod[0]==ssr->iod[2]&&ssr->t0[2].time&&fabs(t3)<MAXAGESSR_HRCLK) {
{
dclk+=ssr->hrclk; dclk+=ssr->hrclk;
} }
if (norm(deph, 3) > MAXECORSSR || fabs(dclk) > MAXCCORSSR) if (norm(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) {
{
trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n", trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n",
time_str(time,0),norm(deph,3),dclk); time_str(time,0),norm(deph,3),dclk);
*svh=-1; *svh=-1;
@ -702,8 +645,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
/* satellite clock for gps, galileo and qzss */ /* satellite clock for gps, galileo and qzss */
sys=satsys(sat,NULL); sys=satsys(sat,NULL);
if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
{
if (!(eph=seleph(teph,sat,ssr->iode,nav))) return 0; if (!(eph=seleph(teph,sat,ssr->iode,nav))) return 0;
/* satellite clock by clock parameters */ /* satellite clock by clock parameters */
@ -712,29 +654,26 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
dts[1]=eph->f1+2.0*eph->f2*tk; dts[1]=eph->f1+2.0*eph->f2*tk;
/* relativity correction */ /* relativity correction */
dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT; dts[0]-=2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT;
} }
/* radial-along-cross directions in ecef */ /* radial-along-cross directions in ecef */
if (!normv3(rs+3,ea)) return 0; if (!normv3(rs+3,ea)) return 0;
cross3(rs,rs+3,rc); cross3(rs,rs+3,rc);
if (!normv3(rc, ec)) if (!normv3(rc,ec)) {
{
*svh=-1; *svh=-1;
return 0; return 0;
} }
cross3(ea,ec,er); cross3(ea,ec,er);
/* satellite antenna offset correction */ /* satellite antenna offset correction */
if (opt) if (opt) {
{
satantoff(time,rs,sat,nav,dant); satantoff(time,rs,sat,nav,dant);
} }
for (i = 0; i < 3; i++) for (i=0;i<3;i++) {
{
rs[i]+=-(er[i]*deph[0]+ea[i]*deph[1]+ec[i]*deph[2])+dant[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) */ /* t_corr = t_sv - (dts(brdc) + dclk(ssr) / CLIGHT) (ref [10] eq.3.12-7) */
dts[0] += dclk / SPEED_OF_LIGHT; dts[0]+=dclk/CLIGHT;
/* variance by ssr ura */ /* variance by ssr ura */
*var=var_urassr(ssr->ura); *var=var_urassr(ssr->ura);
@ -744,8 +683,6 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
return 1; return 1;
} }
/* satellite position and clock ------------------------------------------------ /* satellite position and clock ------------------------------------------------
* compute satellite position, velocity and clock * compute satellite position, velocity and clock
* args : gtime_t time I time (gpst) * args : gtime_t time I time (gpst)
@ -770,8 +707,7 @@ int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
*svh=0; *svh=0;
switch (ephopt) switch (ephopt) {
{
case EPHOPT_BRDC : return ephpos (time,teph,sat,nav,-1,rs,dts,var,svh); 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_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_SSRAPC: return satpos_ssr (time,teph,sat,nav, 0,rs,dts,var,svh);
@ -785,8 +721,6 @@ int satpos(gtime_t time, gtime_t teph, int sat, int ephopt,
*svh=-1; *svh=-1;
return 0; return 0;
} }
/* satellite positions and clocks ---------------------------------------------- /* satellite positions and clocks ----------------------------------------------
* compute satellite positions, velocities and clocks * compute satellite positions, velocities and clocks
* args : gtime_t teph I time to select ephemeris (gpst) * args : gtime_t teph I time to select ephemeris (gpst)
@ -820,27 +754,23 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
trace(3,"satposs : teph=%s n=%d ephopt=%d\n",time_str(teph,3),n,ephopt); 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 (i=0;i<n&&i<MAXOBS;i++) {
{
for (j=0;j<6;j++) rs [j+i*6]=0.0; for (j=0;j<6;j++) rs [j+i*6]=0.0;
for (j=0;j<2;j++) dts[j+i*2]=0.0; for (j=0;j<2;j++) dts[j+i*2]=0.0;
var[i] = 0.0; var[i]=0.0; svh[i]=0;
svh[i] = 0;
/* search any pseudorange */ /* search any psuedorange */
for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break; for (j=0,pr=0.0;j<NFREQ;j++) if ((pr=obs[i].P[j])!=0.0) break;
if (j >= NFREQ) if (j>=NFREQ) {
{
trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat); trace(2,"no pseudorange %s sat=%2d\n",time_str(obs[i].time,3),obs[i].sat);
continue; continue;
} }
/* transmission time by satellite clock */ /* transmission time by satellite clock */
time[i] = timeadd(obs[i].time, -pr / SPEED_OF_LIGHT); time[i]=timeadd(obs[i].time,-pr/CLIGHT);
/* satellite clock bias by broadcast ephemeris */ /* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i], teph, obs[i].sat, nav, &dt)) 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); trace(3,"no broadcast clock %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
continue; continue;
} }
@ -848,23 +778,20 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
/* satellite position and clock at transmission time */ /* 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, if (!satpos(time[i],teph,obs[i].sat,ephopt,nav,rs+i*6,dts+i*2,var+i,
svh + i)) svh+i)) {
{
trace(3,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat); trace(3,"no ephemeris %s sat=%2d\n",time_str(time[i],3),obs[i].sat);
continue; continue;
} }
/* if no precise clock available, use broadcast clock instead */ /* if no precise clock available, use broadcast clock instead */
if (dts[i * 2] == 0.0) if (dts[i*2]==0.0) {
{
if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue; if (!ephclk(time[i],teph,obs[i].sat,nav,dts+i*2)) continue;
dts[1+i*2]=0.0; dts[1+i*2]=0.0;
*var = std::pow(2, STD_BRDCCLK); *var=SQR(STD_BRDCCLK);
} }
} }
for (i = 0; i < n && i < MAXOBS; i++) 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", 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], 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]); dts[i*2]*1E9,var[i],svh[i]);
} }
} }

34
src/algorithms/libs/rtklib/rtklib_ephemeris.h
View File

@ -47,12 +47,38 @@
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* history : 2010/07/28 1.1 moved from rtkcmn.c
* added api:
* eph2clk(),geph2clk(),seph2clk(),satantoff()
* satposs()
* changed api:
* eph2pos(),geph2pos(),satpos()
* deleted api:
* satposv(),satposiode()
* 2010/08/26 1.2 add ephemeris option EPHOPT_LEX
* 2010/09/09 1.3 fix problem when precise clock outage
* 2011/01/12 1.4 add api alm2pos()
* change api satpos(),satposs()
* enable valid unhealthy satellites and output status
* fix bug on exception by glonass ephem computation
* 2013/01/10 1.5 support beidou (compass)
* use newton's method to solve kepler eq.
* update ssr correction algorithm
* 2013/03/20 1.6 fix problem on ssr clock relativitic correction
* 2013/09/01 1.7 support negative pseudorange
* fix bug on variance in case of ura ssr = 63
* 2013/11/11 1.8 change constant MAXAGESSR 70.0 -> 90.0
* 2014/10/24 1.9 fix bug on return of var_uraeph() if ura<0||15<ura
* 2014/12/07 1.10 modify MAXDTOE for qzss,gal and bds
* test max number of iteration for Kepler
* 2015/08/26 1.11 update RTOL_ELPLER 1E-14 -> 1E-13
* set MAX_ITER_KEPLER for alm2pos()
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
#ifndef GNSS_SDR_RTKLIB_EPHEMERIS_H_ #ifndef RTKLIB_EPHEMERIS_H_
#define GNSS_SDR_RTKLIB_EPHEMERIS_H_ #define RTKLIB_EPHEMERIS_H_
#include "rtklib.h" #include "rtklib.h"
#include "rtklib_rtkcmn.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_ */

210
src/algorithms/libs/rtklib/rtklib_ionex.cc
View File

@ -55,11 +55,14 @@
* Maps based on GPS Carrier Phase Data Routinely producted by CODE * Maps based on GPS Carrier Phase Data Routinely producted by CODE
* Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 * 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" #include "rtklib_ionex.h"
/* get index -----------------------------------------------------------------*/ /* get index -----------------------------------------------------------------*/
int getindex(double value, const double *range) int getindex(double value, const double *range)
{ {
@ -68,23 +71,17 @@ int getindex(double value, const double *range)
if (range[1]<0.0&&(value<range[1]||range[0]<value)) return -1; if (range[1]<0.0&&(value<range[1]||range[0]<value)) return -1;
return (int)floor((value-range[0])/range[2]+0.5); return (int)floor((value-range[0])/range[2]+0.5);
} }
/* get number of items -------------------------------------------------------*/ /* get number of items -------------------------------------------------------*/
int nitem(const double *range) int nitem(const double *range)
{ {
return getindex(range[1],range)+1; return getindex(range[1],range)+1;
} }
/* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/ /* data index (i:lat,j:lon,k:hgt) --------------------------------------------*/
int dataindex(int i, int j, int k, const int *ndata) int dataindex(int i, int j, int k, const int *ndata)
{ {
if (i<0||ndata[0]<=i||j<0||ndata[1]<=j||k<0||ndata[2]<=k) return -1; if (i<0||ndata[0]<=i||j<0||ndata[1]<=j||k<0||ndata[2]<=k) return -1;
return i+ndata[0]*(j+ndata[1]*k); return i+ndata[0]*(j+ndata[1]*k);
} }
/* add tec data to navigation data -------------------------------------------*/ /* add tec data to navigation data -------------------------------------------*/
tec_t *addtec(const double *lats, const double *lons, const double *hgts, tec_t *addtec(const double *lats, const double *lons, const double *hgts,
double rb, nav_t *nav) double rb, nav_t *nav)
@ -100,11 +97,9 @@ tec_t *addtec(const double *lats, const double *lons, const double *hgts,
ndata[2]=nitem(hgts); ndata[2]=nitem(hgts);
if (ndata[0]<=1||ndata[1]<=1||ndata[2]<=0) return NULL; if (ndata[0]<=1||ndata[1]<=1||ndata[2]<=0) return NULL;
if (nav->nt >= nav->ntmax) if (nav->nt>=nav->ntmax) {
{
nav->ntmax+=256; nav->ntmax+=256;
if (!(nav_tec = (tec_t *)realloc(nav->tec, sizeof(tec_t)*nav->ntmax))) if (!(nav_tec=(tec_t *)realloc(nav->tec,sizeof(tec_t)*nav->ntmax))) {
{
trace(1,"readionex malloc error ntmax=%d\n",nav->ntmax); trace(1,"readionex malloc error ntmax=%d\n",nav->ntmax);
free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0; free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0;
return NULL; return NULL;
@ -114,8 +109,7 @@ tec_t *addtec(const double *lats, const double *lons, const double *hgts,
p=nav->tec+nav->nt; p=nav->tec+nav->nt;
p->time=time0; p->time=time0;
p->rb=rb; p->rb=rb;
for (i = 0; i < 3; i++) for (i=0;i<3;i++) {
{
p->ndata[i]=ndata[i]; p->ndata[i]=ndata[i];
p->lats[i]=lats[i]; p->lats[i]=lats[i];
p->lons[i]=lons[i]; p->lons[i]=lons[i];
@ -124,20 +118,16 @@ tec_t *addtec(const double *lats, const double *lons, const double *hgts,
n=ndata[0]*ndata[1]*ndata[2]; n=ndata[0]*ndata[1]*ndata[2];
if (!(p->data=(double *)malloc(sizeof(double)*n))|| if (!(p->data=(double *)malloc(sizeof(double)*n))||
!(p->rms = (float *)malloc(sizeof(float ) * n))) !(p->rms =(float *)malloc(sizeof(float )*n))) {
{
return NULL; return NULL;
} }
for (i = 0; i < n; i++) for (i=0;i<n;i++) {
{
p->data[i]=0.0; p->data[i]=0.0;
p->rms [i]=0.0f; p->rms [i]=0.0f;
} }
nav->nt++; nav->nt++;
return p; return p;
} }
/* read ionex dcb aux data ----------------------------------------------------*/ /* read ionex dcb aux data ----------------------------------------------------*/
void readionexdcb(FILE *fp, double *dcb, double *rms) void readionexdcb(FILE *fp, double *dcb, double *rms)
{ {
@ -148,17 +138,15 @@ void readionexdcb(FILE *fp, double *dcb, double *rms)
for (i=0;i<MAXSAT;i++) dcb[i]=rms[i]=0.0; for (i=0;i<MAXSAT;i++) dcb[i]=rms[i]=0.0;
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff,sizeof(buff),fp)) {
{
if (strlen(buff)<60) continue; if (strlen(buff)<60) continue;
label=buff+60; label=buff+60;
if (strstr(label, "PRN / BIAS / RMS") == label) if (strstr(label,"PRN / BIAS / RMS")==label) {
{
strncpy(id,buff+3,3); id[3]='\0'; strncpy(id,buff+3,3); id[3]='\0';
if (!(sat = satid2no(id))) if (!(sat=satid2no(id))) {
{
trace(2,"ionex invalid satellite: %s\n",id); trace(2,"ionex invalid satellite: %s\n",id);
continue; continue;
} }
@ -168,8 +156,6 @@ void readionexdcb(FILE *fp, double *dcb, double *rms)
else if (strstr(label,"END OF AUX DATA")==label) break; else if (strstr(label,"END OF AUX DATA")==label) break;
} }
} }
/* read ionex header ---------------------------------------------------------*/ /* read ionex header ---------------------------------------------------------*/
double readionexh(FILE *fp, double *lats, double *lons, double *hgts, double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
double *rb, double *nexp, double *dcb, double *rms) double *rb, double *nexp, double *dcb, double *rms)
@ -179,56 +165,45 @@ double readionexh(FILE *fp, double *lats, double *lons, double *hgts,
trace(3,"readionexh:\n"); trace(3,"readionexh:\n");
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff,sizeof(buff),fp)) {
{
if (strlen(buff)<60) continue; if (strlen(buff)<60) continue;
label=buff+60; label=buff+60;
if (strstr(label, "IONEX VERSION / TYPE") == label) if (strstr(label,"IONEX VERSION / TYPE")==label) {
{
if (buff[20]=='I') ver=str2num(buff,0,8); if (buff[20]=='I') ver=str2num(buff,0,8);
} }
else if (strstr(label, "BASE RADIUS") == label) else if (strstr(label,"BASE RADIUS")==label) {
{
*rb=str2num(buff,0,8); *rb=str2num(buff,0,8);
} }
else if (strstr(label, "HGT1 / HGT2 / DHGT") == label) else if (strstr(label,"HGT1 / HGT2 / DHGT")==label) {
{
hgts[0]=str2num(buff, 2,6); hgts[0]=str2num(buff, 2,6);
hgts[1]=str2num(buff, 8,6); hgts[1]=str2num(buff, 8,6);
hgts[2]=str2num(buff,14,6); hgts[2]=str2num(buff,14,6);
} }
else if (strstr(label, "LAT1 / LAT2 / DLAT") == label) else if (strstr(label,"LAT1 / LAT2 / DLAT")==label) {
{
lats[0]=str2num(buff, 2,6); lats[0]=str2num(buff, 2,6);
lats[1]=str2num(buff, 8,6); lats[1]=str2num(buff, 8,6);
lats[2]=str2num(buff,14,6); lats[2]=str2num(buff,14,6);
} }
else if (strstr(label,"LON1 / LON2 / DLON") == label) else if (strstr(label,"LON1 / LON2 / DLON")==label) {
{
lons[0]=str2num(buff, 2,6); lons[0]=str2num(buff, 2,6);
lons[1]=str2num(buff, 8,6); lons[1]=str2num(buff, 8,6);
lons[2]=str2num(buff,14,6); lons[2]=str2num(buff,14,6);
} }
else if (strstr(label, "EXPONENT") == label) else if (strstr(label,"EXPONENT")==label) {
{
*nexp=str2num(buff,0,6); *nexp=str2num(buff,0,6);
} }
else if (strstr(label,"START OF AUX DATA")==label&& else if (strstr(label,"START OF AUX DATA")==label&&
strstr(buff, "DIFFERENTIAL CODE BIASES")) strstr(buff,"DIFFERENTIAL CODE BIASES")) {
{
readionexdcb(fp,dcb,rms); readionexdcb(fp,dcb,rms);
} }
else if (strstr(label, "END OF HEADER") == label) else if (strstr(label,"END OF HEADER")==label) {
{
return ver; return ver;
} }
} }
return 0.0; return 0.0;
} }
/* read ionex body -----------------------------------------------------------*/ /* read ionex body -----------------------------------------------------------*/
int readionexb(FILE *fp, const double *lats, const double *lons, int readionexb(FILE *fp, const double *lats, const double *lons,
const double *hgts, double rb, double nexp, nav_t *nav) const double *hgts, double rb, double nexp, nav_t *nav)
@ -241,40 +216,32 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
trace(3,"readionexb:\n"); trace(3,"readionexb:\n");
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff,sizeof(buff),fp)) {
{
if (strlen(buff)<60) continue; if (strlen(buff)<60) continue;
if (strstr(label, "START OF TEC MAP") == label) if (strstr(label,"START OF TEC MAP")==label) {
{
if ((p=addtec(lats,lons,hgts,rb,nav))) type=1; if ((p=addtec(lats,lons,hgts,rb,nav))) type=1;
} }
else if (strstr(label, "END OF TEC MAP") == label) else if (strstr(label,"END OF TEC MAP")==label) {
{
type=0; type=0;
p=NULL; p=NULL;
} }
else if (strstr(label, "START OF RMS MAP") == label) else if (strstr(label,"START OF RMS MAP")==label) {
{
type=2; type=2;
p=NULL; p=NULL;
} }
else if (strstr(label, "END OF RMS MAP") == label) else if (strstr(label,"END OF RMS MAP")==label) {
{
type=0; type=0;
p=NULL; p=NULL;
} }
else if (strstr(label, "EPOCH OF CURRENT MAP") == label) else if (strstr(label,"EPOCH OF CURRENT MAP")==label) {
{ if (str2time(buff,0,36,&time)) {
if (str2time(buff, 0, 36, &time))
{
trace(2,"ionex epoch invalid: %-36.36s\n",buff); trace(2,"ionex epoch invalid: %-36.36s\n",buff);
continue; continue;
} }
if (type == 2) if (type==2) {
{ for (i=nav->nt-1;i>=0;i--) {
for (i = nav->nt-1; i >= 0; i--)
{
if (fabs(timediff(time,nav->tec[i].time))>=1.0) continue; if (fabs(timediff(time,nav->tec[i].time))>=1.0) continue;
p=nav->tec+i; p=nav->tec+i;
break; break;
@ -282,8 +249,7 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
} }
else if (p) p->time=time; else if (p) p->time=time;
} }
else if (strstr(label, "LAT/LON1/LON2/DLON/H") == label && p) else if (strstr(label,"LAT/LON1/LON2/DLON/H")==label&&p) {
{
lat =str2num(buff, 2,6); lat =str2num(buff, 2,6);
lon[0]=str2num(buff, 8,6); lon[0]=str2num(buff, 8,6);
lon[1]=str2num(buff,14,6); lon[1]=str2num(buff,14,6);
@ -294,8 +260,7 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
k=getindex(hgt,p->hgts); k=getindex(hgt,p->hgts);
n=nitem(lon); n=nitem(lon);
for (m = 0; m < n; m++) for (m=0;m<n;m++) {
{
if (m%16==0&&!fgets(buff,sizeof(buff),fp)) break; if (m%16==0&&!fgets(buff,sizeof(buff),fp)) break;
j=getindex(lon[0]+lon[2]*m,p->lons); j=getindex(lon[0]+lon[2]*m,p->lons);
@ -310,8 +275,6 @@ int readionexb(FILE *fp, const double *lats, const double *lons,
} }
return 1; return 1;
} }
/* combine tec grid data -----------------------------------------------------*/ /* combine tec grid data -----------------------------------------------------*/
void combtec(nav_t *nav) void combtec(nav_t *nav)
{ {
@ -320,22 +283,17 @@ void combtec(nav_t *nav)
trace(3,"combtec : nav->nt=%d\n",nav->nt); trace(3,"combtec : nav->nt=%d\n",nav->nt);
for (i = 0; i < nav->nt - 1; i++) for (i=0;i<nav->nt-1;i++) {
{ for (j=i+1;j<nav->nt;j++) {
for (j = i + 1; j < nav->nt; j++) if (timediff(nav->tec[j].time,nav->tec[i].time)<0.0) {
{
if (timediff(nav->tec[j].time, nav->tec[i].time) < 0.0)
{
tmp=nav->tec[i]; tmp=nav->tec[i];
nav->tec[i]=nav->tec[j]; nav->tec[i]=nav->tec[j];
nav->tec[j]=tmp; nav->tec[j]=tmp;
} }
} }
} }
for (i = 0; i < nav->nt; i++) for (i=0;i<nav->nt;i++) {
{ if (i>0&&timediff(nav->tec[i].time,nav->tec[n-1].time)==0.0) {
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].data);
free(nav->tec[n-1].rms ); free(nav->tec[n-1].rms );
nav->tec[n-1]=nav->tec[i]; nav->tec[n-1]=nav->tec[i];
@ -347,8 +305,6 @@ void combtec(nav_t *nav)
trace(4,"combtec : nav->nt=%d\n",nav->nt); trace(4,"combtec : nav->nt=%d\n",nav->nt);
} }
/* read ionex tec grid file ---------------------------------------------------- /* read ionex tec grid file ----------------------------------------------------
* read ionex ionospheric tec grid file * read ionex ionospheric tec grid file
* args : char *file I ionex tec grid file * args : char *file I ionex tec grid file
@ -370,16 +326,11 @@ void readtec(const char *file, nav_t *nav, int opt)
trace(3,"readtec : file=%s\n",file); trace(3,"readtec : file=%s\n",file);
/* clear of tec grid data option */ /* clear of tec grid data option */
if (!opt) if (!opt) {
{ free(nav->tec); nav->tec=NULL; nav->nt=nav->ntmax=0;
free(nav->tec);
nav->tec = NULL;
nav->nt = nav->ntmax = 0;
} }
for (i = 0; i < MAXEXFILE; i++) for (i=0;i<MAXEXFILE;i++) {
{ if (!(efiles[i]=(char *)malloc(1024))) {
if (!(efiles[i] = (char *)malloc(1024)))
{
for (i--;i>=0;i--) free(efiles[i]); for (i--;i>=0;i--) free(efiles[i]);
return; return;
} }
@ -387,16 +338,13 @@ void readtec(const char *file, nav_t *nav, int opt)
/* expand wild card in file path */ /* expand wild card in file path */
n=expath(file,efiles,MAXEXFILE); n=expath(file,efiles,MAXEXFILE);
for (i = 0; i < n; i++) for (i=0;i<n;i++) {
{ if (!(fp=fopen(efiles[i],"r"))) {
if (!(fp = fopen(efiles[i], "r")))
{
trace(2,"ionex file open error %s\n",efiles[i]); trace(2,"ionex file open error %s\n",efiles[i]);
continue; continue;
} }
/* read ionex header */ /* read ionex header */
if (readionexh(fp, lats, lons, hgts, &rb, &nexp, dcb, rms) <= 0.0) if (readionexh(fp,lats,lons,hgts,&rb,&nexp,dcb,rms)<=0.0) {
{
trace(2,"ionex file format error %s\n",efiles[i]); trace(2,"ionex file format error %s\n",efiles[i]);
continue; continue;
} }
@ -411,13 +359,10 @@ void readtec(const char *file, nav_t *nav, int opt)
if (nav->nt>0) combtec(nav); if (nav->nt>0) combtec(nav);
/* P1-P2 dcb */ /* P1-P2 dcb */
for (i = 0; i < MAXSAT; i++) for (i=0;i<MAXSAT;i++) {
{ nav->cbias[i][0]=CLIGHT*dcb[i]*1E-9; /* ns->m */
nav->cbias[i][0] = SPEED_OF_LIGHT * dcb[i] * 1e-9; /* ns->m */
} }
} }
/* interpolate tec grid data -------------------------------------------------*/ /* interpolate tec grid data -------------------------------------------------*/
int interptec(const tec_t *tec, int k, const double *posp, double *value, int interptec(const tec_t *tec, int k, const double *posp, double *value,
double *rms) double *rms)
@ -437,20 +382,17 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
a=dlat/tec->lats[2]; a=dlat/tec->lats[2];
b=dlon/tec->lons[2]; b=dlon/tec->lons[2];
i = (int)floor(a); i=(int)floor(a); a-=i;
a -= i; j=(int)floor(b); b-=j;
j = (int)floor(b);
b -= j;
/* get gridded tec data */ /* get gridded tec data */
for (n = 0; n < 4; n++) for (n=0;n<4;n++) {
{
if ((index=dataindex(i+(n%2),j+(n<2?0:1),k,tec->ndata))<0) continue; if ((index=dataindex(i+(n%2),j+(n<2?0:1),k,tec->ndata))<0) continue;
d[n]=tec->data[index]; d[n]=tec->data[index];
r[n]=tec->rms [index]; r[n]=tec->rms [index];
} }
if (d[0] > 0.0 && d[1] > 0.0 && d[2] > 0.0 && d[3] > 0.0) if (d[0]>0.0&&d[1]>0.0&&d[2]>0.0&&d[3]>0.0) {
{
/* bilinear interpolation (inside of grid) */ /* 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]; *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]; *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];
@ -460,18 +402,14 @@ int interptec(const tec_t *tec, int k, const double *posp, double *value,
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[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[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 if (a> 0.5&&b> 0.5&&d[3]>0.0) {*value=d[3]; *rms=r[3];}
else else {
{
i=0; i=0;
for (n=0;n<4;n++) if (d[n]>0.0) {i++; *value+=d[n]; *rms+=r[n];} for (n=0;n<4;n++) if (d[n]>0.0) {i++; *value+=d[n]; *rms+=r[n];}
if(i==0) return 0; if(i==0) return 0;
*value /= i; *value/=i; *rms/=i;
*rms /= i;
} }
return 1; return 1;
} }
/* ionosphere delay by tec grid data -----------------------------------------*/ /* ionosphere delay by tec grid data -----------------------------------------*/
int iondelay(gtime_t time, const tec_t *tec, const double *pos, 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)
@ -485,21 +423,19 @@ int iondelay(gtime_t time, const tec_t *tec, const double *pos,
*delay=*var=0.0; *delay=*var=0.0;
for (i = 0;i < tec->ndata[2]; i++) for (i=0;i<tec->ndata[2];i++) { /* for a layer */
{ /* for a layer */
hion=tec->hgts[0]+tec->hgts[2]*i; hion=tec->hgts[0]+tec->hgts[2]*i;
/* ionospheric pierce point position */ /* ionospheric pierce point position */
fs=ionppp(pos,azel,tec->rb,hion,posp); fs=ionppp(pos,azel,tec->rb,hion,posp);
if (opt&2) if (opt&2) {
{
/* modified single layer mapping function (M-SLM) ref [2] */ /* modified single layer mapping function (M-SLM) ref [2] */
rp=tec->rb/(tec->rb+hion)*sin(0.9782*(PI/2.0-azel[1])); rp=tec->rb/(tec->rb+hion)*sin(0.9782*(PI/2.0-azel[1]));
fs=1.0/sqrt(1.0-rp*rp); fs=1.0/sqrt(1.0-rp*rp);
} }
if (opt&1) if (opt&1) {
{
/* earth rotation correction (sun-fixed coordinate) */ /* earth rotation correction (sun-fixed coordinate) */
posp[1]+=2.0*PI*timediff(time,tec->time)/86400.0; posp[1]+=2.0*PI*timediff(time,tec->time)/86400.0;
} }
@ -513,8 +449,6 @@ int iondelay(gtime_t time, const tec_t *tec, const double *pos,
return 1; return 1;
} }
/* ionosphere model by tec grid data ------------------------------------------- /* ionosphere model by tec grid data -------------------------------------------
* compute ionospheric delay by tec grid data * compute ionospheric delay by tec grid data
* args : gtime_t time I time (gpst) * args : gtime_t time I time (gpst)
@ -539,23 +473,19 @@ int iontec(gtime_t time, const nav_t *nav, const double *pos,
trace(3,"iontec : time=%s pos=%.1f %.1f azel=%.1f %.1f\n",time_str(time,0), 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); pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);
if (azel[1] < MIN_EL || pos[2] < MIN_HGT) if (azel[1]<MIN_EL||pos[2]<MIN_HGT) {
{
*delay=0.0; *delay=0.0;
*var=VAR_NOTEC; *var=VAR_NOTEC;
return 1; return 1;
} }
for (i = 0; i < nav->nt; i++) for (i=0;i<nav->nt;i++) {
{
if (timediff(nav->tec[i].time,time)>0.0) break; if (timediff(nav->tec[i].time,time)>0.0) break;
} }
if (i == 0 || i >= nav->nt) if (i==0||i>=nav->nt) {
{
trace(2,"%s: tec grid out of period\n",time_str(time,0)); trace(2,"%s: tec grid out of period\n",time_str(time,0));
return 0; return 0;
} }
if ((tt = timediff(nav->tec[i].time, nav->tec[i-1].time)) == 0.0) if ((tt=timediff(nav->tec[i].time,nav->tec[i-1].time))==0.0) {
{
trace(2,"tec grid time interval error\n"); trace(2,"tec grid time interval error\n");
return 0; return 0;
} }
@ -563,25 +493,21 @@ int iontec(gtime_t time, const nav_t *nav, const double *pos,
stat[0]=iondelay(time,nav->tec+i-1,pos,azel,opt,dels ,vars ); 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); stat[1]=iondelay(time,nav->tec+i ,pos,azel,opt,dels+1,vars+1);
if (!stat[0] && !stat[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", 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); time_str(time,0),pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D);
return 0; return 0;
} }
if (stat[0] && stat[1]) if (stat[0]&&stat[1]) { /* linear interpolation by time */
{ /* linear interpolation by time */
a=timediff(time,nav->tec[i-1].time)/tt; a=timediff(time,nav->tec[i-1].time)/tt;
*delay=dels[0]*(1.0-a)+dels[1]*a; *delay=dels[0]*(1.0-a)+dels[1]*a;
*var =vars[0]*(1.0-a)+vars[1]*a; *var =vars[0]*(1.0-a)+vars[1]*a;
} }
else if (stat[0]) else if (stat[0]) { /* nearest-neighbour extrapolation by time */
{ /* nearest-neighbour extrapolation by time */
*delay=dels[0]; *delay=dels[0];
*var =vars[0]; *var =vars[0];
} }
else else {
{
*delay=dels[1]; *delay=dels[1];
*var =vars[1]; *var =vars[1];
} }

23
src/algorithms/libs/rtklib/rtklib_ionex.h
View File

@ -1,5 +1,5 @@
/*! /*!
* \file rtklib_ionex.h * \file rtklib_ionex.cc
* \brief ionex functions * \brief ionex functions
* \authors <ul> * \authors <ul>
* <li> 2007-2013, T. Takasu * <li> 2007-2013, T. Takasu
@ -48,23 +48,28 @@
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
* *
* References: * references:
* [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange * [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange
* Format Version 1, February 25, 1998 * Format Version 1, February 25, 1998
* [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere * [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 * Maps based on GPS Carrier Phase Data Routinely producted by CODE
* Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 * 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_
#ifndef GNSS_SDR_RTKLIB_IONEX_H_ #define RTKLIB_IONEX_H_
#define GNSS_SDR_RTKLIB_IONEX_H_
#include "rtklib_rtkcmn.h" #include "rtklib_rtkcmn.h"
const double VAR_NOTEC = 30.0 * 30.0; /* variance of no tec */ #define SQR(x) ((x)*(x))
const double MIN_EL = 0.0; /* min elevation angle (rad) */ #define VAR_NOTEC SQR(30.0) /* variance of no tec */
const double MIN_HGT = -1000.0; /* min user height (m) */ #define MIN_EL 0.0 /* min elevation angle (rad) */
#define MIN_HGT -1000.0 /* min user height (m) */
int getindex(double value, const double *range); int 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, int iontec(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, int opt, double *delay, double *var); const double *azel, int opt, double *delay, double *var);
#endif /* GNSS_SDR_RTKLIB_IONEX_H_ */ #endif /* SRC_ALGORITHMS_PVT_LIBS_RTKLIB_IONEX_H_ */

288
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -47,9 +47,19 @@
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * 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" #include "rtklib_pntpos.h"
@ -58,25 +68,20 @@ double varerr(const prcopt_t *opt, double el, int sys)
{ {
double fact,varr; double fact,varr;
fact=sys==SYS_GLO?EFACT_GLO:(sys==SYS_SBS?EFACT_SBS:EFACT_GPS); 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)); varr=SQR(opt->err[0])*(SQR(opt->err[1])+SQR(opt->err[2])/sin(el));
if (opt->ionoopt == IONOOPT_IFLC) varr *= std::pow(2, 3.0); /* iono-free */ if (opt->ionoopt==IONOOPT_IFLC) varr*=SQR(3.0); /* iono-free */
return std::pow(2, fact) * varr; return SQR(fact)*varr;
} }
/* get tgd parameter (m) -----------------------------------------------------*/ /* get tgd parameter (m) -----------------------------------------------------*/
double gettgd(int sat, const nav_t *nav) double gettgd(int sat, const nav_t *nav)
{ {
int i; int i;
for (i = 0; i < nav->n; i++) for (i=0;i<nav->n;i++) {
{
if (nav->eph[i].sat!=sat) continue; if (nav->eph[i].sat!=sat) continue;
return SPEED_OF_LIGHT * nav->eph[i].tgd[0]; return CLIGHT*nav->eph[i].tgd[0];
} }
return 0.0; return 0.0;
} }
/* psendorange with code bias correction -------------------------------------*/ /* psendorange with code bias correction -------------------------------------*/
double prange(const obsd_t *obs, const nav_t *nav, const double *azel, 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)
@ -105,16 +110,13 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
} }
/* test snr mask */ /* test snr mask */
if (iter>0) if (iter>0) {
{ if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) {
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", 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); time_str(obs->time,0),obs->sat,azel[1]*R2D,obs->SNR[i]*0.25);
return 0.0; return 0.0;
} }
if (opt->ionoopt == IONOOPT_IFLC) if (opt->ionoopt==IONOOPT_IFLC) {
{
if (testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) if (testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask))
{ {
trace(4,"prange: testsnr error\n"); trace(4,"prange: testsnr error\n");
@ -122,7 +124,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
} }
} }
} }
gamma = std::pow(2, lam[j]) / std::pow(2, lam[i]); /* f1^2/f2^2 */ gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */
P1=obs->P[i]; P1=obs->P[i];
P2=obs->P[j]; P2=obs->P[j];
P1_P2=nav->cbias[obs->sat-1][0]; P1_P2=nav->cbias[obs->sat-1][0];
@ -130,12 +132,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
P2_C2=nav->cbias[obs->sat-1][2]; P2_C2=nav->cbias[obs->sat-1][2];
/* if no P1-P2 DCB, use TGD instead */ /* if no P1-P2 DCB, use TGD instead */
if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) if (P1_P2==0.0&&(sys&(SYS_GPS|SYS_GAL|SYS_QZS))) {
{
P1_P2=(1.0-gamma)*gettgd(obs->sat,nav); P1_P2=(1.0-gamma)*gettgd(obs->sat,nav);
} }
if (opt->ionoopt == IONOOPT_IFLC) if (opt->ionoopt==IONOOPT_IFLC) { /* dual-frequency */
{ /* dual-frequency */
if (P1==0.0||P2==0.0) return 0.0; if (P1==0.0||P2==0.0) return 0.0;
if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
@ -144,8 +144,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
/* iono-free combination */ /* iono-free combination */
PC=(gamma*P1-P2)/(gamma-1.0); PC=(gamma*P1-P2)/(gamma-1.0);
} }
else else { /* single-frequency */
{ /* single-frequency */
if (P1==0.0) return 0.0; if (P1==0.0) return 0.0;
if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */ if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
@ -153,12 +152,10 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
} }
if (opt->sateph==EPHOPT_SBAS) PC-=P1_C1; /* sbas clock based C1 */ if (opt->sateph==EPHOPT_SBAS) PC-=P1_C1; /* sbas clock based C1 */
*var = std::pow(2, ERR_CBIAS); *var=SQR(ERR_CBIAS);
return PC; return PC;
} }
/* ionospheric correction ------------------------------------------------------ /* ionospheric correction ------------------------------------------------------
* compute ionospheric correction * compute ionospheric correction
* args : gtime_t time I time * args : gtime_t time I time
@ -179,27 +176,23 @@ int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
azel[1]*R2D); azel[1]*R2D);
/* broadcast model */ /* broadcast model */
if (ionoopt == IONOOPT_BRDC) if (ionoopt==IONOOPT_BRDC) {
{
*ion=ionmodel(time,nav->ion_gps,pos,azel); *ion=ionmodel(time,nav->ion_gps,pos,azel);
*var=SQR(*ion*ERR_BRDCI); *var=SQR(*ion*ERR_BRDCI);
return 1; return 1;
} }
/* sbas ionosphere model */ /* sbas ionosphere model */
if (ionoopt == IONOOPT_SBAS) if (ionoopt==IONOOPT_SBAS) {
{
return sbsioncorr(time,nav,pos,azel,ion,var); return sbsioncorr(time,nav,pos,azel,ion,var);
} }
/* ionex tec model */ /* ionex tec model */
if (ionoopt == IONOOPT_TEC) if (ionoopt==IONOOPT_TEC) {
{
return iontec(time,nav,pos,azel,1,ion,var); return iontec(time,nav,pos,azel,1,ion,var);
} }
/* qzss broadcast model */ /* qzss broadcast model */
if (ionoopt == IONOOPT_QZS && norm(nav->ion_qzs, 8)>0.0) if (ionoopt==IONOOPT_QZS&&norm(nav->ion_qzs,8)>0.0) {
{
*ion=ionmodel(time,nav->ion_qzs,pos,azel); *ion=ionmodel(time,nav->ion_qzs,pos,azel);
*var = std::pow(2, *ion * ERR_BRDCI); *var=SQR(*ion*ERR_BRDCI);
return 1; return 1;
} }
/* lex ionosphere model */ /* lex ionosphere model */
@ -207,11 +200,9 @@ int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos,
// return lexioncorr(time,nav,pos,azel,ion,var); // return lexioncorr(time,nav,pos,azel,ion,var);
//} //}
*ion=0.0; *ion=0.0;
*var = ionoopt == IONOOPT_OFF ? std::pow(2, ERR_ION) : 0.0; *var=ionoopt==IONOOPT_OFF?SQR(ERR_ION):0.0;
return 1; return 1;
} }
/* tropospheric correction ----------------------------------------------------- /* tropospheric correction -----------------------------------------------------
* compute tropospheric correction * compute tropospheric correction
* args : gtime_t time I time * args : gtime_t time I time
@ -231,25 +222,21 @@ int tropcorr(gtime_t time, const nav_t *nav, const double *pos,
azel[1]*R2D); azel[1]*R2D);
/* saastamoinen model */ /* saastamoinen model */
if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG) if (tropopt==TROPOPT_SAAS||tropopt==TROPOPT_EST||tropopt==TROPOPT_ESTG) {
{
*trp=tropmodel(time,pos,azel,REL_HUMI); *trp=tropmodel(time,pos,azel,REL_HUMI);
*var=SQR(ERR_SAAS/(sin(azel[1])+0.1)); *var=SQR(ERR_SAAS/(sin(azel[1])+0.1));
return 1; return 1;
} }
/* sbas troposphere model */ /* sbas troposphere model */
if (tropopt == TROPOPT_SBAS) if (tropopt==TROPOPT_SBAS) {
{
*trp=sbstropcorr(time,pos,azel,var); *trp=sbstropcorr(time,pos,azel,var);
return 1; return 1;
} }
/* no correction */ /* no correction */
*trp=0.0; *trp=0.0;
*var = tropopt == TROPOPT_OFF ? std::pow(2, ERR_TROP) : 0.0; *var=tropopt==TROPOPT_OFF?SQR(ERR_TROP):0.0;
return 1; return 1;
} }
/* pseudorange residuals -----------------------------------------------------*/ /* pseudorange residuals -----------------------------------------------------*/
int rescode(int iter, const obsd_t *obs, int n, const double *rs, int rescode(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh, const double *dts, const double *vare, const int *svh,
@ -262,28 +249,25 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
trace(3,"resprng : n=%d\n",n); trace(3,"resprng : n=%d\n",n);
for (i = 0; i < 3; i++) rr[i] = x[i]; for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3];
dtr = x[3];
ecef2pos(rr,pos); ecef2pos(rr,pos);
for (i = *ns = 0; i < n && i < MAXOBS; i++) for (i=*ns=0;i<n&&i<MAXOBS;i++) {
{ vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;
vsat[i] = 0;
azel[i*2] = azel[1+i*2] = resp[i] = 0.0;
if (!(sys=satsys(obs[i].sat,NULL))) continue; if (!(sys=satsys(obs[i].sat,NULL))) continue;
/* reject duplicated observation data */ /* reject duplicated observation data */
if (i < n - 1 && i < MAXOBS - 1 && obs[i].sat == obs[i+1].sat) if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
{
trace(2,"duplicated observation data %s sat=%2d\n", trace(2,"duplicated observation data %s sat=%2d\n",
time_str(obs[i].time,3),obs[i].sat); time_str(obs[i].time,3),obs[i].sat);
i++; i++;
continue; continue;
} }
/* geometric distance/azimuth/elevation angle */ /* geometric distance/azimuth/elevation angle */
if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || satazel(pos, e, azel + i * 2) < opt->elmin) if ((r=geodist(rs+i*6,rr,e))<=0.0||
satazel(pos,e,azel+i*2)<opt->elmin)
{ {
trace(4,"geodist / satazel error\n"); trace(4,"geodist / satazel error\n");
continue; continue;
@ -312,19 +296,17 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
} }
/* GPS-L1 -> L1/B1 */ /* GPS-L1 -> L1/B1 */
if ((lam_L1 = nav->lam[obs[i].sat - 1][0]) > 0.0) if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) {
{ dion*=SQR(lam_L1/lam_carr[0]);
dion *= pow(2, lam_L1 / lam_carr[0]);
} }
/* tropospheric corrections */ /* tropospheric corrections */
if (!tropcorr(obs[i].time,nav,pos,azel+i*2, if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
iter > 0 ? opt->tropopt : TROPOPT_SAAS, &dtrp, &vtrp)) iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
{
trace(4,"tropocorr error\n"); trace(4,"tropocorr error\n");
continue; continue;
} }
/* pseudorange residual */ /* pseudorange residual */
v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i*2] + dion + dtrp); v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);
/* design matrix */ /* design matrix */
for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0); for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);
@ -332,12 +314,10 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
/* time system and receiver bias offset correction */ /* time system and receiver bias offset correction */
if (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;} 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_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 if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
else mask[0]=1; else mask[0]=1;
vsat[i] = 1; vsat[i]=1; resp[i]=v[nv]; (*ns)++;
resp[i] = v[nv];
(*ns)++;
/* error variance */ /* error variance */
var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp; var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
@ -346,8 +326,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1])); azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));
} }
/* constraint to avoid rank-deficient */ /* constraint to avoid rank-deficient */
for (i = 0; i < 4; i++) for (i=0;i<4;i++) {
{
if (mask[i]) continue; if (mask[i]) continue;
v[nv]=0.0; v[nv]=0.0;
for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0; for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
@ -355,8 +334,6 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
} }
return nv; return nv;
} }
/* validate solution ---------------------------------------------------------*/ /* validate solution ---------------------------------------------------------*/
int valsol(const double *azel, const int *vsat, int n, int valsol(const double *azel, const int *vsat, int n,
const prcopt_t *opt, const double *v, int nv, int nx, const prcopt_t *opt, const double *v, int nv, int nx,
@ -369,29 +346,24 @@ int valsol(const double *azel, const int *vsat, int n,
/* chi-square validation of residuals */ /* chi-square validation of residuals */
vv=dot(v,v,nv); vv=dot(v,v,nv);
if (nv > nx && vv > chisqr[nv-nx-1]) 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]); sprintf(msg,"chi-square error nv=%d vv=%.1f cs=%.1f",nv,vv,chisqr[nv-nx-1]);
return 0; return 0;
} }
/* large gdop check */ /* large gdop check */
for (i = ns = 0; i < n; i++) for (i=ns=0;i<n;i++) {
{
if (!vsat[i]) continue; if (!vsat[i]) continue;
azels[ ns*2]=azel[ i*2]; azels[ ns*2]=azel[ i*2];
azels[1+ns*2]=azel[1+i*2]; azels[1+ns*2]=azel[1+i*2];
ns++; ns++;
} }
dops(ns,azels,opt->elmin,dop); dops(ns,azels,opt->elmin,dop);
if (dop[0] <= 0.0 || dop[0] > opt->maxgdop) if (dop[0]<=0.0||dop[0]>opt->maxgdop) {
{
sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]); sprintf(msg,"gdop error nv=%d gdop=%.1f",nv,dop[0]);
return 0; return 0;
} }
return 1; return 1;
} }
/* estimate receiver position ------------------------------------------------*/ /* estimate receiver position ------------------------------------------------*/
int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, 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 double *vare, const int *svh, const nav_t *nav,
@ -403,45 +375,40 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
trace(3,"estpos : n=%d\n",n); trace(3,"estpos : n=%d\n",n);
v = mat(n + 4, 1); v=mat(n+4,1); H=mat(NX,n+4); var=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<3;i++) x[i]=sol->rr[i];
for (i = 0; i < MAXITR; 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) /* 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); sprintf(msg,"lack of valid sats ns=%d",nv);
break; break;
} }
/* weight by variance */ /* weight by variance */
for (j = 0;j < nv; j++) for (j=0;j<nv;j++) {
{
sig=sqrt(var[j]); sig=sqrt(var[j]);
v[j]/=sig; v[j]/=sig;
for (k=0;k<NX;k++) H[k+j*NX]/=sig; for (k=0;k<NX;k++) H[k+j*NX]/=sig;
} }
/* least square estimation */ /* least square estimation */
if ((info = lsq(H, v, NX, nv, dx, Q))) if ((info=lsq(H,v,NX,nv,dx,Q))) {
{
sprintf(msg,"lsq error info=%d",info); sprintf(msg,"lsq error info=%d",info);
break; break;
} }
for (j=0;j<NX;j++) x[j]+=dx[j]; for (j=0;j<NX;j++) x[j]+=dx[j];
if (norm(dx, NX) < 1e-4) if (norm(dx,NX)<1E-4) {
{
sol->type=0; sol->type=0;
sol->time = timeadd(obs[0].time, -x[3] / SPEED_OF_LIGHT); sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
sol->dtr[0] = x[3] / SPEED_OF_LIGHT; /* receiver clock bias (s) */ sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */ sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */ sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-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<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]; for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
sol->qr[3]=(float)Q[1]; /* cov xy */ sol->qr[3]=(float)Q[1]; /* cov xy */
@ -451,27 +418,20 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
sol->age=sol->ratio=0.0; sol->age=sol->ratio=0.0;
/* validate solution */ /* validate solution */
if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
{
sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE; sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
} }
free(v); free(v); free(H); free(var);
free(H);
free(var);
return stat; return stat;
} }
} }
if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i); if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
free(v); free(v); free(H); free(var);
free(H);
free(var);
return 0; return 0;
} }
/* raim fde (failure detection and exclution) -------------------------------*/ /* raim fde (failure detection and exclution) -------------------------------*/
int raim_fde(const obsd_t *obs, int n, const double *rs, int raim_fde(const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh, const double *dts, const double *vare, const int *svh,
@ -487,19 +447,13 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
trace(3,"raim_fde: %s n=%2d\n",time_str(obs[0].time,0),n); 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; if (!(obs_e=(obsd_t *)malloc(sizeof(obsd_t)*n))) return 0;
rs_e = mat(6, n); rs_e = mat(6,n); dts_e = mat(2,n); vare_e=mat(1,n); azel_e=zeros(2,n);
dts_e = mat(2, n); svh_e=imat(1,n); vsat_e=imat(1,n); resp_e=mat(1,n);
vare_e = mat(1, n);
azel_e = zeros(2, n); for (i=0;i<n;i++) {
svh_e = imat(1, n);
vsat_e = imat(1, n);
resp_e = mat(1, n);
for (i = 0; i < n; i++)
{
/* satellite exclution */ /* satellite exclution */
for (j = k = 0; j < n; j++) for (j=k=0;j<n;j++) {
{
if (j==i) continue; if (j==i) continue;
obs_e[k]=obs[j]; obs_e[k]=obs[j];
matcpy(rs_e +6*k,rs +6*j,6,1); matcpy(rs_e +6*k,rs +6*j,6,1);
@ -509,19 +463,16 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
} }
/* estimate receiver position without a satellite */ /* 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, 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)) vsat_e,resp_e,msg_e)) {
{
trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg); trace(3,"raim_fde: exsat=%2d (%s)\n",obs[i].sat,msg);
continue; continue;
} }
for (j = nvsat = 0, rms_e = 0.0; j < n - 1; j++) for (j=nvsat=0,rms_e=0.0;j<n-1;j++) {
{
if (!vsat_e[j]) continue; if (!vsat_e[j]) continue;
rms_e += std::pow(2, resp_e[j]); rms_e+=SQR(resp_e[j]);
nvsat++; nvsat++;
} }
if (nvsat < 5) if (nvsat<5) {
{
trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n", trace(3,"raim_fde: exsat=%2d lack of satellites nvsat=%2d\n",
obs[i].sat,nvsat); obs[i].sat,nvsat);
continue; continue;
@ -533,8 +484,7 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
if (rms_e>rms) continue; if (rms_e>rms) continue;
/* save result */ /* save result */
for (j = k = 0; j < n; j++) for (j=k=0;j<n;j++) {
{
if (j==i) continue; if (j==i) continue;
matcpy(azel+2*j,azel_e+2*k,2,1); matcpy(azel+2*j,azel_e+2*k,2,1);
vsat[j]=vsat_e[k]; vsat[j]=vsat_e[k];
@ -547,24 +497,15 @@ int raim_fde(const obsd_t *obs, int n, const double *rs,
vsat[i]=0; vsat[i]=0;
strcpy(msg,msg_e); strcpy(msg,msg_e);
} }
if (stat) if (stat) {
{
time2str(obs[0].time,tstr,2); satno2id(sat,name); time2str(obs[0].time,tstr,2); satno2id(sat,name);
trace(2,"%s: %s excluded by raim\n",tstr+11,name); trace(2,"%s: %s excluded by raim\n",tstr+11,name);
} }
free(obs_e); free(obs_e);
free(rs_e); free(rs_e ); free(dts_e ); free(vare_e); free(azel_e);
free(dts_e); free(svh_e); free(vsat_e); free(resp_e);
free(vare_e);
free(azel_e);
free(svh_e);
free(vsat_e);
free(resp_e);
return stat; return stat;
} }
/* doppler residuals ---------------------------------------------------------*/ /* doppler residuals ---------------------------------------------------------*/
int resdop(const obsd_t *obs, int n, const double *rs, const double *dts, int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
const nav_t *nav, const double *rr, const double *x, const nav_t *nav, const double *rr, const double *x,
@ -575,15 +516,13 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
trace(3,"resdop : n=%d\n",n); trace(3,"resdop : n=%d\n",n);
ecef2pos(rr, pos); ecef2pos(rr,pos); xyz2enu(pos,E);
xyz2enu(pos, E);
for (i=0;i<n&&i<MAXOBS;i++) {
for (i = 0; i < n && i < MAXOBS; i++)
{
lam=nav->lam[obs[i].sat-1][0]; lam=nav->lam[obs[i].sat-1][0];
if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm(rs + 3 + i * 6, 3) <= 0.0) if (obs[i].D[0]==0.0||lam==0.0||!vsat[i]||norm(rs+3+i*6,3)<=0.0) {
{
continue; continue;
} }
/* line-of-sight vector in ecef */ /* line-of-sight vector in ecef */
@ -597,11 +536,11 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j]; for (j=0;j<3;j++) vs[j]=rs[j+3+i*6]-x[j];
/* range rate with earth rotation correction */ /* 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]- 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]); rs[3+i*6]*rr[1]-rs[ i*6]*x[1]);
/* doppler residual */ /* doppler residual */
v[nv] =- lam * obs[i].D[0] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i *2]); v[nv]=-lam*obs[i].D[0]-(rate+x[3]-CLIGHT*dts[1+i*2]);
/* design matrix */ /* design matrix */
for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0; for (j=0;j<4;j++) H[j+nv*4]=j<3?-e[j]:1.0;
@ -610,8 +549,6 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
} }
return nv; return nv;
} }
/* estimate receiver velocity ------------------------------------------------*/ /* estimate receiver velocity ------------------------------------------------*/
void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, 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 nav_t *nav, const prcopt_t *opt, sol_t *sol,
@ -622,14 +559,12 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
trace(3,"estvel : n=%d\n",n); trace(3,"estvel : n=%d\n",n);
v = mat(n, 1); v=mat(n,1); H=mat(4,n);
H = mat(4, n);
for (i=0;i<MAXITR;i++) {
for (i = 0; i < MAXITR; i++)
{
/* doppler residuals */ /* doppler residuals */
if ((nv = resdop(obs, n, rs, dts, nav, sol->rr, x, azel, vsat, v, H)) < 4) if ((nv=resdop(obs,n,rs,dts,nav,sol->rr,x,azel,vsat,v,H))<4) {
{
break; break;
} }
/* least square estimation */ /* least square estimation */
@ -637,17 +572,13 @@ void estvel(const obsd_t *obs, int n, const double *rs, const double *dts,
for (j=0;j<4;j++) x[j]+=dx[j]; for (j=0;j<4;j++) x[j]+=dx[j];
if (norm(dx, 4) < 1e-6) if (norm(dx,4)<1E-6) {
{
for (i=0;i<3;i++) sol->rr[i+3]=x[i]; for (i=0;i<3;i++) sol->rr[i+3]=x[i];
break; break;
} }
} }
free(v); free(v); free(H);
free(H);
} }
/* single-point positioning ---------------------------------------------------- /* single-point positioning ----------------------------------------------------
* compute receiver position, velocity, clock bias by single-point positioning * compute receiver position, velocity, clock bias by single-point positioning
* with pseudorange and doppler observables * with pseudorange and doppler observables
@ -725,17 +656,11 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
sol->time = obs[0].time; sol->time=obs[0].time; msg[0]='\0';
msg[0] = '\0';
rs = mat(6, n); rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
dts = mat(2, n);
var = mat(1, n);
azel_ = zeros(2, n);
resp = mat(1, n);
if (opt_.mode != PMODE_SINGLE) if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
{ /* for precise positioning */
#if 0 #if 0
opt_.sateph =EPHOPT_BRDC; opt_.sateph =EPHOPT_BRDC;
#endif #endif
@ -749,28 +674,23 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
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 */ /* raim fde */
if (!stat && n >= 6 && opt->posopt[4]) if (!stat&&n>=6&&opt->posopt[4]) {
{
stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
} }
/* estimate receiver velocity with doppler */ /* estimate receiver velocity with doppler */
if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
if (azel) if (azel) {
{
for (i=0;i<n*2;i++) azel[i]=azel_[i]; for (i=0;i<n*2;i++) azel[i]=azel_[i];
} }
if (ssat) if (ssat) {
{ for (i=0;i<MAXSAT;i++) {
for (i = 0; i < MAXSAT; i++)
{
ssat[i].vs=0; ssat[i].vs=0;
ssat[i].azel[0]=ssat[i].azel[1]=0.0; ssat[i].azel[0]=ssat[i].azel[1]=0.0;
ssat[i].resp[0]=ssat[i].resc[0]=0.0; ssat[i].resp[0]=ssat[i].resc[0]=0.0;
ssat[i].snr[0]=0; ssat[i].snr[0]=0;
} }
for (i = 0; i < n; i++) for (i=0;i<n;i++) {
{
ssat[obs[i].sat-1].azel[0]=azel_[ i*2]; ssat[obs[i].sat-1].azel[0]=azel_[ i*2];
ssat[obs[i].sat-1].azel[1]=azel_[1+i*2]; ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0]; ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
@ -779,10 +699,6 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
ssat[obs[i].sat-1].resp[0]=resp[i]; ssat[obs[i].sat-1].resp[0]=resp[i];
} }
} }
free(rs); free(rs); free(dts); free(var); free(azel_); free(resp);
free(dts);
free(var);
free(azel_);
free(resp);
return stat; return stat;
} }

18
src/algorithms/libs/rtklib/rtklib_pntpos.h
View File

@ -48,10 +48,22 @@
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
* *
* history : 2010/07/28 1.0 moved from rtkcmn.c
* changed api:
* pntpos()
* deleted api:
* pntvel()
* 2011/01/12 1.1 add option to include unhealthy satellite
* reject duplicated observation data
* changed api: ionocorr()
* 2011/11/08 1.2 enable snr mask for single-mode (rtklib_2.4.1_p3)
* 2012/12/25 1.3 add variable snr mask
* 2014/05/26 1.4 support galileo and beidou
* 2015/03/19 1.5 fix bug on ionosphere correction for GLO and BDS
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
#ifndef GNSS_SDR_RTKLIB_PNTPOS_H_ #ifndef RTKLIB_PNTPOS_H_
#define GNSS_SDR_RTKLIB_PNTPOS_H_ #define RTKLIB_PNTPOS_H_
#include "rtklib.h" #include "rtklib.h"
#include "rtklib_rtkcmn.h" #include "rtklib_rtkcmn.h"
@ -165,4 +177,4 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
char *msg); char *msg);
#endif /* GNSS_SDR_RTKLIB_PNTPOS_H_ */ #endif /* RTKLIB_PNTPOS_H_ */

361
src/algorithms/libs/rtklib/rtklib_preceph.cc
View File

@ -49,7 +49,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
* *
* *
* References : * references :
* [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c), * [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c),
* 12 February, 2007 * 12 February, 2007
* [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information, * [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information,
@ -58,6 +58,34 @@
* [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed, * [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
* Space Technology Library, 2004 * Space Technology Library, 2004
* *
* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
* history : 2009/01/18 1.0 new
* 2009/01/31 1.1 fix bug on numerical error to read sp3a ephemeris
* 2009/05/15 1.2 support glonass,galileo,qzs
* 2009/12/11 1.3 support wild-card expansion of file path
* 2010/07/21 1.4 added api:
* eci2ecef(),sunmoonpos(),peph2pos(),satantoff(),
* readdcb()
* changed api:
* readsp3()
* deleted api:
* eph2posp()
* 2010/09/09 1.5 fix problem when precise clock outage
* 2011/01/23 1.6 support qzss satellite code
* 2011/09/12 1.7 fix problem on precise clock outage
* move sunmmonpos() to rtkcmn.c
* 2011/12/01 1.8 modify api readsp3()
* precede later ephemeris if ephemeris is NULL
* move eci2ecef() to rtkcmn.c
* 2013/05/08 1.9 fix bug on computing std-dev of precise clocks
* 2013/11/20 1.10 modify option for api readsp3()
* 2014/04/03 1.11 accept extenstion including sp3,eph,SP3,EPH
* 2014/05/23 1.12 add function to read sp3 velocity records
* change api: satantoff()
* 2014/08/31 1.13 add member cov and vco in peph_t sturct
* 2014/10/13 1.14 fix bug on clock error variance in peph2pos()
* 2015/05/10 1.15 add api readfcb()
* modify api readdcb()
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
#include "rtklib_preceph.h" #include "rtklib_preceph.h"
@ -69,12 +97,10 @@ int code2sys(char code)
if (code=='R') return SYS_GLO; if (code=='R') return SYS_GLO;
if (code=='E') return SYS_GAL; /* extension to sp3-c */ if (code=='E') return SYS_GAL; /* extension to sp3-c */
if (code=='J') return SYS_QZS; /* extension to sp3-c */ if (code=='J') return SYS_QZS; /* extension to sp3-c */
if (code == 'C') return SYS_BDS; /* extension to sp3-c */ if (code=='C') return SYS_CMP; /* extension to sp3-c */
if (code=='L') return SYS_LEO; /* extension to sp3-c */ if (code=='L') return SYS_LEO; /* extension to sp3-c */
return SYS_NONE; return SYS_NONE;
} }
/* read sp3 header -----------------------------------------------------------*/ /* read sp3 header -----------------------------------------------------------*/
int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats, int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
double *bfact, char *tsys) double *bfact, char *tsys)
@ -84,56 +110,43 @@ int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
trace(3,"readsp3h:\n"); trace(3,"readsp3h:\n");
for (i = 0;i<22;i++) for (i=0;i<22;i++) {
{
if (!fgets(buff,sizeof(buff),fp)) break; if (!fgets(buff,sizeof(buff),fp)) break;
if (i == 0) if (i==0) {
{
*type=buff[2]; *type=buff[2];
if (str2time(buff,3,28,time)) return 0; if (str2time(buff,3,28,time)) return 0;
} }
else if (2<=i && i<=6) else if (2<=i&&i<=6) {
{ if (i==2) {
if (i == 2)
{
ns=(int)str2num(buff,4,2); ns=(int)str2num(buff,4,2);
} }
for (j = 0;j<17 && k<ns;j++) for (j=0;j<17&&k<ns;j++) {
{
sys=code2sys(buff[9+3*j]); sys=code2sys(buff[9+3*j]);
prn=(int)str2num(buff,10+3*j,2); prn=(int)str2num(buff,10+3*j,2);
if (k<MAXSAT) sats[k++]=satno(sys,prn); if (k<MAXSAT) sats[k++]=satno(sys,prn);
} }
} }
else if (i == 12) else if (i==12) {
{
strncpy(tsys,buff+9,3); tsys[3]='\0'; strncpy(tsys,buff+9,3); tsys[3]='\0';
} }
else if (i == 14) else if (i==14) {
{
bfact[0]=str2num(buff, 3,10); bfact[0]=str2num(buff, 3,10);
bfact[1]=str2num(buff,14,12); bfact[1]=str2num(buff,14,12);
} }
} }
return ns; return ns;
} }
/* add precise ephemeris -----------------------------------------------------*/ /* add precise ephemeris -----------------------------------------------------*/
int addpeph(nav_t *nav, peph_t *peph) int addpeph(nav_t *nav, peph_t *peph)
{ {
peph_t *nav_peph; peph_t *nav_peph;
if (nav->ne >= nav->nemax) if (nav->ne>=nav->nemax) {
{
nav->nemax+=256; nav->nemax+=256;
if (!(nav_peph = (peph_t *)realloc(nav->peph, sizeof(peph_t)*nav->nemax))) if (!(nav_peph=(peph_t *)realloc(nav->peph,sizeof(peph_t)*nav->nemax))) {
{
trace(1,"readsp3b malloc error n=%d\n",nav->nemax); trace(1,"readsp3b malloc error n=%d\n",nav->nemax);
free(nav->peph); free(nav->peph); nav->peph=NULL; nav->ne=nav->nemax=0;
nav->peph = NULL;
nav->ne = nav->nemax = 0;
return 0; return 0;
} }
nav->peph=nav_peph; nav->peph=nav_peph;
@ -141,8 +154,6 @@ int addpeph(nav_t *nav, peph_t *peph)
nav->peph[nav->ne++]=*peph; nav->peph[nav->ne++]=*peph;
return 1; return 1;
} }
/* read sp3 body -------------------------------------------------------------*/ /* read sp3 body -------------------------------------------------------------*/
void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, 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)
@ -155,12 +166,11 @@ void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact,
trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt); trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt);
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff,sizeof(buff),fp)) {
{
if (!strncmp(buff,"EOF",3)) break; if (!strncmp(buff,"EOF",3)) break;
if (buff[0] != '*' || str2time(buff, 3, 28, &time)) if (buff[0]!='*'||str2time(buff,3,28,&time)) {
{
trace(2,"sp3 invalid epoch %31.31s\n",buff); trace(2,"sp3 invalid epoch %31.31s\n",buff);
continue; continue;
} }
@ -168,23 +178,20 @@ void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact,
peph.time =time; peph.time =time;
peph.index=index; peph.index=index;
for (i = 0; i < MAXSAT; i++) for (i=0;i<MAXSAT;i++) {
{ for (j=0;j<4;j++) {
for (j = 0; j < 4; j++)
{
peph.pos[i][j]=0.0; peph.pos[i][j]=0.0;
peph.std[i][j]=0.0f; peph.std[i][j]=0.0f;
peph.vel[i][j]=0.0; peph.vel[i][j]=0.0;
peph.vst[i][j]=0.0f; peph.vst[i][j]=0.0f;
} }
for (j = 0; j < 3; j++) for (j=0;j<3;j++) {
{
peph.cov[i][j]=0.0f; peph.cov[i][j]=0.0f;
peph.vco[i][j]=0.0f; peph.vco[i][j]=0.0f;
} }
} }
for (i = pred_o = pred_c = v = 0;i<n && fgets(buff, sizeof(buff), fp);i++) for (i=pred_o=pred_c=v=0;i<n&&fgets(buff,sizeof(buff),fp);i++) {
{
if (strlen(buff)<4||(buff[0]!='P'&&buff[0]!='V')) continue; if (strlen(buff)<4||(buff[0]!='P'&&buff[0]!='V')) continue;
sys=buff[1]==' '?SYS_GPS:code2sys(buff[1]); sys=buff[1]==' '?SYS_GPS:code2sys(buff[1]);
@ -194,13 +201,12 @@ void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact,
if (!(sat=satno(sys,prn))) continue; if (!(sat=satno(sys,prn))) continue;
if (buff[0] == 'P') if (buff[0]=='P') {
{
pred_c=strlen(buff)>=76&&buff[75]=='P'; pred_c=strlen(buff)>=76&&buff[75]=='P';
pred_o=strlen(buff)>=80&&buff[79]=='P'; pred_o=strlen(buff)>=80&&buff[79]=='P';
} }
for (j = 0;j<4;j++) for (j=0;j<4;j++) {
{
/* read option for predicted value */ /* read option for predicted value */
if (j< 3&&(opt&1)&& pred_o) continue; if (j< 3&&(opt&1)&& pred_o) continue;
if (j< 3&&(opt&2)&&!pred_o) continue; if (j< 3&&(opt&2)&&!pred_o) continue;
@ -211,45 +217,36 @@ void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact,
std=str2num(buff,61+j* 3,j<3?2:3); std=str2num(buff,61+j* 3,j<3?2:3);
if (buff[0]=='P') { /* position */ if (buff[0]=='P') { /* position */
if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6) if (val!=0.0&&fabs(val-999999.999999)>=1E-6) {
{ peph.pos[sat-1][j]=val*(j<3?1000.0:1E-6);
peph.pos[sat-1][j] = val*(j<3?1000.0:1e-6);
v=1; /* valid epoch */ v=1; /* valid epoch */
} }
if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0) 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));
peph.std[sat-1][j] = (float)(pow(base, std)*(j < 3 ? 1e-3 : 1e-12));
} }
} }
else if (v) { /* velocity */ else if (v) { /* velocity */
if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6) if (val!=0.0&&fabs(val-999999.999999)>=1E-6) {
{ peph.vel[sat-1][j]=val*(j<3?0.1:1E-10);
peph.vel[sat-1][j] = val*( j < 3 ? 0.1 : 1e-10);
} }
if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0) 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));
peph.vst[sat-1][j] = (float)(pow(base, std)*(j < 3 ? 1e-7 : 1e-16));
} }
} }
} }
} }
if (v) if (v) {
{
if (!addpeph(nav,&peph)) return; if (!addpeph(nav,&peph)) return;
} }
} }
} }
/* compare precise ephemeris -------------------------------------------------*/ /* 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; peph_t *q1=(peph_t *)p1,*q2=(peph_t *)p2;
double tt=timediff(q1->time,q2->time); double tt=timediff(q1->time,q2->time);
return tt < -1e-9 ? -1 : (tt > 1e-9 ? 1 : q1->index - q2->index); return tt<-1E-9?-1:(tt>1E-9?1:q1->index-q2->index);
} }
/* combine precise ephemeris -------------------------------------------------*/ /* combine precise ephemeris -------------------------------------------------*/
void combpeph(nav_t *nav, int opt) void combpeph(nav_t *nav, int opt)
{ {
@ -261,12 +258,11 @@ void combpeph(nav_t *nav, int opt)
if (opt&4) return; if (opt&4) return;
for (i = 0, j = 1; j < nav->ne; j++) for (i=0,j=1;j<nav->ne;j++) {
{
if (fabs(timediff(nav->peph[i].time, nav->peph[j].time)) < 1e-9) if (fabs(timediff(nav->peph[i].time,nav->peph[j].time))<1E-9) {
{
for (k = 0; k < MAXSAT; k++) for (k=0;k<MAXSAT;k++) {
{
if (norm(nav->peph[j].pos[k],4)<=0.0) continue; 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].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].std[k][m]=nav->peph[j].std[k][m];
@ -280,8 +276,6 @@ void combpeph(nav_t *nav, int opt)
trace(4,"combpeph: ne=%d\n",nav->ne); trace(4,"combpeph: ne=%d\n",nav->ne);
} }
/* read sp3 precise ephemeris file --------------------------------------------- /* read sp3 precise ephemeris file ---------------------------------------------
* read sp3 precise ephemeris/clock files and set them to navigation data * read sp3 precise ephemeris/clock files and set them to navigation data
* args : char *file I sp3-c precise ephemeris file * args : char *file I sp3-c precise ephemeris file
@ -306,10 +300,8 @@ void readsp3(const char *file, nav_t *nav, int opt)
trace(3,"readpephs: file=%s\n",file); trace(3,"readpephs: file=%s\n",file);
for (i = 0; i < MAXEXFILE; i++) for (i=0;i<MAXEXFILE;i++) {
{ if (!(efiles[i]=(char *)malloc(1024))) {
if (!(efiles[i] = (char *)malloc(1024)))
{
for (i--;i>=0;i--) free(efiles[i]); for (i--;i>=0;i--) free(efiles[i]);
return; return;
} }
@ -317,15 +309,13 @@ void readsp3(const char *file, nav_t *nav, int opt)
/* expand wild card in file path */ /* expand wild card in file path */
n=expath(file,efiles,MAXEXFILE); n=expath(file,efiles,MAXEXFILE);
for (i = j = 0; i < n; i++) for (i=j=0;i<n;i++) {
{
if (!(ext=strrchr(efiles[i],'.'))) continue; if (!(ext=strrchr(efiles[i],'.'))) continue;
if (!strstr(ext+1,"sp3")&&!strstr(ext+1,".SP3")&& if (!strstr(ext+1,"sp3")&&!strstr(ext+1,".SP3")&&
!strstr(ext+1,"eph")&&!strstr(ext+1,".EPH")) continue; !strstr(ext+1,"eph")&&!strstr(ext+1,".EPH")) continue;
if (!(fp = fopen(efiles[i], "r"))) if (!(fp=fopen(efiles[i],"r"))) {
{
trace(2,"sp3 file open error %s\n",efiles[i]); trace(2,"sp3 file open error %s\n",efiles[i]);
continue; continue;
} }
@ -342,8 +332,6 @@ void readsp3(const char *file, nav_t *nav, int opt)
/* combine precise ephemeris */ /* combine precise ephemeris */
if (nav->ne>0) combpeph(nav,opt); if (nav->ne>0) combpeph(nav,opt);
} }
/* read satellite antenna parameters ------------------------------------------- /* read satellite antenna parameters -------------------------------------------
* read satellite antenna parameters * read satellite antenna parameters
* args : char *file I antenna parameter file * args : char *file I antenna parameter file
@ -362,16 +350,13 @@ int readsap(const char *file, gtime_t time, nav_t *nav)
if (!readpcv(file,&pcvs)) return 0; if (!readpcv(file,&pcvs)) return 0;
for (i = 0; i < MAXSAT; i++) for (i=0;i<MAXSAT;i++) {
{
pcv=searchpcv(i+1,"",time,&pcvs); pcv=searchpcv(i+1,"",time,&pcvs);
nav->pcvs[i]=pcv?*pcv:pcv0; nav->pcvs[i]=pcv?*pcv:pcv0;
} }
free(pcvs.pcv); free(pcvs.pcv);
return 1; return 1;
} }
/* read dcb parameters file --------------------------------------------------*/ /* read dcb parameters file --------------------------------------------------*/
int readdcbf(const char *file, nav_t *nav, const sta_t *sta) int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
{ {
@ -382,13 +367,12 @@ int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
trace(3,"readdcbf: file=%s\n",file); trace(3,"readdcbf: file=%s\n",file);
if (!(fp = fopen(file, "r"))) if (!(fp=fopen(file,"r"))) {
{
trace(2,"dcb parameters file open error: %s\n",file); trace(2,"dcb parameters file open error: %s\n",file);
return 0; return 0;
} }
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff,sizeof(buff),fp)) {
{
if (strstr(buff,"DIFFERENTIAL (P1-P2) CODE BIASES")) type=1; 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 (P1-C1) CODE BIASES")) type=2;
else if (strstr(buff,"DIFFERENTIAL (P2-C2) CODE BIASES")) type=3; else if (strstr(buff,"DIFFERENTIAL (P2-C2) CODE BIASES")) type=3;
@ -397,29 +381,23 @@ int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
if ((cbias=str2num(buff,26,9))==0.0) continue; if ((cbias=str2num(buff,26,9))==0.0) continue;
if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R"))) if (sta&&(!strcmp(str1,"G")||!strcmp(str1,"R"))) { /* receiver dcb */
{ /* receiver dcb */ for (i=0;i<MAXRCV;i++) {
for (i = 0; i < MAXRCV; i++)
{
if (!strcmp(sta[i].name,str2)) break; if (!strcmp(sta[i].name,str2)) break;
} }
if (i < MAXRCV) if (i<MAXRCV) {
{
j=!strcmp(str1,"G")?0:1; j=!strcmp(str1,"G")?0:1;
nav->rbias[i][j][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */ nav->rbias[i][j][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */
} }
} }
else if ((sat = satid2no(str1))) else if ((sat=satid2no(str1))) { /* satellite dcb */
{ /* satellite dcb */ nav->cbias[sat-1][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */
nav->cbias[sat-1][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
} }
} }
fclose(fp); fclose(fp);
return 1; return 1;
} }
/* read dcb parameters --------------------------------------------------------- /* read dcb parameters ---------------------------------------------------------
* read differential code bias (dcb) parameters * read differential code bias (dcb) parameters
* args : char *file I dcb parameters file (wild-card * expanded) * args : char *file I dcb parameters file (wild-card * expanded)
@ -436,30 +414,24 @@ int readdcb(const char *file, nav_t *nav, const sta_t *sta)
trace(3,"readdcb : file=%s\n",file); trace(3,"readdcb : file=%s\n",file);
for (i = 0;i < MAXSAT; i++) for (j = 0; j < 3; j++) for (i=0;i<MAXSAT;i++) for (j=0;j<3;j++) {
{
nav->cbias[i][j]=0.0; nav->cbias[i][j]=0.0;
} }
for (i = 0; i < MAXEXFILE; i++) for (i=0;i<MAXEXFILE;i++) {
{ if (!(efiles[i]=(char *)malloc(1024))) {
if (!(efiles[i] = (char *)malloc(1024)))
{
for (i--;i>=0;i--) free(efiles[i]); for (i--;i>=0;i--) free(efiles[i]);
return 0; return 0;
} }
} }
n=expath(file,efiles,MAXEXFILE); n=expath(file,efiles,MAXEXFILE);
for (i = 0; i < n; i++) for (i=0;i<n;i++) {
{
readdcbf(efiles[i],nav,sta); readdcbf(efiles[i],nav,sta);
} }
for (i=0;i<MAXEXFILE;i++) free(efiles[i]); for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
return 1; return 1;
} }
/* add satellite fcb ---------------------------------------------------------*/ /* add satellite fcb ---------------------------------------------------------*/
int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat, int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
const double *bias, const double *std) const double *bias, const double *std)
@ -467,32 +439,25 @@ int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
fcbd_t *nav_fcb; fcbd_t *nav_fcb;
int i,j; int i,j;
if (nav->nf > 0 && fabs(timediff(ts, nav->fcb[nav->nf-1].ts)) <= 1e-3) if (nav->nf>0&&fabs(timediff(ts,nav->fcb[nav->nf-1].ts))<=1e-3) {
{ for (i=0;i<3;i++) {
for (i = 0; i < 3; i++)
{
nav->fcb[nav->nf-1].bias[sat-1][i]=bias[i]; nav->fcb[nav->nf-1].bias[sat-1][i]=bias[i];
nav->fcb[nav->nf-1].std [sat-1][i]=std [i]; nav->fcb[nav->nf-1].std [sat-1][i]=std [i];
} }
return 1; return 1;
} }
if (nav->nf >= nav->nfmax) if (nav->nf>=nav->nfmax) {
{
nav->nfmax=nav->nfmax<=0?2048:nav->nfmax*2; nav->nfmax=nav->nfmax<=0?2048:nav->nfmax*2;
if (!(nav_fcb = (fcbd_t *)realloc(nav->fcb, sizeof(fcbd_t)*nav->nfmax))) if (!(nav_fcb=(fcbd_t *)realloc(nav->fcb,sizeof(fcbd_t)*nav->nfmax))) {
{ free(nav->fcb); nav->nf=nav->nfmax=0;
free(nav->fcb);
nav->nf = nav->nfmax = 0;
return 0; return 0;
} }
nav->fcb=nav_fcb; nav->fcb=nav_fcb;
} }
for (i = 0; i < MAXSAT; i++) for (j = 0; j < 3; j++) 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; nav->fcb[nav->nf].bias[i][j]=nav->fcb[nav->nf].std[i][j]=0.0;
} }
for (i = 0; i < 3; i++) for (i=0;i<3;i++) {
{
nav->fcb[nav->nf].bias[sat-1][i]=bias[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].std [sat-1][i]=std [i];
} }
@ -500,8 +465,6 @@ int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
nav->fcb[nav->nf++].te=te; nav->fcb[nav->nf++].te=te;
return 1; return 1;
} }
/* read satellite fcb file ---------------------------------------------------*/ /* read satellite fcb file ---------------------------------------------------*/
int readfcbf(const char *file, nav_t *nav) int readfcbf(const char *file, nav_t *nav)
{ {
@ -513,13 +476,11 @@ int readfcbf(const char *file, nav_t *nav)
trace(3,"readfcbf: file=%s\n",file); trace(3,"readfcbf: file=%s\n",file);
if (!(fp = fopen(file, "r"))) if (!(fp=fopen(file,"r"))) {
{
trace(2,"fcb parameters file open error: %s\n",file); trace(2,"fcb parameters file open error: %s\n",file);
return 0; return 0;
} }
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff,sizeof(buff),fp)) {
{
if ((p=strchr(buff,'#'))) *p='\0'; if ((p=strchr(buff,'#'))) *p='\0';
if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s" 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, "%lf %lf %lf %lf %lf %lf",ep1,ep1+1,ep1+2,ep1+3,ep1+4,ep1+5,
@ -533,17 +494,13 @@ int readfcbf(const char *file, nav_t *nav)
fclose(fp); fclose(fp);
return 1; return 1;
} }
/* compare satellite fcb -----------------------------------------------------*/ /* compare satellite fcb -----------------------------------------------------*/
int cmpfcb(const void *p1, const void *p2) int cmpfcb(const void *p1, const void *p2)
{ {
fcbd_t *q1=(fcbd_t *)p1,*q2=(fcbd_t *)p2; fcbd_t *q1=(fcbd_t *)p1,*q2=(fcbd_t *)p2;
double tt=timediff(q1->ts,q2->ts); double tt=timediff(q1->ts,q2->ts);
return tt < -1e-3 ? -1 : (tt > 1e-3 ? 1 : 0); return tt<-1E-3?-1:(tt>1E-3?1:0);
} }
/* read satellite fcb data ----------------------------------------------------- /* read satellite fcb data -----------------------------------------------------
* read satellite fractional cycle bias (dcb) parameters * read satellite fractional cycle bias (dcb) parameters
* args : char *file I fcb parameters file (wild-card * expanded) * args : char *file I fcb parameters file (wild-card * expanded)
@ -558,46 +515,36 @@ int readfcb(const char *file, nav_t *nav)
trace(3,"readfcb : file=%s\n",file); trace(3,"readfcb : file=%s\n",file);
for (i = 0; i < MAXEXFILE; i++) for (i=0;i<MAXEXFILE;i++) {
{ if (!(efiles[i]=(char *)malloc(1024))) {
if (!(efiles[i] = (char *)malloc(1024)))
{
for (i--;i>=0;i--) free(efiles[i]); for (i--;i>=0;i--) free(efiles[i]);
return 0; return 0;
} }
} }
n=expath(file,efiles,MAXEXFILE); n=expath(file,efiles,MAXEXFILE);
for (i = 0; i < n; i++) for (i=0;i<n;i++) {
{
readfcbf(efiles[i],nav); readfcbf(efiles[i],nav);
} }
for (i=0;i<MAXEXFILE;i++) free(efiles[i]); for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
if (nav->nf > 1) if (nav->nf>1) {
{
qsort(nav->fcb,nav->nf,sizeof(fcbd_t),cmpfcb); qsort(nav->fcb,nav->nf,sizeof(fcbd_t),cmpfcb);
} }
return 1; return 1;
} }
/* polynomial interpolation by Neville's algorithm ---------------------------*/ /* polynomial interpolation by Neville's algorithm ---------------------------*/
double interppol(const double *x, double *y, int n) double interppol(const double *x, double *y, int n)
{ {
int i,j; int i,j;
for (j = 1; j < n; j++) for (j=1;j<n;j++) {
{ for (i=0;i<n-j;i++) {
for (i = 0; i < n - j; i++)
{
y[i]=(x[i+j]*y[i]-x[i]*y[i+1])/(x[i+j]-x[i]); y[i]=(x[i+j]*y[i]-x[i]*y[i+1])/(x[i+j]-x[i]);
} }
} }
return y[0]; return y[0];
} }
/* satellite position by precise ephemeris -----------------------------------*/ /* satellite position by precise ephemeris -----------------------------------*/
int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs, int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *vare, double *varc) double *dts, double *vare, double *varc)
@ -611,14 +558,12 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
if (nav->ne<NMAX+1|| if (nav->ne<NMAX+1||
timediff(time,nav->peph[0].time)<-MAXDTE|| timediff(time,nav->peph[0].time)<-MAXDTE||
timediff(time, nav->peph[nav->ne-1].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); trace(3,"no prec ephem %s sat=%2d\n",time_str(time,0),sat);
return 0; return 0;
} }
/* binary search */ /* binary search */
for (i = 0, j = nav->ne - 1; i < j;) for (i=0,j=nav->ne-1;i<j;) {
{
k=(i+j)/2; k=(i+j)/2;
if (timediff(nav->peph[k].time,time)<0.0) i=k+1; else j=k; if (timediff(nav->peph[k].time,time)<0.0) i=k+1; else j=k;
} }
@ -628,43 +573,38 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
i=index-(NMAX+1)/2; i=index-(NMAX+1)/2;
if (i<0) i=0; else if (i+NMAX>=nav->ne) i=nav->ne-NMAX-1; if (i<0) i=0; else if (i+NMAX>=nav->ne) i=nav->ne-NMAX-1;
for (j = 0; j <= NMAX; j++) for (j=0;j<=NMAX;j++) {
{
t[j]=timediff(nav->peph[i+j].time,time); t[j]=timediff(nav->peph[i+j].time,time);
if (norm(nav->peph[i+j].pos[sat-1], 3) <= 0.0) 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); trace(3,"prec ephem outage %s sat=%2d\n",time_str(time,0),sat);
return 0; return 0;
} }
} }
for (j = 0;j<=NMAX;j++) for (j=0;j<=NMAX;j++) {
{
pos=nav->peph[i+j].pos[sat-1]; pos=nav->peph[i+j].pos[sat-1];
#if 0 #if 0
p[0][j]=pos[0]; p[0][j]=pos[0];
p[1][j]=pos[1]; p[1][j]=pos[1];
#else #else
/* correciton for earh rotation ver.2.4.0 */ /* correciton for earh rotation ver.2.4.0 */
sinl = sin(DEFAULT_OMEGA_EARTH_DOT * t[j]); sinl=sin(OMGE*t[j]);
cosl = cos(DEFAULT_OMEGA_EARTH_DOT * t[j]); cosl=cos(OMGE*t[j]);
p[0][j]=cosl*pos[0]-sinl*pos[1]; p[0][j]=cosl*pos[0]-sinl*pos[1];
p[1][j]=sinl*pos[0]+cosl*pos[1]; p[1][j]=sinl*pos[0]+cosl*pos[1];
#endif #endif
p[2][j]=pos[2]; p[2][j]=pos[2];
} }
for (i = 0; i < 3; i++) for (i=0;i<3;i++) {
{
rs[i]=interppol(t,p[i],NMAX+1); rs[i]=interppol(t,p[i],NMAX+1);
} }
if (vare) if (vare) {
{
for (i=0;i<3;i++) s[i]=nav->peph[index].std[sat-1][i]; for (i=0;i<3;i++) s[i]=nav->peph[index].std[sat-1][i];
std=norm(s,3); std=norm(s,3);
/* extrapolation error for orbit */ /* extrapolation error for orbit */
if (t[0 ] > 0.0) std += EXTERR_EPH * pow(2, t[0 ]) / 2.0; if (t[0 ]>0.0) std+=EXTERR_EPH*SQR(t[0 ])/2.0;
else if (t[NMAX] < 0.0) std += EXTERR_EPH * pow(2, t[NMAX]) / 2.0; else if (t[NMAX]<0.0) std+=EXTERR_EPH*SQR(t[NMAX])/2.0;
*vare = pow(2, std); *vare=SQR(std);
} }
/* linear interpolation for clock */ /* linear interpolation for clock */
t[0]=timediff(time,nav->peph[index ].time); t[0]=timediff(time,nav->peph[index ].time);
@ -672,35 +612,27 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
c[0]=nav->peph[index ].pos[sat-1][3]; c[0]=nav->peph[index ].pos[sat-1][3];
c[1]=nav->peph[index+1].pos[sat-1][3]; c[1]=nav->peph[index+1].pos[sat-1][3];
if (t[0] <= 0.0) if (t[0]<=0.0) {
{ if ((dts[0]=c[0])!=0.0) {
if ((dts[0] = c[0]) != 0.0) std=nav->peph[index].std[sat-1][3]*CLIGHT-EXTERR_CLK*t[0];
{
std = nav->peph[index].std[sat-1][3] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
} }
} }
else if (t[1] >= 0.0) else if (t[1]>=0.0) {
{ if ((dts[0]=c[1])!=0.0) {
if ((dts[0] = c[1]) != 0.0) std=nav->peph[index+1].std[sat-1][3]*CLIGHT+EXTERR_CLK*t[1];
{
std = nav->peph[index+1].std[sat-1][3] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
} }
} }
else if (c[0] != 0.0 && c[1] != 0.0) else if (c[0]!=0.0&&c[1]!=0.0) {
{
dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]);
i=t[0]<-t[1]?0:1; i=t[0]<-t[1]?0:1;
std=nav->peph[index+i].std[sat-1][3]+EXTERR_CLK*fabs(t[i]); std=nav->peph[index+i].std[sat-1][3]+EXTERR_CLK*fabs(t[i]);
} }
else else {
{
dts[0]=0.0; dts[0]=0.0;
} }
if (varc) *varc = std::pow(2, std); if (varc) *varc=SQR(std);
return 1; return 1;
} }
/* satellite clock by precise clock ------------------------------------------*/ /* satellite clock by precise clock ------------------------------------------*/
int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
double *varc) double *varc)
@ -712,14 +644,12 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
if (nav->nc<2|| if (nav->nc<2||
timediff(time,nav->pclk[0].time)<-MAXDTE|| timediff(time,nav->pclk[0].time)<-MAXDTE||
timediff(time, nav->pclk[nav->nc-1].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); trace(3,"no prec clock %s sat=%2d\n",time_str(time,0),sat);
return 1; return 1;
} }
/* binary search */ /* binary search */
for (i = 0, j = nav->nc - 1; i < j;) for (i=0,j=nav->nc-1;i<j;) {
{
k=(i+j)/2; k=(i+j)/2;
if (timediff(nav->pclk[k].time,time)<0.0) i=k+1; else j=k; if (timediff(nav->pclk[k].time,time)<0.0) i=k+1; else j=k;
} }
@ -731,32 +661,26 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
c[0]=nav->pclk[index ].clk[sat-1][0]; c[0]=nav->pclk[index ].clk[sat-1][0];
c[1]=nav->pclk[index+1].clk[sat-1][0]; c[1]=nav->pclk[index+1].clk[sat-1][0];
if (t[0] <= 0.0) if (t[0]<=0.0) {
{
if ((dts[0]=c[0])==0.0) return 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]; std=nav->pclk[index].std[sat-1][0]*CLIGHT-EXTERR_CLK*t[0];
} }
else if (t[1] >= 0.0) else if (t[1]>=0.0) {
{
if ((dts[0]=c[1])==0.0) return 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]; std=nav->pclk[index+1].std[sat-1][0]*CLIGHT+EXTERR_CLK*t[1];
} }
else if (c[0] != 0.0 && c[1] != 0.0) else if (c[0]!=0.0&&c[1]!=0.0) {
{
dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]);
i=t[0]<-t[1]?0: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]); std=nav->pclk[index+i].std[sat-1][0]*CLIGHT+EXTERR_CLK*fabs(t[i]);
} }
else else {
{
trace(3,"prec clock outage %s sat=%2d\n",time_str(time,0),sat); trace(3,"prec clock outage %s sat=%2d\n",time_str(time,0),sat);
return 0; return 0;
} }
if (varc) *varc = std::pow(2, std); if (varc) *varc=SQR(std);
return 1; return 1;
} }
/* satellite antenna phase center offset --------------------------------------- /* satellite antenna phase center offset ---------------------------------------
* compute satellite antenna phase center offset in ecef * compute satellite antenna phase center offset in ecef
* args : gtime_t time I time (gpst) * args : gtime_t time I time (gpst)
@ -795,20 +719,17 @@ void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
if (NFREQ<2||lam[j]==0.0||lam[k]==0.0) return; if (NFREQ<2||lam[j]==0.0||lam[k]==0.0) return;
gamma = std::pow(2, lam[k]) / std::pow(2, lam[j]); gamma=SQR(lam[k])/SQR(lam[j]);
C1=gamma/(gamma-1.0); C1=gamma/(gamma-1.0);
C2=-1.0 /(gamma-1.0); C2=-1.0 /(gamma-1.0);
/* iono-free LC */ /* iono-free LC */
for (i = 0; i < 3; i++) 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]; 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]; 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; dant[i]=C1*dant1+C2*dant2;
} }
} }
/* satellite position/clock by precise ephemeris/clock ------------------------- /* satellite position/clock by precise ephemeris/clock -------------------------
* compute satellite position/clock with precise ephemeris/clock * compute satellite position/clock with precise ephemeris/clock
* args : gtime_t time I time (gpst) * args : gtime_t time I time (gpst)
@ -830,7 +751,7 @@ void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, 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; int i;
trace(4,"peph2pos: time=%s sat=%2d opt=%d\n",time_str(time,3),sat,opt); trace(4,"peph2pos: time=%s sat=%2d opt=%d\n",time_str(time,3),sat,opt);
@ -846,23 +767,19 @@ int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
!pephclk(time,sat,nav,dtst,NULL)) return 0; !pephclk(time,sat,nav,dtst,NULL)) return 0;
/* satellite antenna offset correction */ /* satellite antenna offset correction */
if (opt) if (opt) {
{
satantoff(time,rss,sat,nav,dant); satantoff(time,rss,sat,nav,dant);
} }
for (i = 0;i<3;i++) for (i=0;i<3;i++) {
{
rs[i ]=rss[i]+dant[i]; rs[i ]=rss[i]+dant[i];
rs[i+3]=(rst[i]-rss[i])/tt; rs[i+3]=(rst[i]-rss[i])/tt;
} }
/* relativistic effect correction */ /* relativistic effect correction */
if (dtss[0] != 0.0) if (dtss[0]!=0.0) {
{ dts[0]=dtss[0]-2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT;
dts[0] = dtss[0] - 2.0 * dot(rs, rs+3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
dts[1]=(dtst[0]-dtss[0])/tt; dts[1]=(dtst[0]-dtss[0])/tt;
} }
else else { /* no precise clock */
{ /* no precise clock */
dts[0]=dts[1]=0.0; dts[0]=dts[1]=0.0;
} }
if (var) *var=vare+varc; if (var) *var=vare+varc;

46
src/algorithms/libs/rtklib/rtklib_preceph.h
View File

@ -49,7 +49,7 @@
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
* *
* *
* References : * references :
* [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c), * [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c),
* 12 February, 2007 * 12 February, 2007
* [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information, * [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information,
@ -58,19 +58,47 @@
* [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed, * [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
* Space Technology Library, 2004 * Space Technology Library, 2004
* *
* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
* history : 2009/01/18 1.0 new
* 2009/01/31 1.1 fix bug on numerical error to read sp3a ephemeris
* 2009/05/15 1.2 support glonass,galileo,qzs
* 2009/12/11 1.3 support wild-card expansion of file path
* 2010/07/21 1.4 added api:
* eci2ecef(),sunmoonpos(),peph2pos(),satantoff(),
* readdcb()
* changed api:
* readsp3()
* deleted api:
* eph2posp()
* 2010/09/09 1.5 fix problem when precise clock outage
* 2011/01/23 1.6 support qzss satellite code
* 2011/09/12 1.7 fix problem on precise clock outage
* move sunmmonpos() to rtkcmn.c
* 2011/12/01 1.8 modify api readsp3()
* precede later ephemeris if ephemeris is NULL
* move eci2ecef() to rtkcmn.c
* 2013/05/08 1.9 fix bug on computing std-dev of precise clocks
* 2013/11/20 1.10 modify option for api readsp3()
* 2014/04/03 1.11 accept extenstion including sp3,eph,SP3,EPH
* 2014/05/23 1.12 add function to read sp3 velocity records
* change api: satantoff()
* 2014/08/31 1.13 add member cov and vco in peph_t sturct
* 2014/10/13 1.14 fix bug on clock error variance in peph2pos()
* 2015/05/10 1.15 add api readfcb()
* modify api readdcb()
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
#ifndef RTKLIB_PRECEPH_H_
#ifndef GNSS_SDR_RTKLIB_PRECEPH_H_ #define RTKLIB_PRECEPH_H_
#define GNSS_SDR_RTKLIB_PRECEPH_H_
#include "rtklib.h" #include "rtklib.h"
#include "rtklib_rtkcmn.h" #include "rtklib_rtkcmn.h"
#define SQR(x) ((x)*(x))
const int NMAX = 10; /* order of polynomial interpolation */ #define NMAX 10 /* order of polynomial interpolation */
const double MAXDTE = 900.0; /* max time difference to ephem time (s) */ #define MAXDTE 900.0 /* max time difference to ephem time (s) */
const double EXTERR_CLK = 1e-3; /* extrapolation error for clock (m/s) */ #define EXTERR_CLK 1E-3 /* extrapolation error for clock (m/s) */
const double EXTERR_EPH = 5e-7; /* extrapolation error for ephem (m/s^2) */ #define EXTERR_EPH 5E-7 /* extrapolation error for ephem (m/s^2) */
int code2sys(char code); int code2sys(char code);
int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats, int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats,
@ -105,4 +133,4 @@ void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav,
int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, 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);
#endif // GNSS_SDR_RTKLIB_PRECEPH_H_ #endif //RTKLIB_PRECEPH_H_

845
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

File diff suppressed because it is too large Load Diff

96
src/algorithms/libs/rtklib/rtklib_rtkcmn.h
View File

@ -82,10 +82,98 @@
* [10] GLONASS/GPS/Galileo/Compass/SBAS NV08C receiver series BINR interface * [10] GLONASS/GPS/Galileo/Compass/SBAS NV08C receiver series BINR interface
* protocol specification ver.1.3, August, 2012 * protocol specification ver.1.3, August, 2012
* *
* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
* history : 2007/01/12 1.0 new
* 2007/03/06 1.1 input initial rover pos of pntpos()
* update only effective states of filter()
* fix bug of atan2() domain error
* 2007/04/11 1.2 add function antmodel()
* add gdop mask for pntpos()
* change constant MAXDTOE value
* 2007/05/25 1.3 add function execcmd(),expandpath()
* 2008/06/21 1.4 add funciton sortobs(),uniqeph(),screent()
* replace geodist() by sagnac correction way
* 2008/10/29 1.5 fix bug of ionosphereic mapping function
* fix bug of seasonal variation term of tropmapf
* 2008/12/27 1.6 add function tickget(), sleepms(), tracenav(),
* xyz2enu(), satposv(), pntvel(), covecef()
* 2009/03/12 1.7 fix bug on error-stop when localtime() returns NULL
* 2009/03/13 1.8 fix bug on time adjustment for summer time
* 2009/04/10 1.9 add function adjgpsweek(),getbits(),getbitu()
* add function geph2pos()
* 2009/06/08 1.10 add function seph2pos()
* 2009/11/28 1.11 change function pntpos()
* add function tracegnav(),tracepeph()
* 2009/12/22 1.12 change default parameter of ionos std
* valid under second for timeget()
* 2010/07/28 1.13 fix bug in tropmapf()
* added api:
* obs2code(),code2obs(),cross3(),normv3(),
* gst2time(),time2gst(),time_str(),timeset(),
* deg2dms(),dms2deg(),searchpcv(),antmodel_s(),
* tracehnav(),tracepclk(),reppath(),reppaths(),
* createdir()
* changed api:
* readpcv(),
* deleted api:
* uniqeph()
* 2010/08/20 1.14 omit to include mkl header files
* fix bug on chi-sqr(n) table
* 2010/12/11 1.15 added api:
* freeobs(),freenav(),ionppp()
* 2011/05/28 1.16 fix bug on half-hour offset by time2epoch()
* added api:
* uniqnav()
* 2012/06/09 1.17 add a leap second after 2012-6-30
* 2012/07/15 1.18 add api setbits(),setbitu(),utc2gmst()
* fix bug on interpolation of antenna pcv
* fix bug on str2num() for string with over 256 char
* add api readblq(),satexclude(),setcodepri(),
* getcodepri()
* change api obs2code(),code2obs(),antmodel()
* 2012/12/25 1.19 fix bug on satwavelen(),code2obs(),obs2code()
* add api testsnr()
* 2013/01/04 1.20 add api gpst2bdt(),bdt2gpst(),bdt2time(),time2bdt()
* readblq(),readerp(),geterp(),crc16()
* change api eci2ecef(),sunmoonpos()
* 2013/03/26 1.21 tickget() uses clock_gettime() for linux
* 2013/05/08 1.22 fix bug on nutation coefficients for ast_args()
* 2013/06/02 1.23 add #ifdef for undefined CLOCK_MONOTONIC_RAW
* 2013/09/01 1.24 fix bug on interpolation of satellite antenna pcv
* 2013/09/06 1.25 fix bug on extrapolation of erp
* 2014/04/27 1.26 add SYS_LEO for satellite system
* add BDS L1 code for RINEX 3.02 and RTCM 3.2
* support BDS L1 in satwavelen()
* 2014/05/29 1.27 fix bug on obs2code() to search obs code table
* 2014/08/26 1.28 fix problem on output of uncompress() for tar file
* add function to swap trace file with keywords
* 2014/10/21 1.29 strtok() -> strtok_r() in expath() for thread-safe
* add bdsmodear in procopt_default
* 2015/03/19 1.30 fix bug on interpolation of erp values in geterp()
* add leap second insertion before 2015/07/01 00:00
* add api read_leaps()
* 2015/05/31 1.31 delte api windupcorr()
* 2015/08/08 1.32 add compile option CPUTIME_IN_GPST
* add api add_fatal()
* support usno leapsec.dat for api read_leaps()
* 2016/01/23 1.33 enable septentrio
* 2016/02/05 1.34 support GLONASS for savenav(), loadnav()
* 2016/06/11 1.35 delete trace() in reppath() to avoid deadlock
* 2016/07/01 1.36 support IRNSS
* add leap second before 2017/1/1 00:00:00
* 2016/07/29 1.37 rename api compress() -> rtk_uncompress()
* rename api crc16() -> rtk_crc16()
* rename api crc24q() -> rtk_crc24q()
* rename api crc32() -> rtk_crc32()
* 2016/08/20 1.38 fix type incompatibility in win64 environment
* change constant _POSIX_C_SOURCE 199309 -> 199506
* 2016/08/21 1.39 fix bug on week overflow in time2gpst()/gpst2time()
* 2016/09/05 1.40 fix bug on invalid nav data read in readnav()
* 2016/09/17 1.41 suppress warnings
* 2016/09/19 1.42 modify api deg2dms() to consider numerical error
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
#ifndef RTKLIB_RTKCMN_H_
#ifndef GNSS_SDR_RTKLIB_RTKCMN_H_ #define RTKLIB_RTKCMN_H_
#define GNSS_SDR_RTKLIB_RTKCMN_H_
#include "rtklib.h" #include "rtklib.h"
@ -272,4 +360,4 @@ void csmooth(obs_t *obs, int ns);
int rtk_uncompress(const char *file, char *uncfile); int rtk_uncompress(const char *file, char *uncfile);
int expath(const char *path, char *paths[], int nmax); int expath(const char *path, char *paths[], int nmax);
#endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ #endif /* RTKLIB_RTKCMN_H_ */

287
src/algorithms/libs/rtklib/rtklib_sbas.cc
View File

@ -51,7 +51,7 @@
* *
* option : -DRRCENA enable rrc correction * option : -DRRCENA enable rrc correction
* *
* References : * references :
* [1] RTCA/DO-229C, Minimum operational performanc standards for global * [1] RTCA/DO-229C, Minimum operational performanc standards for global
* positioning system/wide area augmentation system airborne equipment, * positioning system/wide area augmentation system airborne equipment,
* RTCA inc, November 28, 2001 * RTCA inc, November 28, 2001
@ -59,6 +59,28 @@
* Interface Specification for QZSS, Japan Aerospace Exploration Agency, * Interface Specification for QZSS, Japan Aerospace Exploration Agency,
* July 31, 2009 * 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" #include "rtklib_sbas.h"
@ -69,8 +91,6 @@ 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; return p;
} }
/* variance of fast correction (udre=UDRE+1) ---------------------------------*/ /* variance of fast correction (udre=UDRE+1) ---------------------------------*/
double varfcorr(int udre) double varfcorr(int udre)
{ {
@ -80,8 +100,6 @@ double varfcorr(int udre)
}; };
return 0<udre&&udre<=14?var[udre-1]:0.0; return 0<udre&&udre<=14?var[udre-1]:0.0;
} }
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/ /* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
double varicorr(int give) double varicorr(int give)
{ {
@ -91,8 +109,6 @@ double varicorr(int give)
}; };
return 0<give&&give<=15?var[give-1]:0.0; return 0<give&&give<=15?var[give-1]:0.0;
} }
/* fast correction degradation -----------------------------------------------*/ /* fast correction degradation -----------------------------------------------*/
double degfcorr(int ai) double degfcorr(int ai)
{ {
@ -102,8 +118,6 @@ double degfcorr(int ai)
}; };
return 0<ai&&ai<=15?degf[ai]:0.0058; return 0<ai&&ai<=15?degf[ai]:0.0058;
} }
/* decode type 1: prn masks --------------------------------------------------*/ /* decode type 1: prn masks --------------------------------------------------*/
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat) int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
@ -111,10 +125,8 @@ int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4,"decode_sbstype1:\n"); trace(4,"decode_sbstype1:\n");
for (i = 1, n = 0;i<=210 && n<MAXSAT;i++) for (i=1,n=0;i<=210&&n<MAXSAT;i++) {
{ if (getbitu(msg->msg,13+i,1)) {
if (getbitu(msg->msg, 13+i, 1))
{
if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */ 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<= 61) sat=satno(SYS_GLO,i-37); /* 38- 61: glonass */
else if (i<=119) sat=0; /* 62-119: future gnss */ else if (i<=119) sat=0; /* 62-119: future gnss */
@ -132,8 +144,6 @@ int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp); trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp);
return 1; return 1;
} }
/* decode type 2-5,0: fast corrections ---------------------------------------*/ /* decode type 2-5,0: fast corrections ---------------------------------------*/
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
@ -148,8 +158,7 @@ int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
type=getbitu(msg->msg, 8,6); type=getbitu(msg->msg, 8,6);
iodf=getbitu(msg->msg,14,2); iodf=getbitu(msg->msg,14,2);
for (i = 0;i<13;i++) for (i=0;i<13;i++) {
{
if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break; if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break;
udre=getbitu(msg->msg,174+4*i,4); udre=getbitu(msg->msg,174+4*i,4);
t0 =sbssat->sat[j].fcorr.t0; t0 =sbssat->sat[j].fcorr.t0;
@ -158,13 +167,11 @@ int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
sbssat->sat[j].fcorr.prc=getbits(msg->msg,18+i*12,12)*0.125f; sbssat->sat[j].fcorr.prc=getbits(msg->msg,18+i*12,12)*0.125f;
sbssat->sat[j].fcorr.udre=udre+1; sbssat->sat[j].fcorr.udre=udre+1;
dt=timediff(sbssat->sat[j].fcorr.t0,t0); dt=timediff(sbssat->sat[j].fcorr.t0,t0);
if (t0.time==0||dt<=0.0||18.0<dt||sbssat->sat[j].fcorr.ai==0) if (t0.time==0||dt<=0.0||18.0<dt||sbssat->sat[j].fcorr.ai==0) {
{
sbssat->sat[j].fcorr.rrc=0.0; sbssat->sat[j].fcorr.rrc=0.0;
sbssat->sat[j].fcorr.dt=0.0; sbssat->sat[j].fcorr.dt=0.0;
} }
else else {
{
sbssat->sat[j].fcorr.rrc=(sbssat->sat[j].fcorr.prc-prc)/dt; sbssat->sat[j].fcorr.rrc=(sbssat->sat[j].fcorr.prc-prc)/dt;
sbssat->sat[j].fcorr.dt=dt; sbssat->sat[j].fcorr.dt=dt;
} }
@ -173,8 +180,6 @@ int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf); trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf);
return 1; return 1;
} }
/* decode type 6: integrity info ---------------------------------------------*/ /* decode type 6: integrity info ---------------------------------------------*/
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat) int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
@ -182,12 +187,10 @@ int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4,"decode_sbstype6:\n"); trace(4,"decode_sbstype6:\n");
for (i = 0;i<4;i++) for (i=0;i<4;i++) {
{
iodf[i]=getbitu(msg->msg,14+i*2,2); iodf[i]=getbitu(msg->msg,14+i*2,2);
} }
for (i = 0;i<sbssat->nsat && i<MAXSAT;i++) for (i=0;i<sbssat->nsat&&i<MAXSAT;i++) {
{
if (sbssat->sat[i].fcorr.iodf!=iodf[i/13]) continue; if (sbssat->sat[i].fcorr.iodf!=iodf[i/13]) continue;
udre=getbitu(msg->msg,22+i*4,4); udre=getbitu(msg->msg,22+i*4,4);
sbssat->sat[i].fcorr.udre=udre+1; sbssat->sat[i].fcorr.udre=udre+1;
@ -195,8 +198,6 @@ int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]); trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]);
return 1; return 1;
} }
/* decode type 7: fast correction degradation factor -------------------------*/ /* decode type 7: fast correction degradation factor -------------------------*/
int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
@ -208,14 +209,11 @@ int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
sbssat->tlat=getbitu(msg->msg,14,4); sbssat->tlat=getbitu(msg->msg,14,4);
for (i = 0;i<sbssat->nsat && i<MAXSAT;i++) for (i=0;i<sbssat->nsat&&i<MAXSAT;i++) {
{
sbssat->sat[i].fcorr.ai=getbitu(msg->msg,22+i*4,4); sbssat->sat[i].fcorr.ai=getbitu(msg->msg,22+i*4,4);
} }
return 1; return 1;
} }
/* decode type 9: geo navigation message -------------------------------------*/ /* decode type 9: geo navigation message -------------------------------------*/
int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
{ {
@ -224,8 +222,7 @@ int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
trace(4,"decode_sbstype9:\n"); trace(4,"decode_sbstype9:\n");
if (!(sat = satno(SYS_SBS, msg->prn))) if (!(sat=satno(SYS_SBS,msg->prn))) {
{
trace(2,"invalid prn in sbas type 9: prn=%3d\n",msg->prn); trace(2,"invalid prn in sbas type 9: prn=%3d\n",msg->prn);
return 0; return 0;
} }
@ -248,12 +245,11 @@ int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
seph.acc[1]=getbits(msg->msg,186,10)*0.0000125; seph.acc[1]=getbits(msg->msg,186,10)*0.0000125;
seph.acc[2]=getbits(msg->msg,196,10)*0.0000625; seph.acc[2]=getbits(msg->msg,196,10)*0.0000625;
seph.af0 = getbits(msg->msg, 206, 12)*TWO_N31; seph.af0=getbits(msg->msg,206,12)*P2_31;
seph.af1 = getbits(msg->msg, 218, 8)*TWO_N39/2.0; seph.af1=getbits(msg->msg,218, 8)*P2_39/2.0;
i=msg->prn-MINPRNSBS; i=msg->prn-MINPRNSBS;
if (!nav->seph||fabs(timediff(nav->seph[i].t0, seph.t0))<1e-3) if (!nav->seph||fabs(timediff(nav->seph[i].t0,seph.t0))<1E-3) { /* not change */
{ /* not change */
return 0; return 0;
} }
nav->seph[NSATSBS+i]=nav->seph[i]; /* previous */ nav->seph[NSATSBS+i]=nav->seph[i]; /* previous */
@ -262,8 +258,6 @@ int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
trace(5,"decode_sbstype9: prn=%d\n",msg->prn); trace(5,"decode_sbstype9: prn=%d\n",msg->prn);
return 1; return 1;
} }
/* decode type 18: ionospheric grid point masks ------------------------------*/ /* decode type 18: ionospheric grid point masks ------------------------------*/
int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion) int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
{ {
@ -278,11 +272,9 @@ int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
sbsion[band].iodi=(short)getbitu(msg->msg,22,2); sbsion[band].iodi=(short)getbitu(msg->msg,22,2);
for (i = 1, n = 0; i<= 201;i++) for (i=1,n=0;i<=201;i++) {
{
if (!getbitu(msg->msg,23+i,1)) continue; if (!getbitu(msg->msg,23+i,1)) continue;
for (j = 0; j < m; j++) for (j=0;j<m;j++) {
{
if (i<p[j].bits||p[j].bite<i) continue; 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].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]; sbsion[band].igp[n++].lon=band<=8?p[j].x:p[j].y[i-p[j].bits];
@ -294,8 +286,6 @@ int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
trace(5,"decode_sbstype18: band=%d nigp=%d\n",band,n); trace(5,"decode_sbstype18: band=%d nigp=%d\n",band,n);
return 1; return 1;
} }
/* decode half long term correction (vel code=0) -----------------------------*/ /* decode half long term correction (vel code=0) -----------------------------*/
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat) int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{ {
@ -307,20 +297,17 @@ int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8);
for (i = 0;i<3;i++) 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.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.dvel[i]=0.0;
} }
sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p + 41, 10) * TWO_N31; 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.daf1=0.0;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow); sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow);
trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat); trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat);
return 1; return 1;
} }
/* decode half long term correction (vel code=1) -----------------------------*/ /* decode half long term correction (vel code=1) -----------------------------*/
int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat) int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{ {
@ -332,13 +319,12 @@ int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8); sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8);
for (i = 0;i<3;i++) 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.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.dvel[i]=getbits(msg->msg,p+58+i* 8, 8)*P2_11;
} }
sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p+47, 11) * TWO_N31; 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) * TWO_N39; 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; t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400;
if (t<=-43200) t+=86400; if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400; else if (t> 43200) t-=86400;
@ -347,28 +333,22 @@ int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
trace(5,"decode_longcorr1: sat=%2d\n",sbssat->sat[n-1].sat); trace(5,"decode_longcorr1: sat=%2d\n",sbssat->sat[n-1].sat);
return 1; return 1;
} }
/* decode half long term correction ------------------------------------------*/ /* decode half long term correction ------------------------------------------*/
int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat) int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{ {
trace(4,"decode_longcorrh:\n"); trace(4,"decode_longcorrh:\n");
if (getbitu(msg->msg,p,1)==0) { /* vel code=0 */ if (getbitu(msg->msg,p,1)==0) { /* vel code=0 */
if (sbssat->iodp==(int)getbitu(msg->msg, p+103, 2)) if (sbssat->iodp==(int)getbitu(msg->msg,p+103,2)) {
{
return decode_longcorr0(msg,p+ 1,sbssat)&& return decode_longcorr0(msg,p+ 1,sbssat)&&
decode_longcorr0(msg,p+52,sbssat); decode_longcorr0(msg,p+52,sbssat);
} }
} }
else if (sbssat->iodp==(int)getbitu(msg->msg, p+104, 2)) else if (sbssat->iodp==(int)getbitu(msg->msg,p+104,2)) {
{
return decode_longcorr1(msg,p+1,sbssat); return decode_longcorr1(msg,p+1,sbssat);
} }
return 0; return 0;
} }
/* decode type 24: mixed fast/long term correction ---------------------------*/ /* decode type 24: mixed fast/long term correction ---------------------------*/
int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
@ -381,8 +361,7 @@ int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
blk =getbitu(msg->msg,112,2); blk =getbitu(msg->msg,112,2);
iodf=getbitu(msg->msg,114,2); iodf=getbitu(msg->msg,114,2);
for (i = 0;i<6;i++) for (i=0;i<6;i++) {
{
if ((j=13*blk+i)>=sbssat->nsat) break; if ((j=13*blk+i)>=sbssat->nsat) break;
udre=getbitu(msg->msg,86+4*i,4); udre=getbitu(msg->msg,86+4*i,4);
@ -393,8 +372,6 @@ int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
} }
return decode_longcorrh(msg,120,sbssat); return decode_longcorrh(msg,120,sbssat);
} }
/* decode type 25: long term satellite error correction ----------------------*/ /* decode type 25: long term satellite error correction ----------------------*/
int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat) int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
@ -402,8 +379,6 @@ int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat)
return decode_longcorrh(msg,14,sbssat)&&decode_longcorrh(msg,120,sbssat); return decode_longcorrh(msg,14,sbssat)&&decode_longcorrh(msg,120,sbssat);
} }
/* decode type 26: ionospheric deley corrections -----------------------------*/ /* decode type 26: ionospheric deley corrections -----------------------------*/
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion) int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
{ {
@ -415,8 +390,7 @@ int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
block=getbitu(msg->msg,18,4); block=getbitu(msg->msg,18,4);
for (i = 0;i<15;i++) for (i=0;i<15;i++) {
{
if ((j=block*15+i)>=sbsion[band].nigp) continue; if ((j=block*15+i)>=sbsion[band].nigp) continue;
give=getbitu(msg->msg,22+i*13+9,4); give=getbitu(msg->msg,22+i*13+9,4);
@ -425,16 +399,13 @@ int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
sbsion[band].igp[j].delay=delay==0x1FF?0.0f:delay*0.125f; sbsion[band].igp[j].delay=delay==0x1FF?0.0f:delay*0.125f;
sbsion[band].igp[j].give=give+1; sbsion[band].igp[j].give=give+1;
if (sbsion[band].igp[j].give>=16) if (sbsion[band].igp[j].give>=16) {
{
sbsion[band].igp[j].give=0; 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; return 1;
} }
/* update sbas corrections ----------------------------------------------------- /* update sbas corrections -----------------------------------------------------
* update sbas correction parameters in navigation data with a sbas message * update sbas correction parameters in navigation data with a sbas message
* args : sbsmg_t *msg I sbas message * args : sbsmg_t *msg I sbas message
@ -452,8 +423,7 @@ int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
if (msg->week==0) return -1; if (msg->week==0) return -1;
switch (type) switch (type) {
{
case 0: stat=decode_sbstype2 (msg,&nav->sbssat); break; case 0: stat=decode_sbstype2 (msg,&nav->sbssat); break;
case 1: stat=decode_sbstype1 (msg,&nav->sbssat); break; case 1: stat=decode_sbstype1 (msg,&nav->sbssat); break;
case 2: case 2:
@ -473,8 +443,6 @@ int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
} }
return stat?type:-1; return stat?type:-1;
} }
/* read sbas log file --------------------------------------------------------*/ /* read sbas log file --------------------------------------------------------*/
void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te, void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
sbs_t *sbs) sbs_t *sbs)
@ -489,35 +457,29 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
trace(3,"readmsgs: file=%s sel=%d\n",file,sel); trace(3,"readmsgs: file=%s sel=%d\n",file,sel);
if (!(fp = fopen(file, "r"))) if (!(fp=fopen(file,"r"))) {
{
trace(2,"sbas message file open error: %s\n",file); trace(2,"sbas message file open error: %s\n",file);
return; return;
} }
while (fgets(buff, sizeof(buff), fp)) while (fgets(buff,sizeof(buff),fp)) {
{ if (sscanf(buff,"%d %lf %d",&week,&tow,&prn)==3&&(p=strstr(buff,": "))) {
if (sscanf(buff, "%d %lf %d", &week, &tow, &prn)==3 && (p = strstr(buff, ": ")))
{
p+=2; /* rtklib form */ p+=2; /* rtklib form */
} }
else if (sscanf(buff,"%d %lf %lf %lf %lf %lf %lf %d", 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) &prn,ep,ep+1,ep+2,ep+3,ep+4,ep+5,&msg)==8) {
{
/* ems (EGNOS Message Service) form */ /* ems (EGNOS Message Service) form */
ep[0]+=ep[0]<70.0?2000.0:1900.0; ep[0]+=ep[0]<70.0?2000.0:1900.0;
tow=time2gpst(epoch2time(ep),&week); tow=time2gpst(epoch2time(ep),&week);
p=buff+(msg>=10?25:24); p=buff+(msg>=10?25:24);
} }
else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) { /* NovAtel OEM4/V */
{ /* NovAtel OEM4/V */
if (!(p=getfield(buff,6))) continue; if (!(p=getfield(buff,6))) continue;
if (sscanf(p,"%d,%lf",&week,&tow)<2) continue; if (sscanf(p,"%d,%lf",&week,&tow)<2) continue;
if (!(p=strchr(++p,';'))) continue; if (!(p=strchr(++p,';'))) continue;
if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue; if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue;
if (!(p=getfield(p,4))) continue; if (!(p=getfield(p,4))) continue;
} }
else if (!strncmp(buff,"$FRMA",5)) else if (!strncmp(buff,"$FRMA",5)) { /* NovAtel OEM3 */
{ /* NovAtel OEM3 */
if (!(p=getfield(buff,2))) continue; if (!(p=getfield(buff,2))) continue;
if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue; if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue;
if (!(p=getfield(p,6))) continue; if (!(p=getfield(p,6))) continue;
@ -531,11 +493,9 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
if (!screent(time,ts,te,0.0)) continue; if (!screent(time,ts,te,0.0)) continue;
if (sbs->n>=sbs->nmax) if (sbs->n>=sbs->nmax) {
{
sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2; sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2;
if (!(sbs_msgs = (sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) if (!(sbs_msgs=(sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) {
{
trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax); trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax);
free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0; free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0;
return; return;
@ -546,16 +506,13 @@ void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
sbs->msgs[sbs->n].tow=(int)(tow+0.5); sbs->msgs[sbs->n].tow=(int)(tow+0.5);
sbs->msgs[sbs->n].prn=prn; sbs->msgs[sbs->n].prn=prn;
for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0; for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0;
for (i = 0; *(p-1) && *p && i < 29; p += 2,i++) 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; if (sscanf(p,"%2X",&b)==1) sbs->msgs[sbs->n].msg[i]=(unsigned char)b;
} }
sbs->msgs[sbs->n++].msg[28]&=0xC0; sbs->msgs[sbs->n++].msg[28]&=0xC0;
} }
fclose(fp); fclose(fp);
} }
/* compare sbas messages -----------------------------------------------------*/ /* compare sbas messages -----------------------------------------------------*/
int cmpmsgs(const void *p1, const void *p2) int cmpmsgs(const void *p1, const void *p2)
{ {
@ -563,8 +520,6 @@ int cmpmsgs(const void *p1, const void *p2)
return q1->week!=q2->week?q1->week-q2->week: return q1->week!=q2->week?q1->week-q2->week:
(q1->tow<q2->tow?-1:(q1->tow>q2->tow?1:q1->prn-q2->prn)); (q1->tow<q2->tow?-1:(q1->tow>q2->tow?1:q1->prn-q2->prn));
} }
/* read sbas message file ------------------------------------------------------ /* read sbas message file ------------------------------------------------------
* read sbas message file * read sbas message file
* args : char *file I sbas message file (wind-card * is expanded) * args : char *file I sbas message file (wind-card * is expanded)
@ -588,10 +543,8 @@ int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel); trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel);
for (i = 0;i<MAXEXFILE;i++) for (i=0;i<MAXEXFILE;i++) {
{ if (!(efiles[i]=(char *)malloc(1024))) {
if (!(efiles[i] = (char *)malloc(1024)))
{
for (i--;i>=0;i--) free(efiles[i]); for (i--;i>=0;i--) free(efiles[i]);
return 0; return 0;
} }
@ -599,8 +552,7 @@ int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
/* expand wild card in file path */ /* expand wild card in file path */
n=expath(file,efiles,MAXEXFILE); n=expath(file,efiles,MAXEXFILE);
for (i = 0;i<n;i++) for (i=0;i<n;i++) {
{
if (!(ext=strrchr(efiles[i],'.'))) continue; if (!(ext=strrchr(efiles[i],'.'))) continue;
if (strcmp(ext,".sbs")&&strcmp(ext,".SBS")&& if (strcmp(ext,".sbs")&&strcmp(ext,".SBS")&&
strcmp(ext,".ems")&&strcmp(ext,".EMS")) continue; strcmp(ext,".ems")&&strcmp(ext,".EMS")) continue;
@ -610,14 +562,11 @@ int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
for (i=0;i<MAXEXFILE;i++) free(efiles[i]); for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
/* sort messages */ /* sort messages */
if (sbs->n>0) if (sbs->n>0) {
{
qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs); qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs);
} }
return sbs->n; return sbs->n;
} }
int sbsreadmsg(const char *file, int sel, sbs_t *sbs) int sbsreadmsg(const char *file, int sel, sbs_t *sbs)
{ {
gtime_t ts={},te={}; gtime_t ts={},te={};
@ -626,8 +575,6 @@ int sbsreadmsg(const char *file, int sel, sbs_t *sbs)
return sbsreadmsgt(file,sel,ts,te,sbs); return sbsreadmsgt(file,sel,ts,te,sbs);
} }
/* output sbas messages -------------------------------------------------------- /* output sbas messages --------------------------------------------------------
* output sbas message record to output file in rtklib sbas log format * output sbas message record to output file in rtklib sbas log format
* args : FILE *fp I output file pointer * args : FILE *fp I output file pointer
@ -644,8 +591,6 @@ void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg)
for (i=0;i<29;i++) fprintf(fp,"%02X",sbsmsg->msg[i]); for (i=0;i<29;i++) fprintf(fp,"%02X",sbsmsg->msg[i]);
fprintf(fp,"\n"); fprintf(fp,"\n");
} }
/* search igps ---------------------------------------------------------------*/ /* search igps ---------------------------------------------------------------*/
void searchigp(gtime_t time, const double *pos, const sbsion_t *ion, void searchigp(gtime_t time, const double *pos, const sbsion_t *ion,
const sbsigp_t **igp, double *x, double *y) const sbsigp_t **igp, double *x, double *y)
@ -657,8 +602,7 @@ void searchigp(gtime_t time, const double *pos, const sbsion_t *ion,
trace(4,"searchigp: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); trace(4,"searchigp: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
if (lon>=180.0) lon-=360.0; if (lon>=180.0) lon-=360.0;
if (-55.0<=lat && lat<55.0) if (-55.0<=lat&&lat<55.0) {
{
latp[0]=(int)floor(lat/5.0)*5; latp[0]=(int)floor(lat/5.0)*5;
latp[1]=latp[0]+5; latp[1]=latp[0]+5;
lonp[0]=lonp[1]=(int)floor(lon/5.0)*5; lonp[0]=lonp[1]=(int)floor(lon/5.0)*5;
@ -666,38 +610,31 @@ void searchigp(gtime_t time, const double *pos, const sbsion_t *ion,
*x=(lon-lonp[0])/5.0; *x=(lon-lonp[0])/5.0;
*y=(lat-latp[0])/5.0; *y=(lat-latp[0])/5.0;
} }
else else {
{
latp[0]=(int)floor((lat-5.0)/10.0)*10+5; latp[0]=(int)floor((lat-5.0)/10.0)*10+5;
latp[1]=latp[0]+10; latp[1]=latp[0]+10;
lonp[0]=lonp[1]=(int)floor(lon/10.0)*10; lonp[0]=lonp[1]=(int)floor(lon/10.0)*10;
lonp[2]=lonp[3]=lonp[0]+10; lonp[2]=lonp[3]=lonp[0]+10;
*x=(lon-lonp[0])/10.0; *x=(lon-lonp[0])/10.0;
*y=(lat-latp[0])/10.0; *y=(lat-latp[0])/10.0;
if (75.0<=lat && lat<85.0) if (75.0<=lat&&lat<85.0) {
{
lonp[1]=(int)floor(lon/90.0)*90; lonp[1]=(int)floor(lon/90.0)*90;
lonp[3]=lonp[1]+90; lonp[3]=lonp[1]+90;
} }
else if (-85.0<=lat && lat<-75.0) else if (-85.0<=lat&&lat<-75.0) {
{
lonp[0]=(int)floor((lon-50.0)/90.0)*90+40; lonp[0]=(int)floor((lon-50.0)/90.0)*90+40;
lonp[2]=lonp[0]+90; lonp[2]=lonp[0]+90;
} }
else if (lat>=85.0) else if (lat>=85.0) {
{
for (i=0;i<4;i++) lonp[i]=(int)floor(lon/90.0)*90; for (i=0;i<4;i++) lonp[i]=(int)floor(lon/90.0)*90;
} }
else if (lat<-85.0) 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++) 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<4;i++) if (lonp[i]==180) lonp[i]=-180;
for (i = 0;i<=MAXBAND;i++) for (i=0;i<=MAXBAND;i++) {
{ for (p=ion[i].igp;p<ion[i].igp+ion[i].nigp;p++) {
for (p = ion[i].igp;p<ion[i].igp+ion[i].nigp;p++)
{
if (p->t0.time==0) continue; if (p->t0.time==0) continue;
if (p->lat==latp[0]&&p->lon==lonp[0]&&p->give>0) igp[0]=p; 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[1]&&p->lon==lonp[1]&&p->give>0) igp[1]=p;
@ -707,8 +644,6 @@ void searchigp(gtime_t time, const double *pos, const sbsion_t *ion,
} }
} }
} }
/* sbas ionospheric delay correction ------------------------------------------- /* sbas ionospheric delay correction -------------------------------------------
* compute sbas ionosphric delay correction * compute sbas ionosphric delay correction
* args : gtime_t time I time * args : gtime_t time I time
@ -743,55 +678,43 @@ int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
searchigp(time,posp,nav->sbsion,igp,&x,&y); searchigp(time,posp,nav->sbsion,igp,&x,&y);
/* weight of igps */ /* weight of igps */
if (igp[0] && igp[1] && igp[2] && igp[3]) 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;
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]) else if (igp[0]&&igp[1]&&igp[2]) {
{
w[1]=y; w[2]=x; w[1]=y; w[2]=x;
if ((w[0]=1.0-w[1]-w[2])<0.0) err=1; if ((w[0]=1.0-w[1]-w[2])<0.0) err=1;
} }
else if (igp[0] && igp[2] && igp[3]) else if (igp[0]&&igp[2]&&igp[3]) {
{
w[0]=1.0-x; w[3]=y; w[0]=1.0-x; w[3]=y;
if ((w[2]=1.0-w[0]-w[3])<0.0) err=1; if ((w[2]=1.0-w[0]-w[3])<0.0) err=1;
} }
else if (igp[0] && igp[1] && igp[3]) else if (igp[0]&&igp[1]&&igp[3]) {
{
w[0]=1.0-y; w[3]=x; w[0]=1.0-y; w[3]=x;
if ((w[1]=1.0-w[0]-w[3])<0.0) err=1; if ((w[1]=1.0-w[0]-w[3])<0.0) err=1;
} }
else if (igp[1] && igp[2] && igp[3]) else if (igp[1]&&igp[2]&&igp[3]) {
{
w[1]=1.0-x; w[2]=1.0-y; w[1]=1.0-x; w[2]=1.0-y;
if ((w[3]=1.0-w[1]-w[2])<0.0) err=1; if ((w[3]=1.0-w[1]-w[2])<0.0) err=1;
} }
else err=1; else err=1;
if (err) if (err) {
{
trace(2,"no sbas iono correction: lat=%3.0f lon=%4.0f\n",posp[0]*R2D, trace(2,"no sbas iono correction: lat=%3.0f lon=%4.0f\n",posp[0]*R2D,
posp[1]*R2D); posp[1]*R2D);
return 0; return 0;
} }
for (i = 0; i < 4; i++) for (i=0;i<4;i++) {
{
if (!igp[i]) continue; if (!igp[i]) continue;
t=timediff(time,igp[i]->t0); t=timediff(time,igp[i]->t0);
*delay+=w[i]*igp[i]->delay; *delay+=w[i]*igp[i]->delay;
*var += w[i] * varicorr(igp[i]->give) * 9e-8 * fabs(t); *var+=w[i]*varicorr(igp[i]->give)*9E-8*fabs(t);
} }
*delay*=fp; *var*=fp*fp; *delay*=fp; *var*=fp*fp;
trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var)); trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var));
return 1; return 1;
} }
/* get meterological parameters ----------------------------------------------*/ /* get meterological parameters ----------------------------------------------*/
void getmet(double lat, double *met) void getmet(double lat, double *met)
{ {
@ -807,14 +730,11 @@ void getmet(double lat, double *met)
lat=fabs(lat); lat=fabs(lat);
if (lat<=15.0) for (i=0;i<10;i++) met[i]=metprm[0][i]; 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 if (lat>=75.0) for (i=0;i<10;i++) met[i]=metprm[4][i];
else else {
{
j=(int)(lat/15.0); a=(lat-j*15.0)/15.0; 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]; for (i=0;i<10;i++) met[i]=(1.0-a)*metprm[j-1][i]+a*metprm[j][i];
} }
} }
/* tropospheric delay correction ----------------------------------------------- /* tropospheric delay correction -----------------------------------------------
* compute sbas tropospheric delay correction (mops model) * compute sbas tropospheric delay correction (mops model)
* args : gtime_t time I time * args : gtime_t time I time
@ -834,19 +754,17 @@ double sbstropcorr(gtime_t time, const double *pos, const double *azel,
trace(4,"sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, trace(4,"sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D,
azel[0]*R2D,azel[1]*R2D); azel[0]*R2D,azel[1]*R2D);
if (pos[2] < -100.0 || 10000.0 < pos[2] || azel[1] <= 0) if (pos[2]<-100.0||10000.0<pos[2]||azel[1]<=0) {
{
*var=0.0; *var=0.0;
return 0.0; return 0.0;
} }
if (zh == 0.0 || fabs(pos[0] - pos_[0]) > 1e-7 || fabs(pos[1] - pos_[1]) > 1e-7 || 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) fabs(pos[2]-pos_[2])>1.0) {
{
getmet(pos[0]*R2D,met); getmet(pos[0]*R2D,met);
c=cos(2.0*PI*(time2doy(time)-(pos[0]>=0.0?28.0:211.0))/365.25); 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; for (i=0;i<5;i++) met[i]-=met[i+5]*c;
zh = 1e-6 * k1 * rd * met[0] / gm; 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]; 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])); 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); 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]; for (i=0;i<3;i++) pos_[i]=pos[i];
@ -855,8 +773,6 @@ double sbstropcorr(gtime_t time, const double *pos, const double *azel,
*var=0.12*0.12*m*m; *var=0.12*0.12*m*m;
return (zh+zw)*m; return (zh+zw)*m;
} }
/* long term correction ------------------------------------------------------*/ /* long term correction ------------------------------------------------------*/
int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat, int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *drs, double *ddts) double *drs, double *ddts)
@ -867,12 +783,10 @@ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
trace(3,"sbslongcorr: sat=%2d\n",sat); trace(3,"sbslongcorr: sat=%2d\n",sat);
for (p = sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) for (p=sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) {
{
if (p->sat!=sat||p->lcorr.t0.time==0) continue; if (p->sat!=sat||p->lcorr.t0.time==0) continue;
t=timediff(time,p->lcorr.t0); t=timediff(time,p->lcorr.t0);
if (fabs(t) > MAXSBSAGEL) if (fabs(t)>MAXSBSAGEL) {
{
trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
time_str(time,0),sat,t); time_str(time,0),sat,t);
return 0; return 0;
@ -881,7 +795,7 @@ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
*ddts=p->lcorr.daf0+p->lcorr.daf1*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", 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); sat,drs[0],drs[1],drs[2],*ddts*CLIGHT);
return 1; return 1;
} }
@ -891,8 +805,6 @@ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
return 0; return 0;
} }
/* fast correction -----------------------------------------------------------*/ /* fast correction -----------------------------------------------------------*/
int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat, int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *prc, double *var) double *prc, double *var)
@ -902,8 +814,7 @@ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
trace(3,"sbsfastcorr: sat=%2d\n",sat); trace(3,"sbsfastcorr: sat=%2d\n",sat);
for (p = sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) for (p=sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) {
{
if (p->sat!=sat) continue; if (p->sat!=sat) continue;
if (p->fcorr.t0.time==0) break; if (p->fcorr.t0.time==0) break;
t=timediff(time,p->fcorr.t0)+sbssat->tlat; t=timediff(time,p->fcorr.t0)+sbssat->tlat;
@ -912,8 +823,7 @@ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue; if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue;
*prc=p->fcorr.prc; *prc=p->fcorr.prc;
#ifdef RRCENA #ifdef RRCENA
if (p->fcorr.ai > 0 && fabs(t) <= 8.0 * p->fcorr.dt) if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) {
{
*prc+=p->fcorr.rrc*t; *prc+=p->fcorr.rrc*t;
} }
#endif #endif
@ -926,8 +836,6 @@ int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat); trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat);
return 0; return 0;
} }
/* sbas satellite ephemeris and clock correction ------------------------------- /* sbas satellite ephemeris and clock correction -------------------------------
* correct satellite position and clock bias with sbas satellite corrections * correct satellite position and clock bias with sbas satellite corrections
* args : gtime_t time I reception time * args : gtime_t time I reception time
@ -954,26 +862,22 @@ int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
trace(3,"sbssatcorr : sat=%2d\n",sat); trace(3,"sbssatcorr : sat=%2d\n",sat);
/* sbas long term corrections */ /* sbas long term corrections */
if (!sbslongcorr(time, sat, &nav->sbssat, drs, &dclk)) if (!sbslongcorr(time,sat,&nav->sbssat,drs,&dclk)) {
{
return 0; return 0;
} }
/* sbas fast corrections */ /* sbas fast corrections */
if (!sbsfastcorr(time, sat, &nav->sbssat, &prc, var)) if (!sbsfastcorr(time,sat,&nav->sbssat,&prc,var)) {
{
return 0; return 0;
} }
for (i=0;i<3;i++) rs[i]+=drs[i]; for (i=0;i<3;i++) rs[i]+=drs[i];
dts[0] += dclk + prc / SPEED_OF_LIGHT; dts[0]+=dclk+prc/CLIGHT;
trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n", 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; return 1;
} }
/* decode sbas message --------------------------------------------------------- /* decode sbas message ---------------------------------------------------------
* decode sbas message frame words and check crc * decode sbas message frame words and check crc
* args : gtime_t time I reception time * args : gtime_t time I reception time
@ -995,8 +899,7 @@ int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
tow=time2gpst(time,&sbsmsg->week); tow=time2gpst(time,&sbsmsg->week);
sbsmsg->tow=(int)(tow+DTTOL); sbsmsg->tow=(int)(tow+DTTOL);
sbsmsg->prn=prn; sbsmsg->prn=prn;
for (i = 0; i < 7; i++) for (j = 0; j < 4; j++) 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[i*4+j]=(unsigned char)(words[i]>>((3-j)*8));
} }
sbsmsg->msg[28]=(unsigned char)(words[7]>>18)&0xC0; sbsmsg->msg[28]=(unsigned char)(words[7]>>18)&0xC0;

32
src/algorithms/libs/rtklib/rtklib_sbas.h
View File

@ -51,7 +51,7 @@
* *
* option : -DRRCENA enable rrc correction * option : -DRRCENA enable rrc correction
* *
* References : * references :
* [1] RTCA/DO-229C, Minimum operational performanc standards for global * [1] RTCA/DO-229C, Minimum operational performanc standards for global
* positioning system/wide area augmentation system airborne equipment, * positioning system/wide area augmentation system airborne equipment,
* RTCA inc, November 28, 2001 * RTCA inc, November 28, 2001
@ -59,10 +59,32 @@
* Interface Specification for QZSS, Japan Aerospace Exploration Agency, * Interface Specification for QZSS, Japan Aerospace Exploration Agency,
* July 31, 2009 * 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_ #ifndef RTKLIB_SBAS_H_
#define GNSS_SDR_RTKLIB_SBAS_H_ #define RTKLIB_SBAS_H_
#include "rtklib.h" #include "rtklib.h"
#include "rtklib_rtkcmn.h" #include "rtklib_rtkcmn.h"
@ -124,7 +146,6 @@ char *getfield(char *p, int pos);
double varfcorr(int udre); double varfcorr(int udre);
double varicorr(int give); double varicorr(int give);
double degfcorr(int ai); double degfcorr(int ai);
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat);
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat);
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_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_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat);
int decode_sbstype25(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 decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion);
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); 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); void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,sbs_t *sbs);
int cmpmsgs(const void *p1, const void *p2); int 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); sbsmsg_t *sbsmsg);
#endif /* GNSS_SDR_RTKLIB_SBAS_H_ */ #endif /* RTKLIB_SBAS_H_ */

7
src/core/system_parameters/GPS_L1_CA.h
View File

@ -35,20 +35,19 @@
#include <vector> #include <vector>
#include <utility> // std::pair #include <utility> // std::pair
#include "MATH_CONSTANTS.h" #include "MATH_CONSTANTS.h"
#include "gnss_frequencies.h"
// Physical constants // 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_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_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 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 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)] const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)]
// carrier and code frequencies // 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_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_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] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds]

3
src/core/system_parameters/GPS_L2C.h
View File

@ -36,7 +36,6 @@
#include <vector> #include <vector>
#include <utility> // std::pair #include <utility> // std::pair
#include "MATH_CONSTANTS.h" #include "MATH_CONSTANTS.h"
#include "gnss_frequencies.h"
// Physical constants // Physical constants
const double GPS_L2_C_m_s = 299792458.0; //!< The speed of light, [m/s] 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 // 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 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] const int GPS_L2_M_CODE_LENGTH_CHIPS = 10230; //!< GPS L2 M code length [chips]

3
src/core/system_parameters/Galileo_E1.h
View File

@ -37,7 +37,6 @@
#include <vector> #include <vector>
#include <utility> // std::pair #include <utility> // std::pair
#include "MATH_CONSTANTS.h" #include "MATH_CONSTANTS.h"
#include "gnss_frequencies.h"
// Physical constants // Physical constants
const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD 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)] const double GALILEO_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)]
// carrier and code frequencies // 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_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_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] const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-carrier 'a' rate [Hz]

4
src/core/system_parameters/Galileo_E5a.h
View File

@ -35,11 +35,11 @@
#include <vector> #include <vector>
#include <utility> // std::pair #include <utility> // std::pair
#include "MATH_CONSTANTS.h" #include "MATH_CONSTANTS.h"
#include "gnss_frequencies.h"
// Physical constants already defined in E1
// Carrier and code frequencies // 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_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_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] const double Galileo_E5a_Q_TIERED_CODE_PERIOD = 0.100; //!< Galileo E5a-Q tiered code period [s]

18
src/core/system_parameters/MATH_CONSTANTS.h
View File

@ -40,9 +40,6 @@
PI_TWO_PX ==> Pi*2^X PI_TWO_PX ==> Pi*2^X
ONE_PI_TWO_PX = (1/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_P4 = (16); //!< 2^4
const double TWO_P11 = (2048); //!< 2^11 const double TWO_P11 = (2048); //!< 2^11
const double TWO_P12 = (4096); //!< 2^12 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_N2 = (0.25); //!< 2^-2
const double TWO_N5 = (0.03125); //!< 2^-5 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_N8 = (0.00390625); //!< 2^-8
const double TWO_N9 = (0.001953125); //!< 2^-9 const double TWO_N9 = (0.001953125); //!< 2^-9
const double TWO_N10 = (0.0009765625); //!< 2^-10 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_N14 = (0.00006103515625); //!< 2^-14
const double TWO_N15 = (0.00003051757813); //!< 2^-15 const double TWO_N15 = (0.00003051757813); //!< 2^-15
const double TWO_N16 = (0.0000152587890625); //!< 2^-16 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_N19 = (1.907348632812500e-006); //!< 2^-19
const double TWO_N20 = (9.536743164062500e-007); //!< 2^-20 const double TWO_N20 = (9.536743164062500e-007); //!< 2^-20
const double TWO_N21 = (4.768371582031250e-007); //!< 2^-21 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_N24 = (5.960464477539063e-008); //!< 2^-24
const double TWO_N25 = (2.980232238769531e-008); //!< 2^-25 const double TWO_N25 = (2.980232238769531e-008); //!< 2^-25
const double TWO_N27 = (7.450580596923828e-009); //!< 2^-27 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_N33 = (1.164153218269348e-010); //!< 2^-33
const double TWO_N34 = (5.82076609134674e-011); //!< 2^-34 const double TWO_N34 = (5.82076609134674e-011); //!< 2^-34
const double TWO_N35 = (2.91038304567337e-011); //!< 2^-35 const double TWO_N35 = (2.91038304567337e-011); //!< 2^-35
const double TWO_N38 = (3.637978807091713e-012); //!< 2^-38 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_N43 = (1.136868377216160e-013); //!< 2^-43
const double TWO_N44 = (5.684341886080802e-14); //!< 2^-44 const double TWO_N44 = (5.684341886080802e-14); //!< 2^-44
const double TWO_N46 = (1.4210854715202e-014); //!< 2^-46 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_N38 = (1.142904749427469e-011); //!< Pi*2^-38
const double PI_TWO_N23 = (3.745070282923929e-007); //!< Pi*2^-23 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_ */ #endif /* GNSS_SDR_MATH_CONSTANTS_H_ */

53
src/core/system_parameters/gnss_frequencies.h
View File

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