代码拉取完成，页面将自动刷新
同步操作将从 Anna Catherine/GNSS 强制同步，此操作会覆盖自 Fork 仓库以来所做的任何修改，且无法恢复！！！
确定后同步将在后台操作，完成时将刷新页面，请耐心等待。
#define _POSIX_C_SOURCE 199506
#include <stdarg.h>
#include <ctype.h>
#include "rtcm.h"
#ifndef WIN32
#include <dirent.h>
#include <time.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <sys/types.h>
#endif
/* constants -----------------------------------------------------------------*/
#define POLYCRC32 0xEDB88320u /* CRC32 polynomial */
#define POLYCRC24Q 0x1864CFBu /* CRC24Q polynomial */
static const double gpst0[]={1980,1, 6,0,0,0}; /* gps time reference */
static const double gst0 []={1999,8,22,0,0,0}; /* galileo system time reference */
static const double bdt0 []={2006,1, 1,0,0,0}; /* beidou time reference */
static double leaps[MAXLEAPS+1][7]={ /* leap seconds (y,m,d,h,m,s,utc-gpst) */
{2017,1,1,0,0,0,-18},
{2015,7,1,0,0,0,-17},
{2012,7,1,0,0,0,-16},
{2009,1,1,0,0,0,-15},
{2006,1,1,0,0,0,-14},
{1999,1,1,0,0,0,-13},
{1997,7,1,0,0,0,-12},
{1996,1,1,0,0,0,-11},
{1994,7,1,0,0,0,-10},
{1993,7,1,0,0,0, -9},
{1992,7,1,0,0,0, -8},
{1991,1,1,0,0,0, -7},
{1990,1,1,0,0,0, -6},
{1988,1,1,0,0,0, -5},
{1985,7,1,0,0,0, -4},
{1983,7,1,0,0,0, -3},
{1982,7,1,0,0,0, -2},
{1981,7,1,0,0,0, -1},
{0}
};
const double chisqr[100]={ /* chi-sqr(n) (alpha=0.001) */
10.8,13.8,16.3,18.5,20.5,22.5,24.3,26.1,27.9,29.6,
31.3,32.9,34.5,36.1,37.7,39.3,40.8,42.3,43.8,45.3,
46.8,48.3,49.7,51.2,52.6,54.1,55.5,56.9,58.3,59.7,
61.1,62.5,63.9,65.2,66.6,68.0,69.3,70.7,72.1,73.4,
74.7,76.0,77.3,78.6,80.0,81.3,82.6,84.0,85.4,86.7,
88.0,89.3,90.6,91.9,93.3,94.7,96.0,97.4,98.7,100 ,
101 ,102 ,103 ,104 ,105 ,107 ,108 ,109 ,110 ,112 ,
113 ,114 ,115 ,116 ,118 ,119 ,120 ,122 ,123 ,125 ,
126 ,127 ,128 ,129 ,131 ,132 ,133 ,134 ,135 ,137 ,
138 ,139 ,140 ,142 ,143 ,144 ,145 ,147 ,148 ,149
};
const double lam_carr[MAXFREQ]={ /* carrier wave length (m) */
CLIGHT/FREQ1,CLIGHT/FREQ2,CLIGHT/FREQ5,CLIGHT/FREQ6,CLIGHT/FREQ7,
CLIGHT/FREQ8,CLIGHT/FREQ9
};
const prcopt_t prcopt_default={ /* defaults processing options */
PMODE_SINGLE,0,2,SYS_GPS, /* mode,soltype,nf,navsys */
15.0*D2R,{{0,0}}, /* elmin,snrmask */
0,1,1,1, /* sateph,modear,glomodear,bdsmodear */
5,0,10,1, /* maxout,minlock,minfix,armaxiter */
0,0,0,0, /* estion,esttrop,dynamics,tidecorr */
1,0,0,0,0, /* niter,codesmooth,intpref,sbascorr,sbassatsel */
0,0, /* rovpos,refpos */
{100.0,100.0}, /* eratio[] */
{100.0,0.003,0.003,0.0,1.0}, /* err[] */
{30.0,0.03,0.3}, /* std[] */
{1E-4,1E-3,1E-4,1E-1,1E-2,0.0}, /* prn[] */
5E-12, /* sclkstab */
{3.0,0.9999,0.25,0.1,0.05}, /* thresar */
0.0,0.0,0.05, /* elmaskar,almaskhold,thresslip */
30.0,30.0,30.0, /* maxtdif,maxinno,maxgdop */
{0},{0},{0}, /* baseline,ru,rb */
{"",""}, /* anttype */
{{0}},{{0}},{0} /* antdel,pcv,exsats */
};
const solopt_t solopt_default={ /* defaults solution output options */
SOLF_LLH,TIMES_GPST,1,3, /* posf,times,timef,timeu */
0,1,0,0,0,0,0, /* degf,outhead,outopt,outvel,datum,height,geoid */
0,0,0, /* solstatic,sstat,trace */
{0.0,0.0}, /* nmeaintv */
" ","" /* separator/program name */
};
static char *obscodes[]={ /* observation code strings */
"" ,"1C","1P","1W","1Y", "1M","1N","1S","1L","1E", /* 0- 9 */
"1A","1B","1X","1Z","2C", "2D","2S","2L","2X","2P", /* 10-19 */
"2W","2Y","2M","2N","5I", "5Q","5X","7I","7Q","7X", /* 20-29 */
"6A","6B","6C","6X","6Z", "6S","6L","8L","8Q","8X", /* 30-39 */
"2I","2Q","6I","6Q","3I", "3Q","3X","1I","1Q","5A", /* 40-49 */
"5B","5C","9A","9B","9C", "9X","" ,"" ,"" ,"" /* 50-59 */
};
static unsigned char obsfreqs[]={
/* 1:L1/E1, 2:L2/B1, 3:L5/E5a/L3, 4:L6/LEX/B3, 5:E5b/B2, 6:E5(a+b), 7:S */
0, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 0- 9 */
1, 1, 1, 1, 2, 2, 2, 2, 2, 2, /* 10-19 */
2, 2, 2, 2, 3, 3, 3, 5, 5, 5, /* 20-29 */
4, 4, 4, 4, 4, 4, 4, 6, 6, 6, /* 30-39 */
2, 2, 4, 4, 3, 3, 3, 1, 1, 3, /* 40-49 */
3, 3, 7, 7, 7, 7, 0, 0, 0, 0 /* 50-59 */
};
static char codepris[7][MAXFREQ][16]={ /* code priority table */
/* L1/E1 L2/B1 L5/E5a/L3 L6/LEX/B3 E5b/B2 E5(a+b) S */
{"CPYWMNSL","PYWCMNDSLX","IQX" ,"" ,"" ,"" ,"" }, /* GPS */
{"PC" ,"PC" ,"IQX" ,"" ,"" ,"" ,"" }, /* GLO */
{"CABXZ" ,"" ,"IQX" ,"ABCXZ" ,"IQX" ,"IQX" ,"" }, /* GAL */
{"CSLXZ" ,"SLX" ,"IQX" ,"SLX" ,"" ,"" ,"" }, /* QZS */
{"C" ,"" ,"IQX" ,"" ,"" ,"" ,"" }, /* SBS */
{"IQX" ,"IQX" ,"IQX" ,"IQX" ,"IQX" ,"" ,"" }, /* BDS */
{"" ,"" ,"ABCX" ,"" ,"" ,"" ,"ABCX"} /* IRN */
};
static fatalfunc_t *fatalfunc=NULL; /* fatal callback function */
/* crc tables generated by util/gencrc ---------------------------------------*/
static const unsigned int tbl_CRC24Q[]={
0x000000,0x864CFB,0x8AD50D,0x0C99F6,0x93E6E1,0x15AA1A,0x1933EC,0x9F7F17,
0xA18139,0x27CDC2,0x2B5434,0xAD18CF,0x3267D8,0xB42B23,0xB8B2D5,0x3EFE2E,
0xC54E89,0x430272,0x4F9B84,0xC9D77F,0x56A868,0xD0E493,0xDC7D65,0x5A319E,
0x64CFB0,0xE2834B,0xEE1ABD,0x685646,0xF72951,0x7165AA,0x7DFC5C,0xFBB0A7,
0x0CD1E9,0x8A9D12,0x8604E4,0x00481F,0x9F3708,0x197BF3,0x15E205,0x93AEFE,
0xAD50D0,0x2B1C2B,0x2785DD,0xA1C926,0x3EB631,0xB8FACA,0xB4633C,0x322FC7,
0xC99F60,0x4FD39B,0x434A6D,0xC50696,0x5A7981,0xDC357A,0xD0AC8C,0x56E077,
0x681E59,0xEE52A2,0xE2CB54,0x6487AF,0xFBF8B8,0x7DB443,0x712DB5,0xF7614E,
0x19A3D2,0x9FEF29,0x9376DF,0x153A24,0x8A4533,0x0C09C8,0x00903E,0x86DCC5,
0xB822EB,0x3E6E10,0x32F7E6,0xB4BB1D,0x2BC40A,0xAD88F1,0xA11107,0x275DFC,
0xDCED5B,0x5AA1A0,0x563856,0xD074AD,0x4F0BBA,0xC94741,0xC5DEB7,0x43924C,
0x7D6C62,0xFB2099,0xF7B96F,0x71F594,0xEE8A83,0x68C678,0x645F8E,0xE21375,
0x15723B,0x933EC0,0x9FA736,0x19EBCD,0x8694DA,0x00D821,0x0C41D7,0x8A0D2C,
0xB4F302,0x32BFF9,0x3E260F,0xB86AF4,0x2715E3,0xA15918,0xADC0EE,0x2B8C15,
0xD03CB2,0x567049,0x5AE9BF,0xDCA544,0x43DA53,0xC596A8,0xC90F5E,0x4F43A5,
0x71BD8B,0xF7F170,0xFB6886,0x7D247D,0xE25B6A,0x641791,0x688E67,0xEEC29C,
0x3347A4,0xB50B5F,0xB992A9,0x3FDE52,0xA0A145,0x26EDBE,0x2A7448,0xAC38B3,
0x92C69D,0x148A66,0x181390,0x9E5F6B,0x01207C,0x876C87,0x8BF571,0x0DB98A,
0xF6092D,0x7045D6,0x7CDC20,0xFA90DB,0x65EFCC,0xE3A337,0xEF3AC1,0x69763A,
0x578814,0xD1C4EF,0xDD5D19,0x5B11E2,0xC46EF5,0x42220E,0x4EBBF8,0xC8F703,
0x3F964D,0xB9DAB6,0xB54340,0x330FBB,0xAC70AC,0x2A3C57,0x26A5A1,0xA0E95A,
0x9E1774,0x185B8F,0x14C279,0x928E82,0x0DF195,0x8BBD6E,0x872498,0x016863,
0xFAD8C4,0x7C943F,0x700DC9,0xF64132,0x693E25,0xEF72DE,0xE3EB28,0x65A7D3,
0x5B59FD,0xDD1506,0xD18CF0,0x57C00B,0xC8BF1C,0x4EF3E7,0x426A11,0xC426EA,
0x2AE476,0xACA88D,0xA0317B,0x267D80,0xB90297,0x3F4E6C,0x33D79A,0xB59B61,
0x8B654F,0x0D29B4,0x01B042,0x87FCB9,0x1883AE,0x9ECF55,0x9256A3,0x141A58,
0xEFAAFF,0x69E604,0x657FF2,0xE33309,0x7C4C1E,0xFA00E5,0xF69913,0x70D5E8,
0x4E2BC6,0xC8673D,0xC4FECB,0x42B230,0xDDCD27,0x5B81DC,0x57182A,0xD154D1,
0x26359F,0xA07964,0xACE092,0x2AAC69,0xB5D37E,0x339F85,0x3F0673,0xB94A88,
0x87B4A6,0x01F85D,0x0D61AB,0x8B2D50,0x145247,0x921EBC,0x9E874A,0x18CBB1,
0xE37B16,0x6537ED,0x69AE1B,0xEFE2E0,0x709DF7,0xF6D10C,0xFA48FA,0x7C0401,
0x42FA2F,0xC4B6D4,0xC82F22,0x4E63D9,0xD11CCE,0x575035,0x5BC9C3,0xDD8538
};
/* function prototypes -------------------------------------------------------*/
/* fatal error ---------------------------------------------------------------*/
static void fatalerr(const char *format, ...)
{
char msg[1024];
va_list ap;
va_start(ap,format); vsprintf(msg,format,ap); va_end(ap);
if (fatalfunc) fatalfunc(msg);
else fprintf(stderr,"%s",msg);
exit(-9);
}
/* add fatal callback function -------------------------------------------------
* add fatal callback function for mat(),zeros(),imat()
* args : fatalfunc_t *func I callback function
* return : none
* notes : if malloc() failed in return : none
*-----------------------------------------------------------------------------*/
extern void add_fatal(fatalfunc_t *func)
{
fatalfunc=func;
}
/* satellite system+prn/slot number to satellite number ------------------------
* convert satellite system+prn/slot number to satellite number
* args : int sys I satellite system (SYS_GPS,SYS_GLO,...)
* int prn I satellite prn/slot number
* return : satellite number (0:error)
*-----------------------------------------------------------------------------*/
extern int satno(int sys, int prn)
{
if (prn<=0) return 0;
switch (sys) {
case SYS_GPS:
if (prn<MINPRNGPS||MAXPRNGPS<prn) return 0;
return prn-MINPRNGPS+1;
case SYS_GAL:
if (prn<MINPRNGAL||MAXPRNGAL<prn) return 0;
return NSATGPS+NSATGLO+prn-MINPRNGAL+1;
case SYS_QZS:
if (prn<MINPRNQZS||MAXPRNQZS<prn) return 0;
return NSATGPS+NSATGLO+NSATGAL+prn-MINPRNQZS+1;
case SYS_CMP:
if (prn<MINPRNCMP||MAXPRNCMP<prn) return 0;
return NSATGPS+NSATGLO+NSATGAL+NSATQZS+prn-MINPRNCMP+1;
case SYS_IRN:
if (prn<MINPRNIRN||MAXPRNIRN<prn) return 0;
return NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+prn-MINPRNIRN+1;
case SYS_LEO:
if (prn<MINPRNLEO||MAXPRNLEO<prn) return 0;
return NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATIRN+
prn-MINPRNLEO+1;
case SYS_SBS:
if (prn<MINPRNSBS||MAXPRNSBS<prn) return 0;
return NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATIRN+NSATLEO+
prn-MINPRNSBS+1;
}
return 0;
}
/* satellite number to satellite system ----------------------------------------
* convert satellite number to satellite system
* args : int sat I satellite number (1-MAXSAT)
* int *prn IO satellite prn/slot number (NULL: no output)
* return : satellite system (SYS_GPS,SYS_GLO,...)
*-----------------------------------------------------------------------------*/
extern int satsys(int sat, int *prn)
{
int sys=SYS_NONE;
if (sat<=0||MAXSAT<sat) sat=0;
else if (sat<=NSATGPS) {
sys=SYS_GPS; sat+=MINPRNGPS-1;
}
else if ((sat-=NSATGPS)<=NSATGLO) {
sys=SYS_GLO; sat+=MINPRNGLO-1;
}
else if ((sat-=NSATGLO)<=NSATGAL) {
sys=SYS_GAL; sat+=MINPRNGAL-1;
}
else if ((sat-=NSATGAL)<=NSATQZS) {
sys=SYS_QZS; sat+=MINPRNQZS-1;
}
else if ((sat-=NSATQZS)<=NSATCMP) {
sys=SYS_CMP; sat+=MINPRNCMP-1;
}
else if ((sat-=NSATCMP)<=NSATIRN) {
sys=SYS_IRN; sat+=MINPRNIRN-1;
}
else if ((sat-=NSATIRN)<=NSATLEO) {
sys=SYS_LEO; sat+=MINPRNLEO-1;
}
else if ((sat-=NSATLEO)<=NSATSBS) {
sys=SYS_SBS; sat+=MINPRNSBS-1;
}
else sat=0;
if (prn) *prn=sat;
return sys;
}
/* satellite id to satellite number --------------------------------------------
* convert satellite id to satellite number
* args : char *id I satellite id (nn,Gnn,Rnn,Enn,Jnn,Cnn,Inn or Snn)
* return : satellite number (0: error)
* notes : 120-142 and 193-199 are also recognized as sbas and qzss
*-----------------------------------------------------------------------------*/
extern int satid2no(const char *id)
{
int sys,prn;
char code;
if (sscanf(id,"%d",&prn)==1) {
if (MINPRNGPS<=prn&&prn<=MAXPRNGPS) sys=SYS_GPS;
else if (MINPRNSBS<=prn&&prn<=MAXPRNSBS) sys=SYS_SBS;
else if (MINPRNQZS<=prn&&prn<=MAXPRNQZS) sys=SYS_QZS;
else return 0;
return satno(sys,prn);
}
if (sscanf(id,"%c%d",&code,&prn)<2) return 0;
switch (code) {
case 'G': sys=SYS_GPS; prn+=MINPRNGPS-1; break;
case 'R': sys=SYS_GLO; prn+=MINPRNGLO-1; break;
case 'E': sys=SYS_GAL; prn+=MINPRNGAL-1; break;
case 'J': sys=SYS_QZS; prn+=MINPRNQZS-1; break;
case 'C': sys=SYS_CMP; prn+=MINPRNCMP-1; break;
case 'I': sys=SYS_IRN; prn+=MINPRNIRN-1; break;
case 'L': sys=SYS_LEO; prn+=MINPRNLEO-1; break;
case 'S': sys=SYS_SBS; prn+=100; break;
default: return 0;
}
return satno(sys,prn);
}
/* satellite number to satellite id --------------------------------------------
* convert satellite number to satellite id
* args : int sat I satellite number
* char *id O satellite id (Gnn,Rnn,Enn,Jnn,Cnn,Inn or nnn)
* return : none
*-----------------------------------------------------------------------------*/
extern void satno2id(int sat, char *id)
{
int prn;
switch (satsys(sat,&prn)) {
case SYS_GPS: sprintf(id,"G%02d",prn-MINPRNGPS+1); return;
case SYS_GLO: sprintf(id,"R%02d",prn-MINPRNGLO+1); return;
case SYS_GAL: sprintf(id,"E%02d",prn-MINPRNGAL+1); return;
case SYS_CMP: sprintf(id,"C%02d",prn-MINPRNCMP+1); return;
case SYS_IRN: sprintf(id,"I%02d",prn-MINPRNIRN+1); return;
case SYS_LEO: sprintf(id,"L%02d",prn-MINPRNLEO+1); return;
case SYS_SBS: sprintf(id,"%03d" ,prn); return;
}
strcpy(id,"");
}
/* test SNR mask ---------------------------------------------------------------
* test SNR mask
* args : int base I rover or base-station (0:rover,1:base station)
* int freq I frequency (0:L1,1:L2,2:L3,...)
* double el I elevation angle (rad)
* double snr I C/N0 (dBHz)
* snrmask_t *mask I SNR mask
* return : status (1:masked,0:unmasked)
*-----------------------------------------------------------------------------*/
extern int testsnr(int base, int freq, double el, double snr,
const snrmask_t *mask)
{
double minsnr,a;
int i;
if (!mask->ena[base]||freq<0||freq>=NFREQ) return 0;
a=(el*R2D+5.0)/10.0;
i=(int)floor(a); a-=i;
if (i<1) minsnr=mask->mask[freq][0];
else if (i>8) minsnr=mask->mask[freq][8];
else minsnr=(1.0-a)*mask->mask[freq][i-1]+a*mask->mask[freq][i];
return snr<minsnr;
}
/* obs type string to obs code -------------------------------------------------
* convert obs code type string to obs code
* args : char *str I obs code string ("1C","1P","1Y",...)
* int *freq IO frequency (1:L1,2:L2,3:L5,4:L6,5:L7,6:L8,0:err)
* (NULL: no output)
* return : obs code (CODE_???)
* notes : obs codes are based on reference [6] and qzss extension
*-----------------------------------------------------------------------------*/
extern unsigned char obs2code(const char *obs, int *freq)
{
int i;
if (freq) *freq=0;
for (i=1;*obscodes[i];i++) {
if (strcmp(obscodes[i],obs)) continue;
if (freq) *freq=obsfreqs[i];
return (unsigned char)i;
}
return CODE_NONE;
}
/* obs code to obs code string -------------------------------------------------
* convert obs code to obs code string
* args : unsigned char code I obs code (CODE_???)
* int *freq IO frequency (NULL: no output)
* (1:L1/E1, 2:L2/B1, 3:L5/E5a/L3, 4:L6/LEX/B3,
5:E5b/B2, 6:E5(a+b), 7:S)
* return : obs code string ("1C","1P","1P",...)
* notes : obs codes are based on reference [6] and qzss extension
*-----------------------------------------------------------------------------*/
extern char *code2obs(unsigned char code, int *freq)
{
if (freq) *freq=0;
if (code<=CODE_NONE||MAXCODE<code) return "";
if (freq) *freq=obsfreqs[code];
return obscodes[code];
}
/* set code priority -----------------------------------------------------------
* set code priority for multiple codes in a frequency
* args : int sys I system (or of SYS_???)
* int freq I frequency (1:L1,2:L2,3:L5,4:L6,5:L7,6:L8,7:L9)
* char *pri I priority of codes (series of code characters)
* (higher priority precedes lower)
* return : none
*-----------------------------------------------------------------------------*/
extern void setcodepri(int sys, int freq, const char *pri)
{
trace(3,"setcodepri:sys=%d freq=%d pri=%s\n",sys,freq,pri);
if (freq<=0||MAXFREQ<freq) return;
if (sys&SYS_GPS) strcpy(codepris[0][freq-1],pri);
if (sys&SYS_GLO) strcpy(codepris[1][freq-1],pri);
if (sys&SYS_GAL) strcpy(codepris[2][freq-1],pri);
if (sys&SYS_QZS) strcpy(codepris[3][freq-1],pri);
if (sys&SYS_SBS) strcpy(codepris[4][freq-1],pri);
if (sys&SYS_CMP) strcpy(codepris[5][freq-1],pri);
if (sys&SYS_IRN) strcpy(codepris[6][freq-1],pri);
}
/* get code priority -----------------------------------------------------------
* get code priority for multiple codes in a frequency
* args : int sys I system (SYS_???)
* unsigned char code I obs code (CODE_???)
* char *opt I code options (NULL:no option)
* return : priority (15:highest-1:lowest,0:error)
*-----------------------------------------------------------------------------*/
extern int getcodepri(int sys, unsigned char code, const char *opt)
{
const char *p,*optstr;
char *obs,str[8]="";
int i,j;
switch (sys) {
case SYS_GPS: i=0; optstr="-GL%2s"; break;
case SYS_GLO: i=1; optstr="-RL%2s"; break;
case SYS_GAL: i=2; optstr="-EL%2s"; break;
case SYS_QZS: i=3; optstr="-JL%2s"; break;
case SYS_SBS: i=4; optstr="-SL%2s"; break;
case SYS_CMP: i=5; optstr="-CL%2s"; break;
case SYS_IRN: i=6; optstr="-IL%2s"; break;
default: return 0;
}
obs=code2obs(code,&j);
/* parse code options */
for (p=opt;p&&(p=strchr(p,'-'));p++) {
if (sscanf(p,optstr,str)<1||str[0]!=obs[0]) continue;
return str[1]==obs[1]?15:0;
}
/* search code priority */
return (p=strchr(codepris[i][j-1],obs[1]))?14-(int)(p-codepris[i][j-1]):0;
}
/* extract unsigned/signed bits ------------------------------------------------
* extract unsigned/signed bits from byte data
* args : unsigned char *buff I byte data
* int pos I bit position from start of data (bits)
* int len I bit length (bits) (len<=32)
* return : extracted unsigned/signed bits
*-----------------------------------------------------------------------------*/
extern unsigned int getbitu(const unsigned char *buff, int pos, int len)
{
unsigned int bits=0;
int i;
for (i=pos;i<pos+len;i++) bits=(bits<<1)+((buff[i/8]>>(7-i%8))&1u);
return bits;
}
extern int getbits(const unsigned char *buff, int pos, int len)
{
unsigned int bits=getbitu(buff,pos,len);
if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits;
return (int)(bits|(~0u<<len)); /* extend sign */
}
/* set unsigned/signed bits ----------------------------------------------------
* set unsigned/signed bits to byte data
* args : unsigned char *buff IO byte data
* int pos I bit position from start of data (bits)
* int len I bit length (bits) (len<=32)
* (unsigned) int I unsigned/signed data
* return : none
*-----------------------------------------------------------------------------*/
extern void setbitu(unsigned char *buff, int pos, int len, unsigned int data)
{
unsigned int mask=1u<<(len-1);
int i;
if (len<=0||32<len) return;
for (i=pos;i<pos+len;i++,mask>>=1) {
if (data&mask) buff[i/8]|=1u<<(7-i%8); else buff[i/8]&=~(1u<<(7-i%8));
}
}
extern void setbits(unsigned char *buff, int pos, int len, int data)
{
if (data<0) data|=1<<(len-1); else data&=~(1<<(len-1)); /* set sign bit */
setbitu(buff,pos,len,(unsigned int)data);
}
/* crc-24q parity --------------------------------------------------------------
* compute crc-24q parity for sbas, rtcm3
* args : unsigned char *buff I data
* int len I data length (bytes)
* return : crc-24Q parity
* notes : see reference [2] A.4.3.3 Parity
*-----------------------------------------------------------------------------*/
extern unsigned int rtk_crc24q(const unsigned char *buff, int len)
{
unsigned int crc=0;
int i;
trace(4,"rtk_crc24q: len=%d\n",len);
for (i=0;i<len;i++) crc=((crc<<8)&0xFFFFFF)^tbl_CRC24Q[(crc>>16)^buff[i]];
return crc;
}
/* decode navigation data word -------------------------------------------------
* check party and decode navigation data word
* args : unsigned int word I navigation data word (2+30bit)
* (previous word D29*-30* + current word D1-30)
* unsigned char *data O decoded navigation data without parity
* (8bitx3)
* return : status (1:ok,0:parity error)
* notes : see reference [1] 20.3.5.2 user parity algorithm
*-----------------------------------------------------------------------------*/
extern int decode_word(unsigned int word, unsigned char *data)
{
const unsigned int hamming[]={
0xBB1F3480,0x5D8F9A40,0xAEC7CD00,0x5763E680,0x6BB1F340,0x8B7A89C0
};
unsigned int parity=0,w;
int i;
trace(5,"decodeword: word=%08x\n",word);
if (word&0x40000000) word^=0x3FFFFFC0;
for (i=0;i<6;i++) {
parity<<=1;
for (w=(word&hamming[i])>>6;w;w>>=1) parity^=w&1;
}
if (parity!=(word&0x3F)) return 0;
for (i=0;i<3;i++) data[i]=(unsigned char)(word>>(22-i*8));
return 1;
}
/* new matrix ------------------------------------------------------------------
* allocate memory of matrix
* args : int n,m I number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *mat(int n, int m)
{
double *p;
if (n<=0||m<=0) return NULL;
if (!(p=(double *)malloc(sizeof(double)*n*m))) {
fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m);
}
return p;
}
/* new integer matrix ----------------------------------------------------------
* allocate memory of integer matrix
* args : int n,m I number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern int *imat(int n, int m)
{
int *p;
if (n<=0||m<=0) return NULL;
if (!(p=(int *)malloc(sizeof(int)*n*m))) {
fatalerr("integer matrix memory allocation error: n=%d,m=%d\n",n,m);
}
return p;
}
/* zero matrix -----------------------------------------------------------------
* generate new zero matrix
* args : int n,m I number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *zeros(int n, int m)
{
double *p;
#if NOCALLOC
if ((p=mat(n,m))) for (n=n*m-1;n>=0;n--) p[n]=0.0;
#else
if (n<=0||m<=0) return NULL;
if (!(p=(double *)calloc(sizeof(double),n*m))) {
fatalerr("matrix memory allocation error: n=%d,m=%d\n",n,m);
}
#endif
return p;
}
/* identity matrix -------------------------------------------------------------
* generate new identity matrix
* args : int n I number of rows and columns of matrix
* return : matrix pointer (if n<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *eye(int n)
{
double *p;
int i;
if ((p=zeros(n,n))) for (i=0;i<n;i++) p[i+i*n]=1.0;
return p;
}
/* inner product ---------------------------------------------------------------
* inner product of vectors
* args : double *a,*b I vector a,b (n x 1)
* int n I size of vector a,b
* return : a'*b
*-----------------------------------------------------------------------------*/
extern double dot(const double *a, const double *b, int n)
{
double c=0.0;
while (--n>=0) c+=a[n]*b[n];
return c;
}
/* euclid norm -----------------------------------------------------------------
* euclid norm of vector
* args : double *a I vector a (n x 1)
* int n I size of vector a
* return : || a ||
*-----------------------------------------------------------------------------*/
extern double norm(const double *a, int n)
{
return sqrt(dot(a,a,n));
}
/* outer product of 3d vectors -------------------------------------------------
* outer product of 3d vectors
* args : double *a,*b I vector a,b (3 x 1)
* double *c O outer product (a x b) (3 x 1)
* return : none
*-----------------------------------------------------------------------------*/
extern void cross3(const double *a, const double *b, double *c)
{
c[0]=a[1]*b[2]-a[2]*b[1];
c[1]=a[2]*b[0]-a[0]*b[2];
c[2]=a[0]*b[1]-a[1]*b[0];
}
/* normalize 3d vector ---------------------------------------------------------
* normalize 3d vector
* args : double *a I vector a (3 x 1)
* double *b O normlized vector (3 x 1) || b || = 1
* return : status (1:ok,0:error)
*-----------------------------------------------------------------------------*/
extern int normv3(const double *a, double *b)
{
double r;
if ((r=norm(a,3))<=0.0) return 0;
b[0]=a[0]/r;
b[1]=a[1]/r;
b[2]=a[2]/r;
return 1;
}
/* copy matrix -----------------------------------------------------------------
* copy matrix
* args : double *A O destination matrix A (n x m)
* double *B I source matrix B (n x m)
* int n,m I number of rows and columns of matrix
* return : none
*-----------------------------------------------------------------------------*/
extern void matcpy(double *A, const double *B, int n, int m)
{
memcpy(A,B,sizeof(double)*n*m);
}
/* matrix routines -----------------------------------------------------------*/
#ifdef LAPACK /* with LAPACK/BLAS or MKL */
/* multiply matrix (wrapper of blas dgemm) -------------------------------------
* multiply matrix by matrix (C=alpha*A*B+beta*C)
* args : char *tr I transpose flags ("N":normal,"T":transpose)
* int n,k,m I size of (transposed) matrix A,B
* double alpha I alpha
* double *A,*B I (transposed) matrix A (n x m), B (m x k)
* double beta I beta
* double *C IO matrix C (n x k)
* return : none
*-----------------------------------------------------------------------------*/
extern void matmul(const char *tr, int n, int k, int m, double alpha,
const double *A, const double *B, double beta, double *C)
{
int lda=tr[0]=='T'?m:n,ldb=tr[1]=='T'?k:m;
dgemm_((char *)tr,(char *)tr+1,&n,&k,&m,&alpha,(double *)A,&lda,(double *)B,
&ldb,&beta,C,&n);
}
/* inverse of matrix -----------------------------------------------------------
* inverse of matrix (A=A^-1)
* args : double *A IO matrix (n x n)
* int n I size of matrix A
* return : status (0:ok,0>:error)
*-----------------------------------------------------------------------------*/
extern int matinv(double *A, int n)
{
double *work;
int info,lwork=n*16,*ipiv=imat(n,1);
work=mat(lwork,1);
dgetrf_(&n,&n,A,&n,ipiv,&info);
if (!info) dgetri_(&n,A,&n,ipiv,work,&lwork,&info);
free(ipiv); free(work);
return info;
}
/* solve linear equation -------------------------------------------------------
* solve linear equation (X=A\Y or X=A'\Y)
* args : char *tr I transpose flag ("N":normal,"T":transpose)
* double *A I input matrix A (n x n)
* double *Y I input matrix Y (n x m)
* int n,m I size of matrix A,Y
* double *X O X=A\Y or X=A'\Y (n x m)
* return : status (0:ok,0>:error)
* notes : matirix stored by column-major order (fortran convention)
* X can be same as Y
*-----------------------------------------------------------------------------*/
extern int solve(const char *tr, const double *A, const double *Y, int n,
int m, double *X)
{
double *B=mat(n,n);
int info,*ipiv=imat(n,1);
matcpy(B,A,n,n);
matcpy(X,Y,n,m);
dgetrf_(&n,&n,B,&n,ipiv,&info);
if (!info) dgetrs_((char *)tr,&n,&m,B,&n,ipiv,X,&n,&info);
free(ipiv); free(B);
return info;
}
#else /* without LAPACK/BLAS or MKL */
/* multiply matrix -----------------------------------------------------------*/
extern void matmul(const char *tr, int n, int k, int m, double alpha,
const double *A, const double *B, double beta, double *C)
{
double d;
int i,j,x,f=tr[0]=='N'?(tr[1]=='N'?1:2):(tr[1]=='N'?3:4);
for (i=0;i<n;i++) for (j=0;j<k;j++) {
d=0.0;
switch (f) {
case 1: for (x=0;x<m;x++) d+=A[i+x*n]*B[x+j*m]; break;
case 2: for (x=0;x<m;x++) d+=A[i+x*n]*B[j+x*k]; break;
case 3: for (x=0;x<m;x++) d+=A[x+i*m]*B[x+j*m]; break;
case 4: for (x=0;x<m;x++) d+=A[x+i*m]*B[j+x*k]; break;
}
if (beta==0.0) C[i+j*n]=alpha*d; else C[i+j*n]=alpha*d+beta*C[i+j*n];
}
}
/* LU decomposition ----------------------------------------------------------*/
static int ludcmp(double *A, int n, int *indx, double *d)
{
double big,s,tmp,*vv=mat(n,1);
int i,imax=0,j,k;
*d=1.0;
for (i=0;i<n;i++) {
big=0.0; for (j=0;j<n;j++) if ((tmp=fabs(A[i+j*n]))>big) big=tmp;
if (big>0.0) vv[i]=1.0/big; else {free(vv); return -1;}
}
for (j=0;j<n;j++) {
for (i=0;i<j;i++) {
s=A[i+j*n]; for (k=0;k<i;k++) s-=A[i+k*n]*A[k+j*n]; A[i+j*n]=s;
}
big=0.0;
for (i=j;i<n;i++) {
s=A[i+j*n]; for (k=0;k<j;k++) s-=A[i+k*n]*A[k+j*n]; A[i+j*n]=s;
if ((tmp=vv[i]*fabs(s))>=big) {big=tmp; imax=i;}
}
if (j!=imax) {
for (k=0;k<n;k++) {
tmp=A[imax+k*n]; A[imax+k*n]=A[j+k*n]; A[j+k*n]=tmp;
}
*d=-(*d); vv[imax]=vv[j];
}
indx[j]=imax;
if (A[j+j*n]==0.0) {free(vv); return -1;}
if (j!=n-1) {
tmp=1.0/A[j+j*n]; for (i=j+1;i<n;i++) A[i+j*n]*=tmp;
}
}
free(vv);
return 0;
}
/* LU back-substitution ------------------------------------------------------*/
static void lubksb(const double *A, int n, const int *indx, double *b)
{
double s;
int i,ii=-1,ip,j;
for (i=0;i<n;i++) {
ip=indx[i]; s=b[ip]; b[ip]=b[i];
if (ii>=0) for (j=ii;j<i;j++) s-=A[i+j*n]*b[j]; else if (s) ii=i;
b[i]=s;
}
for (i=n-1;i>=0;i--) {
s=b[i]; for (j=i+1;j<n;j++) s-=A[i+j*n]*b[j]; b[i]=s/A[i+i*n];
}
}
/* inverse of matrix ---------------------------------------------------------*/
extern int matinv(double *A, int n)
{
double d,*B;
int i,j,*indx;
indx=imat(n,1); B=mat(n,n); matcpy(B,A,n,n);
if (ludcmp(B,n,indx,&d)) {free(indx); free(B); return -1;}
for (j=0;j<n;j++) {
for (i=0;i<n;i++) A[i+j*n]=0.0;
A[j+j*n]=1.0;
lubksb(B,n,indx,A+j*n);
}
free(indx); free(B);
return 0;
}
/* solve linear equation -----------------------------------------------------*/
extern int solve(const char *tr, const double *A, const double *Y, int n,
int m, double *X)
{
double *B=mat(n,n);
int info;
matcpy(B,A,n,n);
if (!(info=matinv(B,n))) matmul(tr[0]=='N'?"NN":"TN",n,m,n,1.0,B,Y,0.0,X);
free(B);
return info;
}
#endif
/* end of matrix routines ----------------------------------------------------*/
/* least square estimation -----------------------------------------------------
* least square estimation by solving normal equation (x=(A*A')^-1*A*y)
* args : double *A I transpose of (weighted) design matrix (n x m)
* double *y I (weighted) measurements (m x 1)
* int n,m I number of parameters and measurements (n<=m)
* double *x O estmated parameters (n x 1)
* double *Q O esimated parameters covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2))
* matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern int lsq(const double *A, const double *y, int n, int m, double *x,
double *Q)
{
double *Ay;
int info;
if (m<n) return -1;
Ay=mat(n,1);
matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */
matmul("NT",n,n,m,1.0,A,A,0.0,Q); /* Q=A*A' */
if (!(info=matinv(Q,n))) matmul("NN",n,1,n,1.0,Q,Ay,0.0,x); /* x=Q^-1*Ay */
free(Ay);
return info;
}
/* kalman filter ---------------------------------------------------------------
* kalman filter state update as follows:
*
* K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P
*
* args : double *x I 状态向量 (n x 1)
* double *P I 状态协方差矩阵 (n x n)
* double *H I 设计矩阵转置 (n x m)
* double *v I innovation (measurement - model) (m x 1)
* double *R I 测量误差的协方差矩阵 (m x m)
* int n,m I 状态量和测量量数量
* double *xp O 跟新后状态向量 (n x 1)
* double *Pp O 更新后协方差矩阵 (n x n)
* return : status (0:ok,<0:error)
* notes : matirix stored by column-major order (fortran convention)
* if state x[i]==0.0, not updates state x[i]/P[i+i*n]
*-----------------------------------------------------------------------------*/
static int filter_(const double *x, const double *P, const double *H,
const double *v, const double *R, int n, int m,
double *xp, double *Pp)
{
double *F=mat(n,m),*Q=mat(m,m),*K=mat(n,m),*I=eye(n);
int info;
matcpy(Q,R,m,m);
matcpy(xp,x,n,1);
matmul("NN",n,m,n,1.0,P,H,0.0,F); /* Q=H'*P*H+R */
matmul("TN",m,m,n,1.0,H,F,1.0,Q);
if (!(info=matinv(Q,m))) {
matmul("NN",n,m,m,1.0,F,Q,0.0,K); /* K=P*H*Q^-1 */
matmul("NN",n,1,m,1.0,K,v,1.0,xp); /* xp=x+K*v */
matmul("NT",n,n,m,-1.0,K,H,1.0,I); /* Pp=(I-K*H')*P */
matmul("NN",n,n,n,1.0,I,P,0.0,Pp);
}
free(F); free(Q); free(K); free(I);
return info;
}
extern int filter(double *x, double *P, const double *H, const double *v,
const double *R, int n, int m)
{
double *x_,*xp_,*P_,*Pp_,*H_;
int i,j,k,info,*ix;
ix=imat(n,1); for (i=k=0;i<n;i++) if (x[i]!=0.0&&P[i+i*n]>0.0) ix[k++]=i;
x_=mat(k,1); xp_=mat(k,1); P_=mat(k,k); Pp_=mat(k,k); H_=mat(k,m);
for (i=0;i<k;i++) {
x_[i]=x[ix[i]];
for (j=0;j<k;j++) P_[i+j*k]=P[ix[i]+ix[j]*n];
for (j=0;j<m;j++) H_[i+j*k]=H[ix[i]+j*n];
}
info=filter_(x_,P_,H_,v,R,k,m,xp_,Pp_);
for (i=0;i<k;i++) {
x[ix[i]]=xp_[i];
for (j=0;j<k;j++) P[ix[i]+ix[j]*n]=Pp_[i+j*k];
}
free(ix); free(x_); free(xp_); free(P_); free(Pp_); free(H_);
return info;
}
/* smoother --------------------------------------------------------------------
* combine forward and backward filters by fixed-interval smoother as follows:
*
* xs=Qs*(Qf^-1*xf+Qb^-1*xb), Qs=(Qf^-1+Qb^-1)^-1)
*
* args : double *xf I forward solutions (n x 1)
* args : double *Qf I forward solutions covariance matrix (n x n)
* double *xb I backward solutions (n x 1)
* double *Qb I backward solutions covariance matrix (n x n)
* int n I number of solutions
* double *xs O smoothed solutions (n x 1)
* double *Qs O smoothed solutions covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes : see reference [4] 5.2
* matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern int smoother(const double *xf, const double *Qf, const double *xb,
const double *Qb, int n, double *xs, double *Qs)
{
double *invQf=mat(n,n),*invQb=mat(n,n),*xx=mat(n,1);
int i,info=-1;
matcpy(invQf,Qf,n,n);
matcpy(invQb,Qb,n,n);
if (!matinv(invQf,n)&&!matinv(invQb,n)) {
for (i=0;i<n*n;i++) Qs[i]=invQf[i]+invQb[i];
if (!(info=matinv(Qs,n))) {
matmul("NN",n,1,n,1.0,invQf,xf,0.0,xx);
matmul("NN",n,1,n,1.0,invQb,xb,1.0,xx);
matmul("NN",n,1,n,1.0,Qs,xx,0.0,xs);
}
}
free(invQf); free(invQb); free(xx);
return info;
}
/* print matrix ----------------------------------------------------------------
* print matrix to stdout
* args : double *A I matrix A (n x m)
* int n,m I number of rows and columns of A
* int p,q I total columns, columns under decimal point
* (FILE *fp I output file pointer)
* return : none
* notes : matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern void matfprint(const double A[], int n, int m, int p, int q, FILE *fp)
{
int i,j;
for (i=0;i<n;i++) {
for (j=0;j<m;j++) fprintf(fp," %*.*f",p,q,A[i+j*n]);
fprintf(fp,"\n");
}
}
extern void matprint(const double A[], int n, int m, int p, int q)
{
matfprint(A,n,m,p,q,stdout);
}
/* string to number ------------------------------------------------------------
* convert substring in string to number
* args : char *s I string ("... nnn.nnn ...")
* int i,n I substring position and width
* return : converted number (0.0:error)
*-----------------------------------------------------------------------------*/
extern double str2num(const char *s, int i, int n)
{
double value;
char str[256],*p=str;
if (i<0||(int)strlen(s)<i||(int)sizeof(str)-1<n) return 0.0;
for (s+=i;*s&&--n>=0;s++) *p++=*s=='d'||*s=='D'?'E':*s;
*p='\0';
return sscanf(str,"%lf",&value)==1?value:0.0;
}
/* string to time --------------------------------------------------------------
* convert substring in string to gtime_t struct
* args : char *s I string ("... yyyy mm dd hh mm ss ...")
* int i,n I substring position and width
* gtime_t *t O gtime_t struct
* return : status (0:ok,0>:error)
*-----------------------------------------------------------------------------*/
extern int str2time(const char *s, int i, int n, gtime_t *t)
{
double ep[6];
char str[256],*p=str;
if (i<0||(int)strlen(s)<i||(int)sizeof(str)-1<i) return -1;
for (s+=i;*s&&--n>=0;) *p++=*s++;
*p='\0';
if (sscanf(str,"%lf %lf %lf %lf %lf %lf",ep,ep+1,ep+2,ep+3,ep+4,ep+5)<6)
return -1;
if (ep[0]<100.0) ep[0]+=ep[0]<80.0?2000.0:1900.0;
*t=epoch2time(ep);
return 0;
}
/* convert calendar day/time to time -------------------------------------------
* convert calendar day/time to gtime_t struct
* args : double *ep I day/time {year,month,day,hour,min,sec}
* return : gtime_t struct
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
extern gtime_t epoch2time(const double *ep)
{
const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335};
gtime_t time={0};
int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2];
if (year<1970||2099<year||mon<1||12<mon) return time;
/* leap year if year%4==0 in 1901-2099 */
days=(year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4==0&&mon>=3?1:0);
sec=(int)floor(ep[5]);
time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec;
time.sec=ep[5]-sec;
return time;
}
/* time to calendar day/time ---------------------------------------------------
* convert gtime_t struct to calendar day/time
* args : gtime_t t I gtime_t struct
* double *ep O day/time {year,month,day,hour,min,sec}
* return : none
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
extern void time2epoch(gtime_t t, double *ep)
{
const int mday[]={ /* # of days in a month */
31,28,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31,
31,29,31,30,31,30,31,31,30,31,30,31,31,28,31,30,31,30,31,31,30,31,30,31
};
int days,sec,mon,day;
/* leap year if year%4==0 in 1901-2099 */
days=(int)(t.time/86400);
sec=(int)(t.time-(time_t)days*86400);
for (day=days%1461,mon=0;mon<48;mon++) {
if (day>=mday[mon]) day-=mday[mon]; else break;
}
ep[0]=1970+days/1461*4+mon/12; ep[1]=mon%12+1; ep[2]=day+1;
ep[3]=sec/3600; ep[4]=sec%3600/60; ep[5]=sec%60+t.sec;
}
/* gps time to time ------------------------------------------------------------
* convert week and tow in gps time to gtime_t struct
* args : int week I week number in gps time
* double sec I time of week in gps time (s)
* return : gtime_t struct
*-----------------------------------------------------------------------------*/
extern gtime_t gpst2time(int week, double sec)
{
gtime_t t=epoch2time(gpst0);
if (sec<-1E9||1E9<sec) sec=0.0;
t.time+=(time_t)86400*7*week+(int)sec;
t.sec=sec-(int)sec;
return t;
}
/* time to gps time ------------------------------------------------------------
* convert gtime_t struct to week and tow in gps time
* args : gtime_t t I gtime_t struct
* int *week IO week number in gps time (NULL: no output)
* return : time of week in gps time (s)
*-----------------------------------------------------------------------------*/
extern double time2gpst(gtime_t t, int *week)
{
gtime_t t0=epoch2time(gpst0);
time_t sec=t.time-t0.time;
int w=(int)(sec/(86400*7));
if (week) *week=w;
return (double)(sec-(double)w*86400*7)+t.sec;
}
/* galileo system time to time -------------------------------------------------
* convert week and tow in galileo system time (gst) to gtime_t struct
* args : int week I week number in gst
* double sec I time of week in gst (s)
* return : gtime_t struct
*-----------------------------------------------------------------------------*/
extern gtime_t gst2time(int week, double sec)
{
gtime_t t=epoch2time(gst0);
if (sec<-1E9||1E9<sec) sec=0.0;
t.time+=(time_t)86400*7*week+(int)sec;
t.sec=sec-(int)sec;
return t;
}
/* time to galileo system time -------------------------------------------------
* convert gtime_t struct to week and tow in galileo system time (gst)
* args : gtime_t t I gtime_t struct
* int *week IO week number in gst (NULL: no output)
* return : time of week in gst (s)
*-----------------------------------------------------------------------------*/
extern double time2gst(gtime_t t, int *week)
{
gtime_t t0=epoch2time(gst0);
time_t sec=t.time-t0.time;
int w=(int)(sec/(86400*7));
if (week) *week=w;
return (double)(sec-(double)w*86400*7)+t.sec;
}
/* beidou time (bdt) to time ---------------------------------------------------
* convert week and tow in beidou time (bdt) to gtime_t struct
* args : int week I week number in bdt
* double sec I time of week in bdt (s)
* return : gtime_t struct
*-----------------------------------------------------------------------------*/
extern gtime_t bdt2time(int week, double sec)
{
gtime_t t=epoch2time(bdt0);
if (sec<-1E9||1E9<sec) sec=0.0;
t.time+=(time_t)86400*7*week+(int)sec;
t.sec=sec-(int)sec;
return t;
}
/* time to beidouo time (bdt) --------------------------------------------------
* convert gtime_t struct to week and tow in beidou time (bdt)
* args : gtime_t t I gtime_t struct
* int *week IO week number in bdt (NULL: no output)
* return : time of week in bdt (s)
*-----------------------------------------------------------------------------*/
extern double time2bdt(gtime_t t, int *week)
{
gtime_t t0=epoch2time(bdt0);
time_t sec=t.time-t0.time;
int w=(int)(sec/(86400*7));
if (week) *week=w;
return (double)(sec-(double)w*86400*7)+t.sec;
}
/* add time --------------------------------------------------------------------
* add time to gtime_t struct
* args : gtime_t t I gtime_t struct
* double sec I time to add (s)
* return : gtime_t struct (t+sec)
*-----------------------------------------------------------------------------*/
extern gtime_t timeadd(gtime_t t, double sec)
{
double tt;
t.sec+=sec; tt=floor(t.sec); t.time+=(int)tt; t.sec-=tt;
return t;
}
/* time difference -------------------------------------------------------------
* difference between gtime_t structs
* args : gtime_t t1,t2 I gtime_t structs
* return : time difference (t1-t2) (s)
*-----------------------------------------------------------------------------*/
extern double timediff(gtime_t t1, gtime_t t2)
{
return difftime(t1.time,t2.time)+t1.sec-t2.sec;
}
/* get current time in utc -----------------------------------------------------
* get current time in utc
* args : none
* return : current time in utc
*-----------------------------------------------------------------------------*/
static double timeoffset_=0.0; /* time offset (s) */
extern gtime_t timeget(void)
{
gtime_t time;
double ep[6]={0};
#ifdef WIN32
SYSTEMTIME ts;
GetSystemTime(&ts); /* utc */
ep[0]=ts.wYear; ep[1]=ts.wMonth; ep[2]=ts.wDay;
ep[3]=ts.wHour; ep[4]=ts.wMinute; ep[5]=ts.wSecond+ts.wMilliseconds*1E-3;
#else
struct timeval tv;
struct tm *tt;
if (!gettimeofday(&tv,NULL)&&(tt=gmtime(&tv.tv_sec))) {
ep[0]=tt->tm_year+1900; ep[1]=tt->tm_mon+1; ep[2]=tt->tm_mday;
ep[3]=tt->tm_hour; ep[4]=tt->tm_min; ep[5]=tt->tm_sec+tv.tv_usec*1E-6;
}
#endif
time=epoch2time(ep);
#ifdef CPUTIME_IN_GPST /* cputime operated in gpst */
time=gpst2utc(time);
#endif
return timeadd(time,timeoffset_);
}
/* set current time in utc -----------------------------------------------------
* set current time in utc
* args : gtime_t I current time in utc
* return : none
* notes : just set time offset between cpu time and current time
* the time offset is reflected to only timeget()
* not reentrant
*-----------------------------------------------------------------------------*/
extern void timeset(gtime_t t)
{
timeoffset_+=timediff(t,timeget());
}
/* read leap seconds table by text -------------------------------------------*/
static int read_leaps_text(FILE *fp)
{
char buff[256],*p;
int i,n=0,ep[6],ls;
rewind(fp);
while (fgets(buff,sizeof(buff),fp)&&n<MAXLEAPS) {
if ((p=strchr(buff,'#'))) *p='\0';
if (sscanf(buff,"%d %d %d %d %d %d %d",ep,ep+1,ep+2,ep+3,ep+4,ep+5,
&ls)<7) continue;
for (i=0;i<6;i++) leaps[n][i]=ep[i];
leaps[n++][6]=ls;
}
return n;
}
/* read leap seconds table by usno -------------------------------------------*/
static int read_leaps_usno(FILE *fp)
{
static const char *months[]={
"JAN","FEB","MAR","APR","MAY","JUN","JUL","AUG","SEP","OCT","NOV","DEC"
};
double jd,tai_utc;
char buff[256],month[32],ls[MAXLEAPS][7]={{0}};
int i,j,y,m,d,n=0;
rewind(fp);
while (fgets(buff,sizeof(buff),fp)&&n<MAXLEAPS) {
if (sscanf(buff,"%d %s %d =JD %lf TAI-UTC= %lf",&y,month,&d,&jd,
&tai_utc)<5) continue;
if (y<1980) continue;
for (m=1;m<=12;m++) if (!strcmp(months[m-1],month)) break;
if (m>=13) continue;
ls[n][0]=y;
ls[n][1]=m;
ls[n][2]=d;
ls[n++][6]=(char)(19.0-tai_utc);
}
for (i=0;i<n;i++) for (j=0;j<7;j++) {
leaps[i][j]=ls[n-i-1][j];
}
return n;
}
/* read leap seconds table -----------------------------------------------------
* read leap seconds table
* args : char *file I leap seconds table file
* return : status (1:ok,0:error)
* notes : The leap second table should be as follows or leapsec.dat provided
* by USNO.
* (1) The records in the table file cosist of the following fields:
* year month day hour min sec UTC-GPST(s)
* (2) The date and time indicate the start UTC time for the UTC-GPST
* (3) The date and time should be descending order.
*-----------------------------------------------------------------------------*/
extern int read_leaps(const char *file)
{
FILE *fp;
int i,n;
if (!(fp=fopen(file,"r"))) return 0;
/* read leap seconds table by text or usno */
if (!(n=read_leaps_text(fp))&&!(n=read_leaps_usno(fp))) {
fclose(fp);
return 0;
}
for (i=0;i<7;i++) leaps[n][i]=0.0;
fclose(fp);
return 1;
}
/* gpstime to utc --------------------------------------------------------------
* convert gpstime to utc considering leap seconds
* args : gtime_t t I time expressed in gpstime
* return : time expressed in utc
* notes : ignore slight time offset under 100 ns
*-----------------------------------------------------------------------------*/
extern gtime_t gpst2utc(gtime_t t)
{
gtime_t tu;
int i;
for (i=0;leaps[i][0]>0;i++) {
tu=timeadd(t,leaps[i][6]);
if (timediff(tu,epoch2time(leaps[i]))>=0.0) return tu;
}
return t;
}
/* utc to gpstime --------------------------------------------------------------
* convert utc to gpstime considering leap seconds
* args : gtime_t t I time expressed in utc
* return : time expressed in gpstime
* notes : ignore slight time offset under 100 ns
*-----------------------------------------------------------------------------*/
extern gtime_t utc2gpst(gtime_t t)
{
int i;
for (i=0;leaps[i][0]>0;i++) {
if (timediff(t,epoch2time(leaps[i]))>=0.0) return timeadd(t,-leaps[i][6]);
}
return t;
}
/* gpstime to bdt --------------------------------------------------------------
* convert gpstime to bdt (beidou navigation satellite system time)
* args : gtime_t t I time expressed in gpstime
* return : time expressed in bdt
* notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC
* no leap seconds in BDT
* ignore slight time offset under 100 ns
*-----------------------------------------------------------------------------*/
extern gtime_t gpst2bdt(gtime_t t)
{
return timeadd(t,-14.0);
}
/* bdt to gpstime --------------------------------------------------------------
* convert bdt (beidou navigation satellite system time) to gpstime
* args : gtime_t t I time expressed in bdt
* return : time expressed in gpstime
* notes : see gpst2bdt()
*-----------------------------------------------------------------------------*/
extern gtime_t bdt2gpst(gtime_t t)
{
return timeadd(t,14.0);
}
/* time to day and sec -------------------------------------------------------*/
static double time2sec(gtime_t time, gtime_t *day)
{
double ep[6],sec;
time2epoch(time,ep);
sec=ep[3]*3600.0+ep[4]*60.0+ep[5];
ep[3]=ep[4]=ep[5]=0.0;
*day=epoch2time(ep);
return sec;
}
/* time to string --------------------------------------------------------------
* convert gtime_t struct to string
* args : gtime_t t I gtime_t struct
* char *s O string ("yyyy/mm/dd hh:mm:ss.ssss")
* int n I number of decimals
* return : none
*-----------------------------------------------------------------------------*/
extern void time2str(gtime_t t, char *s, int n)
{
double ep[6];
if (n<0) n=0; else if (n>12) n=12;
if (1.0-t.sec<0.5/pow(10.0,n)) {t.time++; t.sec=0.0;};
time2epoch(t,ep);
sprintf(s,"%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f",ep[0],ep[1],ep[2],
ep[3],ep[4],n<=0?2:n+3,n<=0?0:n,ep[5]);
}
/* get time string -------------------------------------------------------------
* get time string
* args : gtime_t t I gtime_t struct
* int n I number of decimals
* return : time string
* notes : not reentrant, do not use multiple in a function
*-----------------------------------------------------------------------------*/
extern char *time_str(gtime_t t, int n)
{
static char buff[64];
time2str(t,buff,n);
return buff;
}
/* time to day of year ---------------------------------------------------------
* convert time to day of year
* args : gtime_t t I gtime_t struct
* return : day of year (days)
*-----------------------------------------------------------------------------*/
extern double time2doy(gtime_t t)
{
double ep[6];
time2epoch(t,ep);
ep[1]=ep[2]=1.0; ep[3]=ep[4]=ep[5]=0.0;
return timediff(t,epoch2time(ep))/86400.0+1.0;
}
/* adjust gps week number ------------------------------------------------------
* adjust gps week number using cpu time
* args : int week I not-adjusted gps week number
* return : adjusted gps week number
*-----------------------------------------------------------------------------*/
extern int adjgpsweek(int week)
{
int w;
(void)time2gpst(utc2gpst(timeget()),&w);
if (w<1560) w=1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */
return week+(w-week+512)/1024*1024;
}
/* convert degree to deg-min-sec -----------------------------------------------
* convert degree to degree-minute-second
* args : double deg I degree
* double *dms O degree-minute-second {deg,min,sec}
* int ndec I number of decimals of second
* return : none
*-----------------------------------------------------------------------------*/
extern void deg2dms(double deg, double *dms, int ndec)
{
double sign=deg<0.0?-1.0:1.0,a=fabs(deg);
double unit=pow(0.1,ndec);
dms[0]=floor(a); a=(a-dms[0])*60.0;
dms[1]=floor(a); a=(a-dms[1])*60.0;
dms[2]=floor(a/unit+0.5)*unit;
if (dms[2]>=60.0) {
dms[2]=0.0;
dms[1]+=1.0;
if (dms[1]>=60.0) {
dms[1]=0.0;
dms[0]+=1.0;
}
}
dms[0]*=sign;
}
/* convert deg-min-sec to degree -----------------------------------------------
* convert degree-minute-second to degree
* args : double *dms I degree-minute-second {deg,min,sec}
* return : degree
*-----------------------------------------------------------------------------*/
extern double dms2deg(const double *dms)
{
double sign=dms[0]<0.0?-1.0:1.0;
return sign*(fabs(dms[0])+dms[1]/60.0+dms[2]/3600.0);
}
/* transform ecef to geodetic postion ------------------------------------------
* transform ecef position to geodetic position
* args : double *r I ecef position {x,y,z} (m)
* double *pos O geodetic position {lat,lon,h} (rad,m)
* return : none
* notes : WGS84, ellipsoidal height
*-----------------------------------------------------------------------------*/
extern void ecef2pos(const double* r, double* pos)
{
double e2 = FE_WGS84 * (2.0 - FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp;
for (z = r[2], zk = 0.0; fabs(z - zk) >= 1E-4;) {
zk = z;
sinp = z / sqrt(r2 + z * z);
v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
z = r[2] + v * e2 * sinp;
}
pos[0] = r2 > 1E-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0);
pos[1] = r2 > 1E-12 ? atan2(r[1], r[0]) : 0.0;
pos[2] = sqrt(r2 + z * z) - v;
}
/* transform geodetic to ecef position -----------------------------------------
* transform geodetic position to ecef position
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
* double *r O ecef position {x,y,z} (m)
* return : none
* notes : WGS84, ellipsoidal height
*-----------------------------------------------------------------------------*/
extern void pos2ecef(const double *pos, double *r)
{
double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]);
double e2=FE_WGS84*(2.0-FE_WGS84),v=RE_WGS84/sqrt(1.0-e2*sinp*sinp);
r[0]=(v+pos[2])*cosp*cosl;
r[1]=(v+pos[2])*cosp*sinl;
r[2]=(v*(1.0-e2)+pos[2])*sinp;
}
/* ecef to local coordinate transfromation matrix ------------------------------
* compute ecef to local coordinate transfromation matrix
* args : double *pos I geodetic position {lat,lon} (rad)
* double *E O ecef to local coord transformation matrix (3x3)
* return : none
* notes : matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern void xyz2enu(const double *pos, double *E)
{
double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]);
E[0]=-sinl; E[3]=cosl; E[6]=0.0;
E[1]=-sinp*cosl; E[4]=-sinp*sinl; E[7]=cosp;
E[2]=cosp*cosl; E[5]=cosp*sinl; E[8]=sinp;
}
/* transform ecef vector to local tangental coordinate -------------------------
* transform ecef vector to local tangental coordinate
* args : double *pos I geodetic position {lat,lon} (rad)
* double *r I vector in ecef coordinate {x,y,z}
* double *e O vector in local tangental coordinate {e,n,u}
* return : none
*-----------------------------------------------------------------------------*/
extern void ecef2enu(const double *pos, const double *r, double *e)
{
double E[9];
xyz2enu(pos,E);
matmul("NN",3,1,3,1.0,E,r,0.0,e);
}
/* transform local vector to ecef coordinate -----------------------------------
* transform local tangental coordinate vector to ecef
* args : double *pos I geodetic position {lat,lon} (rad)
* double *e I vector in local tangental coordinate {e,n,u}
* double *r O vector in ecef coordinate {x,y,z}
* return : none
*-----------------------------------------------------------------------------*/
extern void enu2ecef(const double *pos, const double *e, double *r)
{
double E[9];
xyz2enu(pos,E);
matmul("TN",3,1,3,1.0,E,e,0.0,r);
}
/* transform covariance to local tangental coordinate --------------------------
* transform ecef covariance to local tangental coordinate
* args : double *pos I geodetic position {lat,lon} (rad)
* double *P I covariance in ecef coordinate
* double *Q O covariance in local tangental coordinate
* return : none
*-----------------------------------------------------------------------------*/
extern void covenu(const double *pos, const double *P, double *Q)
{
double E[9],EP[9];
xyz2enu(pos,E);
matmul("NN",3,3,3,1.0,E,P,0.0,EP);
matmul("NT",3,3,3,1.0,EP,E,0.0,Q);
}
/* transform local enu coordinate covariance to xyz-ecef -----------------------
* transform local enu covariance to xyz-ecef coordinate
* args : double *pos I geodetic position {lat,lon} (rad)
* double *Q I covariance in local enu coordinate
* double *P O covariance in xyz-ecef coordinate
* return : none
*-----------------------------------------------------------------------------*/
extern void covecef(const double *pos, const double *Q, double *P)
{
double E[9],EQ[9];
xyz2enu(pos,E);
matmul("TN",3,3,3,1.0,E,Q,0.0,EQ);
matmul("NN",3,3,3,1.0,EQ,E,0.0,P);
}
/* coordinate rotation matrix ------------------------------------------------*/
#define Rx(t,X) do { \
(X)[0]=1.0; (X)[1]=(X)[2]=(X)[3]=(X)[6]=0.0; \
(X)[4]=(X)[8]=cos(t); (X)[7]=sin(t); (X)[5]=-(X)[7]; \
} while (0)
#define Ry(t,X) do { \
(X)[4]=1.0; (X)[1]=(X)[3]=(X)[5]=(X)[7]=0.0; \
(X)[0]=(X)[8]=cos(t); (X)[2]=sin(t); (X)[6]=-(X)[2]; \
} while (0)
#define Rz(t,X) do { \
(X)[8]=1.0; (X)[2]=(X)[5]=(X)[6]=(X)[7]=0.0; \
(X)[0]=(X)[4]=cos(t); (X)[3]=sin(t); (X)[1]=-(X)[3]; \
} while (0)
/* debug trace functions -----------------------------------------------------*/
#ifdef TRACE
static FILE *fp_trace=NULL; /* file pointer of trace */
static char file_trace[1024]; /* trace file */
static int level_trace=0; /* level of trace */
static unsigned int tick_trace=0; /* tick time at traceopen (ms) */
static gtime_t time_trace={0}; /* time at traceopen */
static lock_t lock_trace; /* lock for trace */
static void traceswap(void)
{
gtime_t time=utc2gpst(timeget());
char path[1024];
lock(&lock_trace);
if ((int)(time2gpst(time ,NULL)/INT_SWAP_TRAC)==
(int)(time2gpst(time_trace,NULL)/INT_SWAP_TRAC)) {
unlock(&lock_trace);
return;
}
time_trace=time;
if (!reppath(file_trace,path,time,"","")) {
unlock(&lock_trace);
return;
}
if (fp_trace) fclose(fp_trace);
if (!(fp_trace=fopen(path,"w"))) {
fp_trace=stderr;
}
unlock(&lock_trace);
}
extern void traceopen(const char *file)
{
gtime_t time=utc2gpst(timeget());
char path[1024];
reppath(file,path,time,"","");
if (!*path||!(fp_trace=fopen(path,"w"))) fp_trace=stderr;
strcpy(file_trace,file);
tick_trace=tickget();
time_trace=time;
initlock(&lock_trace);
}
extern void traceclose(void)
{
if (fp_trace&&fp_trace!=stderr) fclose(fp_trace);
fp_trace=NULL;
file_trace[0]='\0';
}
extern void tracelevel(int level)
{
level_trace=level;
}
extern void trace(int level, const char *format, ...)
{
va_list ap;
/* print error message to stderr */
if (level<=1) {
va_start(ap,format); vfprintf(stderr,format,ap); va_end(ap);
}
if (!fp_trace||level>level_trace) return;
traceswap();
fprintf(fp_trace,"%d ",level);
va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap);
fflush(fp_trace);
}
extern void tracet(int level, const char *format, ...)
{
va_list ap;
if (!fp_trace||level>level_trace) return;
traceswap();
fprintf(fp_trace,"%d %9.3f: ",level,(tickget()-tick_trace)/1000.0);
va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap);
fflush(fp_trace);
}
extern void tracemat(int level, const double *A, int n, int m, int p, int q)
{
if (!fp_trace||level>level_trace) return;
matfprint(A,n,m,p,q,fp_trace); fflush(fp_trace);
}
extern void traceobs(int level, const obsd_t *obs, int n)
{
char str[64],id[16];
int i;
if (!fp_trace||level>level_trace) return;
for (i=0;i<n;i++) {
time2str(obs[i].time,str,3);
satno2id(obs[i].sat,id);
fprintf(fp_trace," (%2d) %s %-3s rcv%d %13.3f %13.3f %13.3f %13.3f %d %d %d %d %3.1f %3.1f\n",
i+1,str,id,obs[i].rcv,obs[i].L[0],obs[i].L[1],obs[i].P[0],
obs[i].P[1],obs[i].LLI[0],obs[i].LLI[1],obs[i].code[0],
obs[i].code[1],obs[i].SNR[0]*0.25,obs[i].SNR[1]*0.25);
}
fflush(fp_trace);
}
extern void tracenav(int level, const nav_t *nav)
{
char s1[64],s2[64],id[16];
int i;
if (!fp_trace||level>level_trace) return;
for (i=0;i<nav->n;i++) {
time2str(nav->eph[i].toe,s1,0);
time2str(nav->eph[i].ttr,s2,0);
satno2id(nav->eph[i].sat,id);
fprintf(fp_trace,"(%3d) %-3s : %s %s %3d %3d %02x\n",i+1,
id,s1,s2,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].svh);
}
fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gps[0],
nav->ion_gps[1],nav->ion_gps[2],nav->ion_gps[3]);
fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gps[4],
nav->ion_gps[5],nav->ion_gps[6],nav->ion_gps[7]);
fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gal[0],
nav->ion_gal[1],nav->ion_gal[2],nav->ion_gal[3]);
}
extern void tracegnav(int level, const nav_t *nav)
{
char s1[64],s2[64],id[16];
int i;
if (!fp_trace||level>level_trace) return;
for (i=0;i<nav->ng;i++) {
time2str(nav->geph[i].toe,s1,0);
time2str(nav->geph[i].tof,s2,0);
satno2id(nav->geph[i].sat,id);
fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d %8.3f\n",i+1,
id,s1,s2,nav->geph[i].frq,nav->geph[i].svh,nav->geph[i].taun*1E6);
}
}
extern void tracehnav(int level, const nav_t *nav)
{
char s1[64],s2[64],id[16];
int i;
if (!fp_trace||level>level_trace) return;
for (i=0;i<nav->ns;i++) {
time2str(nav->seph[i].t0,s1,0);
time2str(nav->seph[i].tof,s2,0);
satno2id(nav->seph[i].sat,id);
fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d\n",i+1,
id,s1,s2,nav->seph[i].svh,nav->seph[i].sva);
}
}
extern void tracepeph(int level, const nav_t *nav)
{
char s[64],id[16];
int i,j;
if (!fp_trace||level>level_trace) return;
for (i=0;i<nav->ne;i++) {
time2str(nav->peph[i].time,s,0);
for (j=0;j<MAXSAT;j++) {
satno2id(j+1,id);
fprintf(fp_trace,"%-3s %d %-3s %13.3f %13.3f %13.3f %13.3f %6.3f %6.3f %6.3f %6.3f\n",
s,nav->peph[i].index,id,
nav->peph[i].pos[j][0],nav->peph[i].pos[j][1],
nav->peph[i].pos[j][2],nav->peph[i].pos[j][3]*1E9,
nav->peph[i].std[j][0],nav->peph[i].std[j][1],
nav->peph[i].std[j][2],nav->peph[i].std[j][3]*1E9);
}
}
}
extern void tracepclk(int level, const nav_t *nav)
{
char s[64],id[16];
int i,j;
if (!fp_trace||level>level_trace) return;
for (i=0;i<nav->nc;i++) {
time2str(nav->pclk[i].time,s,0);
for (j=0;j<MAXSAT;j++) {
satno2id(j+1,id);
fprintf(fp_trace,"%-3s %d %-3s %13.3f %6.3f\n",
s,nav->pclk[i].index,id,
nav->pclk[i].clk[j][0]*1E9,nav->pclk[i].std[j][0]*1E9);
}
}
}
extern void traceb(int level, const unsigned char *p, int n)
{
int i;
if (!fp_trace||level>level_trace) return;
for (i=0;i<n;i++) fprintf(fp_trace,"%02X%s",*p++,i%8==7?" ":"");
fprintf(fp_trace,"\n");
}
#else
extern void traceopen(const char *file) {}
extern void traceclose(void) {}
extern void tracelevel(int level) {}
extern void trace (int level, const char *format, ...) {}
extern void tracet (int level, const char *format, ...) {}
extern void tracemat(int level, const double *A, int n, int m, int p, int q) {}
extern void traceobs(int level, const obsd_t *obs, int n) {}
extern void tracenav(int level, const nav_t *nav) {}
extern void tracegnav(int level, const nav_t *nav) {}
extern void tracehnav(int level, const nav_t *nav) {}
extern void tracepeph(int level, const nav_t *nav) {}
extern void tracepclk(int level, const nav_t *nav) {}
extern void traceb (int level, const unsigned char *p, int n) {}
#endif /* TRACE */
/* satellite carrier wave length -----------------------------------------------
* get satellite carrier wave lengths
* args : int sat I satellite number
* int frq I frequency index (0:L1,1:L2,2:L5/3,...)
* nav_t *nav I navigation messages
* return : carrier wave length (m) (0.0: error)
*-----------------------------------------------------------------------------*/
extern double satwavelen(int sat, int frq, const nav_t *nav)
{
const double freq_glo[]={FREQ1_GLO,FREQ2_GLO};
const double dfrq_glo[]={DFRQ1_GLO,DFRQ2_GLO};
int i,sys=satsys(sat,NULL);
if (sys==SYS_GLO) {
if (0<=frq&&frq<=1) {
for (i=0;i<nav->ng;i++) {
if (nav->geph[i].sat!=sat) continue;
return CLIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq);
}
}
else if (frq==2) { /* L3 */
return CLIGHT/FREQ3_GLO;
}
}
else if (sys==SYS_CMP) {
if (frq==0) return CLIGHT/FREQ1_CMP; /* B1 */
else if (frq==1) return CLIGHT/FREQ2_CMP; /* B2 */
else if (frq==2) return CLIGHT/FREQ3_CMP; /* B3 */
}
else {
if (frq==0) return CLIGHT/FREQ1; /* L1/E1 */
else if (frq==1) return CLIGHT/FREQ2; /* L2 */
else if (frq==2) return CLIGHT/FREQ5; /* L5/E5a */
else if (frq==3) return CLIGHT/FREQ6; /* L6/LEX */
else if (frq==4) return CLIGHT/FREQ7; /* E5b */
else if (frq==5) return CLIGHT/FREQ8; /* E5a+b */
else if (frq==6) return CLIGHT/FREQ9; /* S */
}
return 0.0;
}
/* geometric distance ----------------------------------------------------------
* compute geometric distance and receiver-to-satellite unit vector
* args : double *rs I satellilte position (ecef at transmission) (m)
* double *rr I receiver position (ecef at reception) (m)
* double *e O line-of-sight vector (ecef)
* return : geometric distance (m) (0>:error/no satellite position)
* notes : distance includes sagnac effect correction
*-----------------------------------------------------------------------------*/
extern double geodist(const double *rs, const double *rr, double *e)
{
double r;
int i;
if (norm(rs,3)<RE_WGS84) return -1.0;
for (i=0;i<3;i++) e[i]=rs[i]-rr[i];
r=norm(e,3);
for (i=0;i<3;i++) e[i]/=r;
return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;
}
/* satellite azimuth/elevation angle -------------------------------------------
* compute satellite azimuth/elevation angle
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
* double *e I receiver-to-satellilte unit vevtor (ecef)
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
* return : elevation angle (rad)
*-----------------------------------------------------------------------------*/
extern double satazel(const double* pos, const double* e, double* azel)
{
double az = 0.0, el = PI / 2.0, enu[3];
if (pos[2] > -RE_WGS84) {
ecef2enu(pos, e, enu);
az = dot(enu, enu, 2) < 1E-12 ? 0.0 : atan2(enu[0], enu[1]);
if (az < 0.0) az += 2 * PI;
el = asin(enu[2]);
}
if (azel) {
azel[0] = az; azel[1] = el;
}
return el;
}
/* compute dops ----------------------------------------------------------------
* compute DOP (dilution of precision)
* args : int ns I number of satellites
* double *azel I satellite azimuth/elevation angle (rad)
* double elmin I elevation cutoff angle (rad)
* double *dop O DOPs {GDOP,PDOP,HDOP,VDOP}
* return : none
* notes : dop[0]-[3] return 0 in case of dop computation error
*-----------------------------------------------------------------------------*/
#define SQRT(x) ((x)<0.0||(x)!=(x)?0.0:sqrt(x))
extern void dops(int ns, const double *azel, double elmin, double *dop)
{
double H[4*MAXSAT],Q[16],cosel,sinel;
int i,n;
for (i=0;i<4;i++) dop[i]=0.0;
for (i=n=0;i<ns&&i<MAXSAT;i++) {
if (azel[1+i*2]<elmin||azel[1+i*2]<=0.0) continue;
cosel=cos(azel[1+i*2]);
sinel=sin(azel[1+i*2]);
H[ 4*n]=cosel*sin(azel[i*2]);
H[1+4*n]=cosel*cos(azel[i*2]);
H[2+4*n]=sinel;
H[3+4*n++]=1.0;
}
if (n<4) return;
matmul("NT",4,4,n,1.0,H,H,0.0,Q);
if (!matinv(Q,4)) {
dop[0]=SQRT(Q[0]+Q[5]+Q[10]+Q[15]); /* GDOP */
dop[1]=SQRT(Q[0]+Q[5]+Q[10]); /* PDOP */
dop[2]=SQRT(Q[0]+Q[5]); /* HDOP */
dop[3]=SQRT(Q[10]); /* VDOP */
}
}
/* ionosphere model ------------------------------------------------------------
* compute ionospheric delay by broadcast ionosphere model (klobuchar model)
* args : gtime_t t I time (gpst)
* double *ion I iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3}
* double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* return : ionospheric delay (L1) (m)
*-----------------------------------------------------------------------------*/
extern double ionmodel(gtime_t t, const double *ion, const double *pos,
const double *azel)
{
const double ion_default[]={ /* 2004/1/1 */
0.1118E-07,-0.7451E-08,-0.5961E-07, 0.1192E-06,
0.1167E+06,-0.2294E+06,-0.1311E+06, 0.1049E+07
};
double tt,f,psi,phi,lam,amp,per,x;
int week;
if (pos[2]<-1E3||azel[1]<=0) return 0.0;
if (norm(ion,8)<=0.0) ion=ion_default;
/* earth centered angle (semi-circle) */
psi=0.0137/(azel[1]/PI+0.11)-0.022;
/* subionospheric latitude/longitude (semi-circle) */
phi=pos[0]/PI+psi*cos(azel[0]);
if (phi> 0.416) phi= 0.416;
else if (phi<-0.416) phi=-0.416;
lam=pos[1]/PI+psi*sin(azel[0])/cos(phi*PI);
/* geomagnetic latitude (semi-circle) */
phi+=0.064*cos((lam-1.617)*PI);
/* local time (s) */
tt=43200.0*lam+time2gpst(t,&week);
tt-=floor(tt/86400.0)*86400.0; /* 0<=tt<86400 */
/* slant factor */
f=1.0+16.0*pow(0.53-azel[1]/PI,3.0);
/* ionospheric delay */
amp=ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3]));
per=ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7]));
amp=amp< 0.0? 0.0:amp;
per=per<72000.0?72000.0:per;
x=2.0*PI*(tt-50400.0)/per;
return CLIGHT*f*(fabs(x)<1.57?5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)):5E-9);
}
/* ionosphere mapping function -------------------------------------------------
* compute ionospheric delay mapping function by single layer model
* args : double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* return : ionospheric mapping function
*-----------------------------------------------------------------------------*/
extern double ionmapf(const double *pos, const double *azel)
{
if (pos[2]>=HION) return 1.0;
return 1.0/cos(asin((RE_WGS84+pos[2])/(RE_WGS84+HION)*sin(PI/2.0-azel[1])));
}
/* ionospheric pierce point position -------------------------------------------
* compute ionospheric pierce point (ipp) position and slant factor
* args : double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* double re I earth radius (km)
* double hion I altitude of ionosphere (km)
* double *posp O pierce point position {lat,lon,h} (rad,m)
* return : slant factor
* notes : see ref [2], only valid on the earth surface
* fixing bug on ref [2] A.4.4.10.1 A-22,23
*-----------------------------------------------------------------------------*/
extern double ionppp(const double *pos, const double *azel, double re,
double hion, double *posp)
{
double cosaz,rp,ap,sinap,tanap;
rp=re/(re+hion)*cos(azel[1]);
ap=PI/2.0-azel[1]-asin(rp);
sinap=sin(ap);
tanap=tan(ap);
cosaz=cos(azel[0]);
posp[0]=asin(sin(pos[0])*cos(ap)+cos(pos[0])*sinap*cosaz);
if ((pos[0]> 70.0*D2R&& tanap*cosaz>tan(PI/2.0-pos[0]))||
(pos[0]<-70.0*D2R&&-tanap*cosaz>tan(PI/2.0+pos[0]))) {
posp[1]=pos[1]+PI-asin(sinap*sin(azel[0])/cos(posp[0]));
}
else {
posp[1]=pos[1]+asin(sinap*sin(azel[0])/cos(posp[0]));
}
return 1.0/sqrt(1.0-rp*rp);
}
/* troposphere model -----------------------------------------------------------
* compute tropospheric delay by standard atmosphere and saastamoinen model
* args : gtime_t time I time
* double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* double humi I relative humidity
* return : tropospheric delay (m)
*-----------------------------------------------------------------------------*/
extern double tropmodel(gtime_t time, const double *pos, const double *azel,
double humi)
{
const double temp0=15.0; /* temparature at sea level */
double hgt,pres,temp,e,z,trph,trpw;
if (pos[2]<-100.0||1E4<pos[2]||azel[1]<=0) return 0.0;
/* standard atmosphere */
hgt=pos[2]<0.0?0.0:pos[2];
pres=1013.25*pow(1.0-2.2557E-5*hgt,5.2568);
temp=temp0-6.5E-3*hgt+273.16;
e=6.108*humi*exp((17.15*temp-4684.0)/(temp-38.45));
/* saastamoninen model */
z=PI/2.0-azel[1];
trph=0.0022768*pres/(1.0-0.00266*cos(2.0*pos[0])-0.00028*hgt/1E3)/cos(z);
trpw=0.002277*(1255.0/temp+0.05)*e/cos(z);
return trph+trpw;
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手
