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
e90a9aa2bf
gnss-sdr/src/algorithms/libs/rtklib/rtklib_sbas.cc

911 lines
34 KiB
C++
Raw Normal View History

Migration of the internal LS PVT solver to RTKLIB solver in progress. First working version for GPS L1. Removing SBAS duplicated code.
2017-04-20 14:10:12 +00:00
/*!
* \file rtklib_sbas.cc
* \brief sbas functions
* \authors <ul>
* <li> 2007-2013, T. Takasu
* <li> 2017, Javier Arribas
* <li> 2017, Carles Fernandez
* </ul>
*
* This is a derived work from RTKLIB http://www.rtklib.com/
* The original source code at https://github.com/tomojitakasu/RTKLIB is
* released under the BSD 2-clause license with an additional exclusive clause
* that does not apply here. This additional clause is reproduced below:
*
* " The software package includes some companion executive binaries or shared
* libraries necessary to execute APs on Windows. These licenses succeed to the
* original ones of these software. "
*
* Neither the executive binaries nor the shared libraries are required by, used
* or included in GNSS-SDR.
*
* -------------------------------------------------------------------------
* Copyright (C) 2007-2013, T. Takasu
* Copyright (C) 2017, Javier Arribas
* Copyright (C) 2017, Carles Fernandez
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*
* option : -DRRCENA enable rrc correction
*
* references :
* [1] RTCA/DO-229C, Minimum operational performanc standards for global
* positioning system/wide area augmentation system airborne equipment,
* RTCA inc, November 28, 2001
* [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service
* Interface Specification for QZSS, Japan Aerospace Exploration Agency,
* July 31, 2009
*
* version : $Revision: 1.1 $ $Date: 2008/07/17 21:48:06 $
* history : 2007/10/14 1.0 new
* 2009/01/24 1.1 modify sbspntpos() api
* improve fast/ion correction update
* 2009/04/08 1.2 move function crc24q() to rcvlog.c
* support glonass, galileo and qzss
* 2009/06/08 1.3 modify sbsupdatestat()
* delete sbssatpos()
* 2009/12/12 1.4 support glonass
* 2010/01/22 1.5 support ems (egnos message service) format
* 2010/06/10 1.6 added api:
* sbssatcorr(),sbstropcorr(),sbsioncorr(),
* sbsupdatecorr()
* changed api:
* sbsreadmsgt(),sbsreadmsg()
* deleted api:
* sbspntpos(),sbsupdatestat()
* 2010/08/16 1.7 not reject udre==14 or give==15 correction message
* (2.4.0_p4)
* 2011/01/15 1.8 use api ionppp()
* add prn mask of qzss for qzss L1SAIF
* 2016/07/29 1.9 crc24q() -> rtk_crc24q()
*-----------------------------------------------------------------------------*/
#include "rtklib_sbas.h"
/* extract field from line ---------------------------------------------------*/
char *getfield(char *p, int pos)
{
for (pos--;pos>0;pos--,p++) if (!(p=strchr(p,','))) return NULL;
return p;
}
/* variance of fast correction (udre=UDRE+1) ---------------------------------*/
double varfcorr(int udre)
{
const double var[14]={
0.052,0.0924,0.1444,0.283,0.4678,0.8315,1.2992,1.8709,2.5465,3.326,
5.1968,20.7870,230.9661,2078.695
};
return 0<udre&&udre<=14?var[udre-1]:0.0;
}
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
double varicorr(int give)
{
const double var[15]={
0.0084,0.0333,0.0749,0.1331,0.2079,0.2994,0.4075,0.5322,0.6735,0.8315,
1.1974,1.8709,3.326,20.787,187.0826
};
return 0<give&&give<=15?var[give-1]:0.0;
}
/* fast correction degradation -----------------------------------------------*/
double degfcorr(int ai)
{
const double degf[16]={
0.00000,0.00005,0.00009,0.00012,0.00015,0.00020,0.00030,0.00045,
0.00060,0.00090,0.00150,0.00210,0.00270,0.00330,0.00460,0.00580
};
return 0<ai&&ai<=15?degf[ai]:0.0058;
}
/* decode type 1: prn masks --------------------------------------------------*/
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,n,sat;
trace(4,"decode_sbstype1:\n");
for (i=1,n=0;i<=210&&n<MAXSAT;i++) {
if (getbitu(msg->msg,13+i,1)) {
if (i<= 37) sat=satno(SYS_GPS,i); /* 0- 37: gps */
else if (i<= 61) sat=satno(SYS_GLO,i-37); /* 38- 61: glonass */
else if (i<=119) sat=0; /* 62-119: future gnss */
else if (i<=138) sat=satno(SYS_SBS,i); /* 120-138: geo/waas */
else if (i<=182) sat=0; /* 139-182: reserved */
else if (i<=192) sat=satno(SYS_SBS,i+10); /* 183-192: qzss ref [2] */
else if (i<=202) sat=satno(SYS_QZS,i); /* 193-202: qzss ref [2] */
else sat=0; /* 203- : reserved */
sbssat->sat[n++].sat=sat;
}
}
sbssat->iodp=getbitu(msg->msg,224,2);
sbssat->nsat=n;
trace(5,"decode_sbstype1: nprn=%d iodp=%d\n",n,sbssat->iodp);
return 1;
}
/* decode type 2-5,0: fast corrections ---------------------------------------*/
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,j,iodf,type,udre;
double prc,dt;
gtime_t t0;
trace(4,"decode_sbstype2:\n");
if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)) return 0;
type=getbitu(msg->msg, 8,6);
iodf=getbitu(msg->msg,14,2);
for (i=0;i<13;i++) {
if ((j=13*((type==0?2:type)-2)+i)>=sbssat->nsat) break;
udre=getbitu(msg->msg,174+4*i,4);
t0 =sbssat->sat[j].fcorr.t0;
prc=sbssat->sat[j].fcorr.prc;
sbssat->sat[j].fcorr.t0=gpst2time(msg->week,msg->tow);
sbssat->sat[j].fcorr.prc=getbits(msg->msg,18+i*12,12)*0.125f;
sbssat->sat[j].fcorr.udre=udre+1;
dt=timediff(sbssat->sat[j].fcorr.t0,t0);
if (t0.time==0||dt<=0.0||18.0<dt||sbssat->sat[j].fcorr.ai==0) {
sbssat->sat[j].fcorr.rrc=0.0;
sbssat->sat[j].fcorr.dt=0.0;
}
else {
sbssat->sat[j].fcorr.rrc=(sbssat->sat[j].fcorr.prc-prc)/dt;
sbssat->sat[j].fcorr.dt=dt;
}
sbssat->sat[j].fcorr.iodf=iodf;
}
trace(5,"decode_sbstype2: type=%d iodf=%d\n",type,iodf);
return 1;
}
/* decode type 6: integrity info ---------------------------------------------*/
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,iodf[4],udre;
trace(4,"decode_sbstype6:\n");
for (i=0;i<4;i++) {
iodf[i]=getbitu(msg->msg,14+i*2,2);
}
for (i=0;i<sbssat->nsat&&i<MAXSAT;i++) {
if (sbssat->sat[i].fcorr.iodf!=iodf[i/13]) continue;
udre=getbitu(msg->msg,22+i*4,4);
sbssat->sat[i].fcorr.udre=udre+1;
}
trace(5,"decode_sbstype6: iodf=%d %d %d %d\n",iodf[0],iodf[1],iodf[2],iodf[3]);
return 1;
}
/* decode type 7: fast correction degradation factor -------------------------*/
int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i;
trace(4,"decode_sbstype7\n");
if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)) return 0;
sbssat->tlat=getbitu(msg->msg,14,4);
for (i=0;i<sbssat->nsat&&i<MAXSAT;i++) {
sbssat->sat[i].fcorr.ai=getbitu(msg->msg,22+i*4,4);
}
return 1;
}
/* decode type 9: geo navigation message -------------------------------------*/
int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
{
seph_t seph={};
int i,sat,t;
trace(4,"decode_sbstype9:\n");
if (!(sat=satno(SYS_SBS,msg->prn))) {
trace(2,"invalid prn in sbas type 9: prn=%3d\n",msg->prn);
return 0;
}
t=(int)getbitu(msg->msg,22,13)*16-(int)msg->tow%86400;
if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400;
seph.sat=sat;
seph.t0 =gpst2time(msg->week,msg->tow+t);
seph.tof=gpst2time(msg->week,msg->tow);
seph.sva=getbitu(msg->msg,35,4);
seph.svh=seph.sva==15?1:0; /* unhealthy if ura==15 */
seph.pos[0]=getbits(msg->msg, 39,30)*0.08;
seph.pos[1]=getbits(msg->msg, 69,30)*0.08;
seph.pos[2]=getbits(msg->msg, 99,25)*0.4;
seph.vel[0]=getbits(msg->msg,124,17)*0.000625;
seph.vel[1]=getbits(msg->msg,141,17)*0.000625;
seph.vel[2]=getbits(msg->msg,158,18)*0.004;
seph.acc[0]=getbits(msg->msg,176,10)*0.0000125;
seph.acc[1]=getbits(msg->msg,186,10)*0.0000125;
seph.acc[2]=getbits(msg->msg,196,10)*0.0000625;
seph.af0=getbits(msg->msg,206,12)*P2_31;
seph.af1=getbits(msg->msg,218, 8)*P2_39/2.0;
i=msg->prn-MINPRNSBS;
if (!nav->seph||fabs(timediff(nav->seph[i].t0,seph.t0))<1E-3) { /* not change */
return 0;
}
nav->seph[NSATSBS+i]=nav->seph[i]; /* previous */
nav->seph[i]=seph; /* current */
trace(5,"decode_sbstype9: prn=%d\n",msg->prn);
return 1;
}
/* decode type 18: ionospheric grid point masks ------------------------------*/
int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
{
const sbsigpband_t *p;
int i,j,n,m,band=getbitu(msg->msg,18,4);
trace(4,"decode_sbstype18:\n");
if (0<=band&&band<= 8) {p=igpband1[band ]; m=8;}
else if (9<=band&&band<=10) {p=igpband2[band-9]; m=5;}
else return 0;
sbsion[band].iodi=(short)getbitu(msg->msg,22,2);
for (i=1,n=0;i<=201;i++) {
if (!getbitu(msg->msg,23+i,1)) continue;
for (j=0;j<m;j++) {
if (i<p[j].bits||p[j].bite<i) continue;
sbsion[band].igp[n].lat=band<=8?p[j].y[i-p[j].bits]:p[j].x;
sbsion[band].igp[n++].lon=band<=8?p[j].x:p[j].y[i-p[j].bits];
break;
}
}
sbsion[band].nigp=n;
trace(5,"decode_sbstype18: band=%d nigp=%d\n",band,n);
return 1;
}
/* decode half long term correction (vel code=0) -----------------------------*/
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{
int i,n=getbitu(msg->msg,p,6);
trace(4,"decode_longcorr0:\n");
if (n==0||n>MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8);
for (i=0;i<3;i++) {
sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+9*i,9)*0.125;
sbssat->sat[n-1].lcorr.dvel[i]=0.0;
}
sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+41,10)*P2_31;
sbssat->sat[n-1].lcorr.daf1=0.0;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow);
trace(5,"decode_longcorr0:sat=%2d\n",sbssat->sat[n-1].sat);
return 1;
}
/* decode half long term correction (vel code=1) -----------------------------*/
int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{
int i,n=getbitu(msg->msg,p,6),t;
trace(4,"decode_longcorr1:\n");
if (n==0||n>MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode=getbitu(msg->msg,p+6,8);
for (i=0;i<3;i++) {
sbssat->sat[n-1].lcorr.dpos[i]=getbits(msg->msg,p+14+i*11,11)*0.125;
sbssat->sat[n-1].lcorr.dvel[i]=getbits(msg->msg,p+58+i* 8, 8)*P2_11;
}
sbssat->sat[n-1].lcorr.daf0=getbits(msg->msg,p+47,11)*P2_31;
sbssat->sat[n-1].lcorr.daf1=getbits(msg->msg,p+82, 8)*P2_39;
t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400;
if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t);
trace(5,"decode_longcorr1: sat=%2d\n",sbssat->sat[n-1].sat);
return 1;
}
/* decode half long term correction ------------------------------------------*/
int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{
trace(4,"decode_longcorrh:\n");
if (getbitu(msg->msg,p,1)==0) { /* vel code=0 */
if (sbssat->iodp==(int)getbitu(msg->msg,p+103,2)) {
return decode_longcorr0(msg,p+ 1,sbssat)&&
decode_longcorr0(msg,p+52,sbssat);
}
}
else if (sbssat->iodp==(int)getbitu(msg->msg,p+104,2)) {
return decode_longcorr1(msg,p+1,sbssat);
}
return 0;
}
/* decode type 24: mixed fast/long term correction ---------------------------*/
int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
{
int i,j,iodf,blk,udre;
trace(4,"decode_sbstype24:\n");
if (sbssat->iodp!=(int)getbitu(msg->msg,110,2)) return 0; /* check IODP */
blk =getbitu(msg->msg,112,2);
iodf=getbitu(msg->msg,114,2);
for (i=0;i<6;i++) {
if ((j=13*blk+i)>=sbssat->nsat) break;
udre=getbitu(msg->msg,86+4*i,4);
sbssat->sat[j].fcorr.t0 =gpst2time(msg->week,msg->tow);
sbssat->sat[j].fcorr.prc =getbits(msg->msg,14+i*12,12)*0.125f;
sbssat->sat[j].fcorr.udre=udre+1;
sbssat->sat[j].fcorr.iodf=iodf;
}
return decode_longcorrh(msg,120,sbssat);
}
/* decode type 25: long term satellite error correction ----------------------*/
int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat)
{
trace(4,"decode_sbstype25:\n");
return decode_longcorrh(msg,14,sbssat)&&decode_longcorrh(msg,120,sbssat);
}
/* decode type 26: ionospheric deley corrections -----------------------------*/
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
{
int i,j,block,delay,give,band=getbitu(msg->msg,14,4);
trace(4,"decode_sbstype26:\n");
if (band>MAXBAND||sbsion[band].iodi!=(int)getbitu(msg->msg,217,2)) return 0;
block=getbitu(msg->msg,18,4);
for (i=0;i<15;i++) {
if ((j=block*15+i)>=sbsion[band].nigp) continue;
give=getbitu(msg->msg,22+i*13+9,4);
delay=getbitu(msg->msg,22+i*13,9);
sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow);
sbsion[band].igp[j].delay=delay==0x1FF?0.0f:delay*0.125f;
sbsion[band].igp[j].give=give+1;
if (sbsion[band].igp[j].give>=16) {
sbsion[band].igp[j].give=0;
}
}
trace(5,"decode_sbstype26: band=%d block=%d\n",band,block);
return 1;
}
/* update sbas corrections -----------------------------------------------------
* update sbas correction parameters in navigation data with a sbas message
* args : sbsmg_t *msg I sbas message
* nav_t *nav IO navigation data
* return : message type (-1: error or not supported type)
* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t)
* seph[prn-MINPRNSBS+1] : sat prn current epehmeris
* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris
*-----------------------------------------------------------------------------*/
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
{
int type=getbitu(msg->msg,8,6),stat=-1;
trace(3,"sbsupdatecorr: type=%d\n",type);
if (msg->week==0) return -1;
switch (type) {
case 0: stat=decode_sbstype2 (msg,&nav->sbssat); break;
case 1: stat=decode_sbstype1 (msg,&nav->sbssat); break;
case 2:
case 3:
case 4:
case 5: stat=decode_sbstype2 (msg,&nav->sbssat); break;
case 6: stat=decode_sbstype6 (msg,&nav->sbssat); break;
case 7: stat=decode_sbstype7 (msg,&nav->sbssat); break;
case 9: stat=decode_sbstype9 (msg,nav); break;
case 18: stat=decode_sbstype18(msg,nav ->sbsion); break;
case 24: stat=decode_sbstype24(msg,&nav->sbssat); break;
case 25: stat=decode_sbstype25(msg,&nav->sbssat); break;
case 26: stat=decode_sbstype26(msg,nav ->sbsion); break;
case 63: break; /* null message */
/*default: trace(2,"unsupported sbas message: type=%d\n",type); break;*/
}
return stat?type:-1;
}
/* read sbas log file --------------------------------------------------------*/
void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,
sbs_t *sbs)
{
sbsmsg_t *sbs_msgs;
int i,week,prn,ch,msg;
unsigned int b;
double tow,ep[6]={};
char buff[256],*p;
gtime_t time;
FILE *fp;
trace(3,"readmsgs: file=%s sel=%d\n",file,sel);
if (!(fp=fopen(file,"r"))) {
trace(2,"sbas message file open error: %s\n",file);
return;
}
while (fgets(buff,sizeof(buff),fp)) {
if (sscanf(buff,"%d %lf %d",&week,&tow,&prn)==3&&(p=strstr(buff,": "))) {
p+=2; /* rtklib form */
}
else if (sscanf(buff,"%d %lf %lf %lf %lf %lf %lf %d",
&prn,ep,ep+1,ep+2,ep+3,ep+4,ep+5,&msg)==8) {
/* ems (EGNOS Message Service) form */
ep[0]+=ep[0]<70.0?2000.0:1900.0;
tow=time2gpst(epoch2time(ep),&week);
p=buff+(msg>=10?25:24);
}
else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) { /* NovAtel OEM4/V */
if (!(p=getfield(buff,6))) continue;
if (sscanf(p,"%d,%lf",&week,&tow)<2) continue;
if (!(p=strchr(++p,';'))) continue;
if (sscanf(++p,"%d,%d",&ch,&prn)<2) continue;
if (!(p=getfield(p,4))) continue;
}
else if (!strncmp(buff,"$FRMA",5)) { /* NovAtel OEM3 */
if (!(p=getfield(buff,2))) continue;
if (sscanf(p,"%d,%lf,%d",&week,&tow,&prn)<3) continue;
if (!(p=getfield(p,6))) continue;
if (week<WEEKOFFSET) week+=WEEKOFFSET;
}
else continue;
if (sel!=0&&sel!=prn) continue;
time=gpst2time(week,tow);
if (!screent(time,ts,te,0.0)) continue;
if (sbs->n>=sbs->nmax) {
sbs->nmax=sbs->nmax==0?1024:sbs->nmax*2;
if (!(sbs_msgs=(sbsmsg_t *)realloc(sbs->msgs,sbs->nmax*sizeof(sbsmsg_t)))) {
trace(1,"readsbsmsg malloc error: nmax=%d\n",sbs->nmax);
free(sbs->msgs); sbs->msgs=NULL; sbs->n=sbs->nmax=0;
return;
}
sbs->msgs=sbs_msgs;
}
sbs->msgs[sbs->n].week=week;
sbs->msgs[sbs->n].tow=(int)(tow+0.5);
sbs->msgs[sbs->n].prn=prn;
for (i=0;i<29;i++) sbs->msgs[sbs->n].msg[i]=0;
for (i=0;*(p-1)&&*p&&i<29;p+=2,i++) {
if (sscanf(p,"%2X",&b)==1) sbs->msgs[sbs->n].msg[i]=(unsigned char)b;
}
sbs->msgs[sbs->n++].msg[28]&=0xC0;
}
fclose(fp);
}
/* compare sbas messages -----------------------------------------------------*/
int cmpmsgs(const void *p1, const void *p2)
{
sbsmsg_t *q1=(sbsmsg_t *)p1,*q2=(sbsmsg_t *)p2;
return q1->week!=q2->week?q1->week-q2->week:
(q1->tow<q2->tow?-1:(q1->tow>q2->tow?1:q1->prn-q2->prn));
}
/* read sbas message file ------------------------------------------------------
* read sbas message file
* args : char *file I sbas message file (wind-card * is expanded)
* int sel I sbas satellite prn number selection (0:all)
* (gtime_t ts I start time)
* (gtime_t te I end time )
* sbs_t *sbs IO sbas messages
* return : number of sbas messages
* notes : sbas message are appended and sorted. before calling the funciton,
* sbs->n, sbs->nmax and sbs->msgs must be set properly. (initially
* sbs->n=sbs->nmax=0, sbs->msgs=NULL)
* only the following file extentions after wild card expanded are valid
* to read. others are skipped
* .sbs, .SBS, .ems, .EMS
*-----------------------------------------------------------------------------*/
int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te,
sbs_t *sbs)
{
char *efiles[MAXEXFILE]={},*ext;
int i,n;
trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel);
for (i=0;i<MAXEXFILE;i++) {
if (!(efiles[i]=(char *)malloc(1024))) {
for (i--;i>=0;i--) free(efiles[i]);
return 0;
}
}
/* expand wild card in file path */
n=expath(file,efiles,MAXEXFILE);
for (i=0;i<n;i++) {
if (!(ext=strrchr(efiles[i],'.'))) continue;
if (strcmp(ext,".sbs")&&strcmp(ext,".SBS")&&
strcmp(ext,".ems")&&strcmp(ext,".EMS")) continue;
readmsgs(efiles[i],sel,ts,te,sbs);
}
for (i=0;i<MAXEXFILE;i++) free(efiles[i]);
/* sort messages */
if (sbs->n>0) {
qsort(sbs->msgs,sbs->n,sizeof(sbsmsg_t),cmpmsgs);
}
return sbs->n;
}
int sbsreadmsg(const char *file, int sel, sbs_t *sbs)
{
gtime_t ts={},te={};
trace(3,"sbsreadmsg: file=%s sel=%d\n",file,sel);
return sbsreadmsgt(file,sel,ts,te,sbs);
}
/* output sbas messages --------------------------------------------------------
* output sbas message record to output file in rtklib sbas log format
* args : FILE *fp I output file pointer
* sbsmsg_t *sbsmsg I sbas messages
* return : none
*-----------------------------------------------------------------------------*/
void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg)
{
int i,type=sbsmsg->msg[1]>>2;
trace(4,"sbsoutmsg:\n");
fprintf(fp,"%4d %6d %3d %2d : ",sbsmsg->week,sbsmsg->tow,sbsmsg->prn,type);
for (i=0;i<29;i++) fprintf(fp,"%02X",sbsmsg->msg[i]);
fprintf(fp,"\n");
}
/* search igps ---------------------------------------------------------------*/
void searchigp(gtime_t time, const double *pos, const sbsion_t *ion,
const sbsigp_t **igp, double *x, double *y)
{
int i,latp[2],lonp[4];
double lat=pos[0]*R2D,lon=pos[1]*R2D;
const sbsigp_t *p;
trace(4,"searchigp: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D);
if (lon>=180.0) lon-=360.0;
if (-55.0<=lat&&lat<55.0) {
latp[0]=(int)floor(lat/5.0)*5;
latp[1]=latp[0]+5;
lonp[0]=lonp[1]=(int)floor(lon/5.0)*5;
lonp[2]=lonp[3]=lonp[0]+5;
*x=(lon-lonp[0])/5.0;
*y=(lat-latp[0])/5.0;
}
else {
latp[0]=(int)floor((lat-5.0)/10.0)*10+5;
latp[1]=latp[0]+10;
lonp[0]=lonp[1]=(int)floor(lon/10.0)*10;
lonp[2]=lonp[3]=lonp[0]+10;
*x=(lon-lonp[0])/10.0;
*y=(lat-latp[0])/10.0;
if (75.0<=lat&&lat<85.0) {
lonp[1]=(int)floor(lon/90.0)*90;
lonp[3]=lonp[1]+90;
}
else if (-85.0<=lat&&lat<-75.0) {
lonp[0]=(int)floor((lon-50.0)/90.0)*90+40;
lonp[2]=lonp[0]+90;
}
else if (lat>=85.0) {
for (i=0;i<4;i++) lonp[i]=(int)floor(lon/90.0)*90;
}
else if (lat<-85.0) {
for (i=0;i<4;i++) lonp[i]=(int)floor((lon-50.0)/90.0)*90+40;
}
}
for (i=0;i<4;i++) if (lonp[i]==180) lonp[i]=-180;
for (i=0;i<=MAXBAND;i++) {
for (p=ion[i].igp;p<ion[i].igp+ion[i].nigp;p++) {
if (p->t0.time==0) continue;
if (p->lat==latp[0]&&p->lon==lonp[0]&&p->give>0) igp[0]=p;
else if (p->lat==latp[1]&&p->lon==lonp[1]&&p->give>0) igp[1]=p;
else if (p->lat==latp[0]&&p->lon==lonp[2]&&p->give>0) igp[2]=p;
else if (p->lat==latp[1]&&p->lon==lonp[3]&&p->give>0) igp[3]=p;
if (igp[0]&&igp[1]&&igp[2]&&igp[3]) return;
}
}
}
/* sbas ionospheric delay correction -------------------------------------------
* compute sbas ionosphric delay correction
* args : gtime_t time I time
* nav_t *nav I navigation data
* double *pos I receiver position {lat,lon,height} (rad/m)
* double *azel I satellite azimuth/elavation angle (rad)
* double *delay O slant ionospheric delay (L1) (m)
* double *var O variance of ionospheric delay (m^2)
* return : status (1:ok, 0:no correction)
* notes : before calling the function, sbas ionosphere correction parameters
* in navigation data (nav->sbsion) must be set by callig
* sbsupdatecorr()
*-----------------------------------------------------------------------------*/
int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos,
const double *azel, double *delay, double *var)
{
const double re=6378.1363,hion=350.0;
int i,err=0;
double fp,posp[2],x=0.0,y=0.0,t,w[4]={};
const sbsigp_t *igp[4]={}; /* {ws,wn,es,en} */
trace(4,"sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D,
azel[0]*R2D,azel[1]*R2D);
*delay=*var=0.0;
if (pos[2]<-100.0||azel[1]<=0) return 1;
/* ipp (ionospheric pierce point) position */
fp=ionppp(pos,azel,re,hion,posp);
/* search igps around ipp */
searchigp(time,posp,nav->sbsion,igp,&x,&y);
/* weight of igps */
if (igp[0]&&igp[1]&&igp[2]&&igp[3]) {
w[0]=(1.0-x)*(1.0-y); w[1]=(1.0-x)*y; w[2]=x*(1.0-y); w[3]=x*y;
}
else if (igp[0]&&igp[1]&&igp[2]) {
w[1]=y; w[2]=x;
if ((w[0]=1.0-w[1]-w[2])<0.0) err=1;
}
else if (igp[0]&&igp[2]&&igp[3]) {
w[0]=1.0-x; w[3]=y;
if ((w[2]=1.0-w[0]-w[3])<0.0) err=1;
}
else if (igp[0]&&igp[1]&&igp[3]) {
w[0]=1.0-y; w[3]=x;
if ((w[1]=1.0-w[0]-w[3])<0.0) err=1;
}
else if (igp[1]&&igp[2]&&igp[3]) {
w[1]=1.0-x; w[2]=1.0-y;
if ((w[3]=1.0-w[1]-w[2])<0.0) err=1;
}
else err=1;
if (err) {
trace(2,"no sbas iono correction: lat=%3.0f lon=%4.0f\n",posp[0]*R2D,
posp[1]*R2D);
return 0;
}
for (i=0;i<4;i++) {
if (!igp[i]) continue;
t=timediff(time,igp[i]->t0);
*delay+=w[i]*igp[i]->delay;
*var+=w[i]*varicorr(igp[i]->give)*9E-8*fabs(t);
}
*delay*=fp; *var*=fp*fp;
trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var));
return 1;
}
/* get meterological parameters ----------------------------------------------*/
void getmet(double lat, double *met)
{
static const double metprm[][10]={ /* lat=15,30,45,60,75 */
{1013.25,299.65,26.31,6.30E-3,2.77, 0.00, 0.00,0.00,0.00E-3,0.00},
{1017.25,294.15,21.79,6.05E-3,3.15, -3.75, 7.00,8.85,0.25E-3,0.33},
{1015.75,283.15,11.66,5.58E-3,2.57, -2.25,11.00,7.24,0.32E-3,0.46},
{1011.75,272.15, 6.78,5.39E-3,1.81, -1.75,15.00,5.36,0.81E-3,0.74},
{1013.00,263.65, 4.11,4.53E-3,1.55, -0.50,14.50,3.39,0.62E-3,0.30}
};
int i,j;
double a;
lat=fabs(lat);
if (lat<=15.0) for (i=0;i<10;i++) met[i]=metprm[0][i];
else if (lat>=75.0) for (i=0;i<10;i++) met[i]=metprm[4][i];
else {
j=(int)(lat/15.0); a=(lat-j*15.0)/15.0;
for (i=0;i<10;i++) met[i]=(1.0-a)*metprm[j-1][i]+a*metprm[j][i];
}
}
/* tropospheric delay correction -----------------------------------------------
* compute sbas tropospheric delay correction (mops model)
* args : gtime_t time I time
* double *pos I receiver position {lat,lon,height} (rad/m)
* double *azel I satellite azimuth/elavation (rad)
* double *var O variance of troposphric error (m^2)
* return : slant tropospheric delay (m)
*-----------------------------------------------------------------------------*/
double sbstropcorr(gtime_t time, const double *pos, const double *azel,
double *var)
{
const double k1=77.604,k2=382000.0,rd=287.054,gm=9.784,g=9.80665;
static double pos_[3]={},zh=0.0,zw=0.0;
int i;
double c,met[10],sinel=sin(azel[1]),h=pos[2],m;
trace(4,"sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D,
azel[0]*R2D,azel[1]*R2D);
if (pos[2]<-100.0||10000.0<pos[2]||azel[1]<=0) {
*var=0.0;
return 0.0;
}
if (zh==0.0||fabs(pos[0]-pos_[0])>1E-7||fabs(pos[1]-pos_[1])>1E-7||
fabs(pos[2]-pos_[2])>1.0) {
getmet(pos[0]*R2D,met);
c=cos(2.0*PI*(time2doy(time)-(pos[0]>=0.0?28.0:211.0))/365.25);
for (i=0;i<5;i++) met[i]-=met[i+5]*c;
zh=1E-6*k1*rd*met[0]/gm;
zw=1E-6*k2*rd/(gm*(met[4]+1.0)-met[3]*rd)*met[2]/met[1];
zh*=pow(1.0-met[3]*h/met[1],g/(rd*met[3]));
zw*=pow(1.0-met[3]*h/met[1],(met[4]+1.0)*g/(rd*met[3])-1.0);
for (i=0;i<3;i++) pos_[i]=pos[i];
}
m=1.001/sqrt(0.002001+sinel*sinel);
*var=0.12*0.12*m*m;
return (zh+zw)*m;
}
/* long term correction ------------------------------------------------------*/
int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *drs, double *ddts)
{
const sbssatp_t *p;
double t;
int i;
trace(3,"sbslongcorr: sat=%2d\n",sat);
for (p=sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) {
if (p->sat!=sat||p->lcorr.t0.time==0) continue;
t=timediff(time,p->lcorr.t0);
if (fabs(t)>MAXSBSAGEL) {
trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
time_str(time,0),sat,t);
return 0;
}
for (i=0;i<3;i++) drs[i]=p->lcorr.dpos[i]+p->lcorr.dvel[i]*t;
*ddts=p->lcorr.daf0+p->lcorr.daf1*t;
trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
sat,drs[0],drs[1],drs[2],*ddts*CLIGHT);
return 1;
}
/* if sbas satellite without correction, no correction applied */
if (satsys(sat,NULL)==SYS_SBS) return 1;
trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
/* fast correction -----------------------------------------------------------*/
int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat,
double *prc, double *var)
{
const sbssatp_t *p;
double t;
trace(3,"sbsfastcorr: sat=%2d\n",sat);
for (p=sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) {
if (p->sat!=sat) continue;
if (p->fcorr.t0.time==0) break;
t=timediff(time,p->fcorr.t0)+sbssat->tlat;
/* expire age of correction or UDRE==14 (not monitored) */
if (fabs(t)>MAXSBSAGEF||p->fcorr.udre>=15) continue;
*prc=p->fcorr.prc;
#ifdef RRCENA
if (p->fcorr.ai>0&&fabs(t)<=8.0*p->fcorr.dt) {
*prc+=p->fcorr.rrc*t;
}
#endif
*var=varfcorr(p->fcorr.udre)+degfcorr(p->fcorr.ai)*t*t/2.0;
trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat,
*prc,sqrt(*var),t);
return 1;
}
trace(2,"no sbas fast correction: %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
/* sbas satellite ephemeris and clock correction -------------------------------
* correct satellite position and clock bias with sbas satellite corrections
* args : gtime_t time I reception time
* int sat I satellite
* nav_t *nav I navigation data
* double *rs IO sat position and corrected {x,y,z} (ecef) (m)
* double *dts IO sat clock bias and corrected (s)
* double *var O sat position and clock variance (m^2)
* return : status (1:ok,0:no correction)
* notes : before calling the function, sbas satellite correction parameters
* in navigation data (nav->sbssat) must be set by callig
* sbsupdatecorr().
* satellite clock correction include long-term correction and fast
* correction.
* sbas clock correction is usually based on L1C/A code. TGD or DCB has
* to be considered for other codes
*-----------------------------------------------------------------------------*/
int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
double *dts, double *var)
{
double drs[3]={},dclk=0.0,prc=0.0;
int i;
trace(3,"sbssatcorr : sat=%2d\n",sat);
/* sbas long term corrections */
if (!sbslongcorr(time,sat,&nav->sbssat,drs,&dclk)) {
return 0;
}
/* sbas fast corrections */
if (!sbsfastcorr(time,sat,&nav->sbssat,&prc,var)) {
return 0;
}
for (i=0;i<3;i++) rs[i]+=drs[i];
dts[0]+=dclk+prc/CLIGHT;
trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n",
sat,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var);
return 1;
}
/* decode sbas message ---------------------------------------------------------
* decode sbas message frame words and check crc
* args : gtime_t time I reception time
* int prn I sbas satellite prn number
* unsigned int *word I message frame words (24bit x 10)
* sbsmsg_t *sbsmsg O sbas message
* return : status (1:ok,0:crc error)
*-----------------------------------------------------------------------------*/
int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words,
sbsmsg_t *sbsmsg)
{
int i,j;
unsigned char f[29];
double tow;
trace(5,"sbsdecodemsg: prn=%d\n",prn);
if (time.time==0) return 0;
tow=time2gpst(time,&sbsmsg->week);
sbsmsg->tow=(int)(tow+DTTOL);
sbsmsg->prn=prn;
for (i=0;i<7;i++) for (j=0;j<4;j++) {
sbsmsg->msg[i*4+j]=(unsigned char)(words[i]>>((3-j)*8));
}
sbsmsg->msg[28]=(unsigned char)(words[7]>>18)&0xC0;
for (i=28;i>0;i--) f[i]=(sbsmsg->msg[i]>>6)+(sbsmsg->msg[i-1]<<2);
f[0]=sbsmsg->msg[0]>>6;
return rtk_crc24q(f,29)==(words[7]&0xFFFFFF); /* check crc */
}
Reference in New Issue Copy Permalink