From 770e6f462e9e67fdeb06fe6f42f5286bf1c2f345 Mon Sep 17 00:00:00 2001 From: oaq Date: Mon, 20 Apr 2026 16:36:36 +1000 Subject: [PATCH 01/25] rtknavi_qt: correct freq change in SNR and sky plots --- app/qtapp/rtknavi_qt/navimain.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/app/qtapp/rtknavi_qt/navimain.cpp b/app/qtapp/rtknavi_qt/navimain.cpp index 38237ca8c..a04e2b303 100644 --- a/app/qtapp/rtknavi_qt/navimain.cpp +++ b/app/qtapp/rtknavi_qt/navimain.cpp @@ -1720,12 +1720,12 @@ void MainWindow::drawSolutionPlot(QLabel *plot, int type, int freq) drawText(c, w / 2 + x, 1, s2, Qt::darkGray, 1, 2); } } else if (type == 1) { // snr plot rover - drawSnr(c, w, h - topMargin, 0, topMargin, 0, type); + drawSnr(c, w, h - topMargin, 0, topMargin, 0, freq); s1 = tr("Rover %L1 SNR (dBHz)").arg(fstr[freq]); drawText(c, x, 1, s1, Qt::darkGray, 1, 2); } else if (type == 2) { // skyplot rover - drawSatellites(c, w, h, 0, 0, 0, type); - s1 = tr("Rover %L1").arg(fstr[type]); + drawSatellites(c, w, h, 0, 0, 0, freq); + s1 = tr("Rover %L1").arg(fstr[freq]); drawText(c, x, 1, s1, Qt::darkGray, 1, 2); } else if (type == 3) { // skyplot+snr plot rover s1 = tr("Rover %L1").arg(fstr[freq]); @@ -1921,7 +1921,7 @@ void MainWindow::drawSatellites(QPainter &c, int w, int h, int x0, int y0, radius = QFontMetrics(optDialog->panelFont).height(); c.setBrush(!validSatellites[index][k] ? Color::Silver : - (freq < NFREQ ? snrColor(snr[freq]) : color_sys[sysIdx])); + (freq < NFREQ + 1 ? snrColor(snr[freq]) : color_sys[sysIdx])); c.setPen(Qt::darkGray); color_text = Qt::white; if (freq < NFREQ + 1 && snr[freq] <= 0) { From 590403b23ac8578f4f01a6ac9c870746bdd2686d Mon Sep 17 00:00:00 2001 From: oaq Date: Mon, 20 Apr 2026 20:12:52 +1000 Subject: [PATCH 02/25] preceph: fix code_bias_ix system indexing --- src/preceph.c | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/preceph.c b/src/preceph.c index b3e3928c2..2bb2a90b9 100644 --- a/src/preceph.c +++ b/src/preceph.c @@ -57,9 +57,10 @@ #define MAXDTE 900.0 /* max time difference to ephem time (s) */ #define EXTERR_CLK 1E-3 /* extrapolation error for clock (m/s) */ #define EXTERR_EPH 5E-7 /* extrapolation error for ephem (m/s^2) */ +#define MAX_BIAS_SYS 6 /* # of constellations supported */ /* table to translate code to code bias table index */ -static int8_t code_bias_ix[NSYS][MAXCODE]; +static int8_t code_bias_ix[MAX_BIAS_SYS][MAXCODE]; /* initialize code bias lookup table ------------------------------------------- * -1 = code not supported * 0 = reference code (0 bias) @@ -68,7 +69,7 @@ static int8_t code_bias_ix[NSYS][MAXCODE]; static void init_bias_ix(void) { int i,j; - for (i=0;irbias[i][j][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ -// } +#ifdef RTK_DISABLED + /* Receiver DCBs never used in RTKLIB so remove support */ + for (i=0;irbias[i][j][type-1]=cbias*1E-9*CLIGHT; /* ns -> m */ + } +#endif } else if ((sat=satid2no(str1))) { /* satellite dcb */ nav->cbias[sat-1][type-1][0]=-cbias*1E-9*CLIGHT; /* ns -> m */ @@ -431,7 +434,7 @@ static int sys2ix(int sys) case SYS_QZS: return 4; case SYS_IRN: return 5; } - return 0; + return -1; } /* lookup code bias from table ---------------- * return 0 if not found @@ -444,7 +447,7 @@ extern double code2bias(const nav_t *nav, int sys, int sat, int code, int mode) sys_ix=sys2ix(sys); frq_ix=code2idx(sys,code); - if (frq_ix>=0&&sat<=MAXSAT) { + if (sys_ix >= 0 && sys_ix < MAX_BIAS_SYS && frq_ix >= 0 && sat <= MAXSAT) { code_ix = code_bias_ix[sys_ix][code]; bias=nav->cbias[sat-1][frq_ix][code_ix]; // absolute bias if (mode==0) @@ -475,6 +478,7 @@ static int readbiaf(const char *file, nav_t *nav) sat=satid2no(prn); sys=satsys(sat,NULL); sys_ix=sys2ix(sys); + if (sys_ix < 0 || sys_ix >= MAX_BIAS_SYS) continue; if (!(code1=obs2code(&obs1[1]))) continue; /* skip if code not valid */ if ((frq_ix=code2idx(sys,code1))<0) continue; if ((bias_ix1=code_bias_ix[sys_ix][code1])<0) continue; From b8198ef11ef9f0d860e5a969dbf68164af011f14 Mon Sep 17 00:00:00 2001 From: pleba Date: Wed, 22 Apr 2026 12:12:46 +0200 Subject: [PATCH 03/25] Fix incorrect install targets for some QT apps (launch, navi, plot, post, srctblbrows and strsvr) --- app/qtapp/rtklaunch_qt/CMakeLists.txt | 2 +- app/qtapp/rtknavi_qt/CMakeLists.txt | 2 +- app/qtapp/rtkplot_qt/CMakeLists.txt | 2 +- app/qtapp/rtkpost_qt/CMakeLists.txt | 2 +- app/qtapp/srctblbrows_qt/CMakeLists.txt | 2 +- app/qtapp/strsvr_qt/CMakeLists.txt | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/app/qtapp/rtklaunch_qt/CMakeLists.txt b/app/qtapp/rtklaunch_qt/CMakeLists.txt index 6013ead83..96c2e0cb8 100644 --- a/app/qtapp/rtklaunch_qt/CMakeLists.txt +++ b/app/qtapp/rtklaunch_qt/CMakeLists.txt @@ -26,7 +26,7 @@ set_target_properties(rtklaunch_qt PROPERTIES WIN32_EXECUTABLE ON MACOSX_BUNDLE ON ) -install(TARGETS rtkget_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) +install(TARGETS rtklaunch_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) if (UNIX) install(FILES rtklaunch_qt.desktop DESTINATION ${XDG_APPS_INSTALL_DIR}) install(FILES ../icon/rtklaunch.png DESTINATION ${XDG_APPS_PIXMAPS_DIR}) diff --git a/app/qtapp/rtknavi_qt/CMakeLists.txt b/app/qtapp/rtknavi_qt/CMakeLists.txt index 3e176f7e9..dd8564e0e 100644 --- a/app/qtapp/rtknavi_qt/CMakeLists.txt +++ b/app/qtapp/rtknavi_qt/CMakeLists.txt @@ -81,7 +81,7 @@ set_target_properties(rtknavi_qt PROPERTIES WIN32_EXECUTABLE ON MACOSX_BUNDLE ON ) -install(TARGETS rtkget_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) +install(TARGETS rtknavi_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) if (UNIX) install(FILES rtknavi_qt.desktop DESTINATION ${XDG_APPS_INSTALL_DIR}) install(FILES ../icon/rtknavi.png DESTINATION ${XDG_APPS_PIXMAPS_DIR}) diff --git a/app/qtapp/rtkplot_qt/CMakeLists.txt b/app/qtapp/rtkplot_qt/CMakeLists.txt index ac52c1396..436a0c9e2 100644 --- a/app/qtapp/rtkplot_qt/CMakeLists.txt +++ b/app/qtapp/rtkplot_qt/CMakeLists.txt @@ -90,7 +90,7 @@ set_target_properties(rtkplot_qt PROPERTIES WIN32_EXECUTABLE ON MACOSX_BUNDLE ON ) -install(TARGETS rtkget_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) +install(TARGETS rtkplot_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) if (UNIX) install(FILES rtkplot_qt.desktop DESTINATION ${XDG_APPS_INSTALL_DIR}) install(FILES ../icon/rtkplot.png DESTINATION ${XDG_APPS_PIXMAPS_DIR}) diff --git a/app/qtapp/rtkpost_qt/CMakeLists.txt b/app/qtapp/rtkpost_qt/CMakeLists.txt index 2cd56ddb2..0ce1be10a 100644 --- a/app/qtapp/rtkpost_qt/CMakeLists.txt +++ b/app/qtapp/rtkpost_qt/CMakeLists.txt @@ -51,7 +51,7 @@ set_target_properties(rtkpost_qt PROPERTIES WIN32_EXECUTABLE ON MACOSX_BUNDLE ON ) -install(TARGETS rtkget_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) +install(TARGETS rtkpost_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) if (UNIX) install(FILES rtkpost_qt.desktop DESTINATION ${XDG_APPS_INSTALL_DIR}) install(FILES ../icon/rtkpost.png DESTINATION ${XDG_APPS_PIXMAPS_DIR}) diff --git a/app/qtapp/srctblbrows_qt/CMakeLists.txt b/app/qtapp/srctblbrows_qt/CMakeLists.txt index 0db977e86..05eba4e95 100644 --- a/app/qtapp/srctblbrows_qt/CMakeLists.txt +++ b/app/qtapp/srctblbrows_qt/CMakeLists.txt @@ -51,7 +51,7 @@ set_target_properties(srctblbrows_qt PROPERTIES WIN32_EXECUTABLE ON MACOSX_BUNDLE ON ) -install(TARGETS rtkget_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) +install(TARGETS srctblbrows_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) if (UNIX) install(FILES srctblbrows_qt.desktop DESTINATION ${XDG_APPS_INSTALL_DIR}) install(FILES ../icon/srctblbrows.png DESTINATION ${XDG_APPS_PIXMAPS_DIR}) diff --git a/app/qtapp/strsvr_qt/CMakeLists.txt b/app/qtapp/strsvr_qt/CMakeLists.txt index c3e2aeaa9..6aeb360f1 100644 --- a/app/qtapp/strsvr_qt/CMakeLists.txt +++ b/app/qtapp/strsvr_qt/CMakeLists.txt @@ -53,7 +53,7 @@ set_target_properties(strsvr_qt PROPERTIES WIN32_EXECUTABLE ON MACOSX_BUNDLE ON ) -install(TARGETS rtkget_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) +install(TARGETS strsvr_qt RUNTIME DESTINATION bin BUNDLE DESTINATION bin) if (UNIX) install(FILES strsvr_qt.desktop DESTINATION ${XDG_APPS_INSTALL_DIR}) install(FILES ../icon/strsvr.png DESTINATION ${XDG_APPS_PIXMAPS_DIR}) From fe5026cb8330c3b9db59a6a2d0d62ebc08ab7b5a Mon Sep 17 00:00:00 2001 From: oaq Date: Wed, 22 Apr 2026 22:44:14 +1000 Subject: [PATCH 04/25] rtknavi: fix the track plot expand and shrink for plots 3 and 4 It was missing implementations for plots 3 and 4. --- app/qtapp/rtknavi_qt/navimain.cpp | 23 ++++++++-------- app/winapp/rtknavi/navimain.cpp | 46 +++++++++++++++++++++++++------ app/winapp/rtknavi/navimain.dfm | 8 +++--- app/winapp/rtknavi/navimain.h | 4 +++ 4 files changed, 57 insertions(+), 24 deletions(-) diff --git a/app/qtapp/rtknavi_qt/navimain.cpp b/app/qtapp/rtknavi_qt/navimain.cpp index 38237ca8c..963700167 100644 --- a/app/qtapp/rtknavi_qt/navimain.cpp +++ b/app/qtapp/rtknavi_qt/navimain.cpp @@ -75,7 +75,7 @@ MainWindow *mainForm; #define KACYCLE 5000 // keep alive cycle (ms) #define TIMEOUT 10000 // inactive timeout time (ms) #define MAX_PORT_OFFSET 9 // max port number offset -#define MAXTRKSCALE 23 // track scale +#define MAXTRKSCALE 26 // track scale #define MAXPANELMODE 7 // max panel mode #define SQRT(x) ((x)<0.0||(x)!=(x)?0.0:sqrt(x)) @@ -126,7 +126,7 @@ MainWindow::MainWindow(QWidget *parent) timeSystem = solutionType = 0; for (int i = 0; i < 4; i++) { plotType[i] = frequencyType[i] = baselineMode[i] = trackType[i] = 0; - trackScale[i] = 5; + trackScale[i] = 8; }; solutionsCurrent = solutionsStart = solutionsEnd = numSatellites[0] = numSatellites[1] = 0; nMapPoint = 0; @@ -2041,17 +2041,18 @@ void MainWindow::drawTrack(QPainter &c, int id, QPaintDevice *plot) QPoint p1, p2; QString label; static const double scale[] = { - 0.00021, 0.00047, 0.001, 0.0021, 0.0047, 0.01, 0.021, 0.047, 0.1, 0.21, 0.47, - 1.0, 2.1, 4.7, 10.0, 21.0, 47.0, 100.0, 210.0, 470.0, 1000.0, 2100.0,4700.0, + 0.000021, 0.000047, 0.0001, 0.00021, 0.00047, 0.001, 0.0021, 0.0047, + 0.01, 0.021, 0.047, 0.1, 0.21, 0.47, + 1.0, 2.1, 4.7, 10.0, 21.0, 47.0, 100.0, 210.0, 470.0, 1000.0, 2100.0, 4700.0, 10000.0 }; double *x, *y, xt, yt, sx, sy, ref[3], pos[3], dr[3], enu[3]; - int i, j, currentPointNo, numPoints = 0, type, scl; + int i, j, currentPointNo, numPoints = 0; trace(3, "drawTrack\n"); - type = id == 0 ? trackType[0] : trackType[1]; - scl = id == 0 ? trackScale[0] : trackScale[1]; + int type = trackType[id]; + int scl = trackScale[id]; x = new double[optDialog->solutionBufferSize]; y = new double[optDialog->solutionBufferSize]; @@ -2535,10 +2536,10 @@ void MainWindow::loadOptions() trackType[1] = settings.value("setting/trktype2", 0).toInt(); trackType[2] = settings.value("setting/trktype3", 0).toInt(); trackType[3] = settings.value("setting/trktype4", 0).toInt(); - trackScale[0] = settings.value("setting/trkscale1", 5).toInt(); - trackScale[1] = settings.value("setting/trkscale2", 5).toInt(); - trackScale[2] = settings.value("setting/trkscale3", 5).toInt(); - trackScale[3] = settings.value("setting/trkscale4", 5).toInt(); + trackScale[0] = settings.value("setting/trkscale1", 8).toInt(); + trackScale[1] = settings.value("setting/trkscale2", 8).toInt(); + trackScale[2] = settings.value("setting/trkscale3", 8).toInt(); + trackScale[3] = settings.value("setting/trkscale4", 8).toInt(); frequencyType[0] = settings.value("setting/freqtype1", 0).toInt(); frequencyType[1] = settings.value("setting/freqtype2", 0).toInt(); frequencyType[2] = settings.value("setting/freqtype3", 0).toInt(); diff --git a/app/winapp/rtknavi/navimain.cpp b/app/winapp/rtknavi/navimain.cpp index a0ce89240..ee31a2f11 100644 --- a/app/winapp/rtknavi/navimain.cpp +++ b/app/winapp/rtknavi/navimain.cpp @@ -127,8 +127,8 @@ __fastcall TMainForm::TMainForm(TComponent* Owner) CmdEna[i][0]=CmdEna[i][1]=CmdEna[i][2]=0; } TimeSys=SolType=PlotType1=PlotType2=FreqType1=FreqType2=0; - TrkType1=TrkType2=0; - TrkScale1=TrkScale2=5; + TrkType1=TrkType2=TrkType3=TrkType4=0; + TrkScale1=TrkScale2=TrkScale3=TrkScale4=8; BLMode1=BLMode2=BLMode3=BLMode4=0; PSol=PSolS=PSolE=Nsat[0]=Nsat[1]=0; NMapPnt=0; @@ -1011,6 +1011,34 @@ void __fastcall TMainForm::BtnShrink2Click(TObject *Sender) TrkScale2++; UpdatePlot(); } +// callback on button expand-2 ---------------------------------------------- +void __fastcall TMainForm::BtnExpand3Click(TObject *Sender) +{ + if (TrkScale3<=0) return; + TrkScale3--; + UpdatePlot(); +} +// callback on button shrink-3 ---------------------------------------------- +void __fastcall TMainForm::BtnShrink3Click(TObject *Sender) +{ + if (TrkScale3>=MAXTRKSCALE) return; + TrkScale3++; + UpdatePlot(); +} +// callback on button expand-4 ---------------------------------------------- +void __fastcall TMainForm::BtnExpand4Click(TObject *Sender) +{ + if (TrkScale4<=0) return; + TrkScale4--; + UpdatePlot(); +} +// callback on button shrink-4 ---------------------------------------------- +void __fastcall TMainForm::BtnShrink4Click(TObject *Sender) +{ + if (TrkScale4>=MAXTRKSCALE) return; + TrkScale4++; + UpdatePlot(); +} // callback on button-rtk-monitor ------------------------------------------- void __fastcall TMainForm::BtnMonitorClick(TObject *Sender) { @@ -2037,12 +2065,12 @@ void __fastcall TMainForm::DrawTrk(TImage *plot) 10000.0 }; double *x,*y,xt,yt,sx,sy,ref[3],pos[3],dr[3],enu[3]; - int i,j,k,n=0,type,scl; + int i,j,k,n=0; trace(3,"DrawTrk\n"); - type=plot->Name=="Plot1"?TrkType1 :TrkType2 ; - scl =plot->Name=="Plot1"?TrkScale1:TrkScale2; + int type=plot->Name=="Plot1"?TrkType1:plot->Name=="Plot2"?TrkType2:plot->Name=="Plot3"?TrkType3:TrkType4; + int scl =plot->Name=="Plot1"?TrkScale1:plot->Name=="Plot2"?TrkScale2:plot->Name=="Plot3"?TrkScale3:TrkScale4; x=new double[SolBuffSize]; y=new double[SolBuffSize]; @@ -2611,10 +2639,10 @@ void __fastcall TMainForm::LoadOpt(void) TrkType2 =ini->ReadInteger("setting","trktype2", 0); TrkType3 =ini->ReadInteger("setting","trktype3", 0); TrkType4 =ini->ReadInteger("setting","trktype4", 0); - TrkScale1 =ini->ReadInteger("setting","trkscale1", 5); - TrkScale2 =ini->ReadInteger("setting","trkscale2", 5); - TrkScale3 =ini->ReadInteger("setting","trkscale3", 5); - TrkScale4 =ini->ReadInteger("setting","trkscale4", 5); + TrkScale1 =ini->ReadInteger("setting","trkscale1", 8); + TrkScale2 =ini->ReadInteger("setting","trkscale2", 8); + TrkScale3 =ini->ReadInteger("setting","trkscale3", 8); + TrkScale4 =ini->ReadInteger("setting","trkscale4", 8); FreqType1 =ini->ReadInteger("setting","freqtype1", 0); FreqType2 =ini->ReadInteger("setting","freqtype2", 0); FreqType3 =ini->ReadInteger("setting","freqtype3", 0); diff --git a/app/winapp/rtknavi/navimain.dfm b/app/winapp/rtknavi/navimain.dfm index 9911304e4..bf001e3d4 100644 --- a/app/winapp/rtknavi/navimain.dfm +++ b/app/winapp/rtknavi/navimain.dfm @@ -707,7 +707,7 @@ object MainForm: TMainForm B4000000000000000000B4B4B4FFFFFFFFFFFFFFFFFFFFFFFF00FFFFFFFFFFFF FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF FF00} - OnClick = BtnExpand2Click + OnClick = BtnExpand3Click end object BtnShrink3: TSpeedButton Left = 1 @@ -735,7 +735,7 @@ object MainForm: TMainForm B4000000000000000000B4B4B4FFFFFFFFFFFFFFFFFFFFFFFF00FFFFFFFFFFFF FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF FF00} - OnClick = BtnShrink2Click + OnClick = BtnShrink3Click end end object Panel25: TPanel @@ -833,7 +833,7 @@ object MainForm: TMainForm B4000000000000000000B4B4B4FFFFFFFFFFFFFFFFFFFFFFFF00FFFFFFFFFFFF FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF FF00} - OnClick = BtnExpand2Click + OnClick = BtnExpand4Click end object BtnShrink4: TSpeedButton Left = 1 @@ -861,7 +861,7 @@ object MainForm: TMainForm B4000000000000000000B4B4B4FFFFFFFFFFFFFFFFFFFFFFFF00FFFFFFFFFFFF FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF FF00} - OnClick = BtnShrink2Click + OnClick = BtnShrink4Click end end end diff --git a/app/winapp/rtknavi/navimain.h b/app/winapp/rtknavi/navimain.h index 4135aa8d2..ffa5725a4 100644 --- a/app/winapp/rtknavi/navimain.h +++ b/app/winapp/rtknavi/navimain.h @@ -186,6 +186,10 @@ class TMainForm : public TForm void __fastcall BtnShrink1Click(TObject *Sender); void __fastcall BtnExpand2Click(TObject *Sender); void __fastcall BtnShrink2Click(TObject *Sender); + void __fastcall BtnExpand3Click(TObject *Sender); + void __fastcall BtnShrink3Click(TObject *Sender); + void __fastcall BtnExpand4Click(TObject *Sender); + void __fastcall BtnShrink4Click(TObject *Sender); void __fastcall BtnMarkClick(TObject *Sender); void __fastcall Panel24Resize(TObject *Sender); void __fastcall Panel25Resize(TObject *Sender); From 72dca37463ccfc7289265dad1ce23ae726ec248e Mon Sep 17 00:00:00 2001 From: oaq Date: Sat, 25 Apr 2026 19:42:45 +1000 Subject: [PATCH 05/25] Add BDS to the systems in the default process options --- src/rtkcmn.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rtkcmn.c b/src/rtkcmn.c index a4c078da3..1639a1950 100644 --- a/src/rtkcmn.c +++ b/src/rtkcmn.c @@ -203,7 +203,7 @@ const double chisqr[100]={ /* chi-sqr(n) (alpha=0.001) */ }; const prcopt_t prcopt_default={ /* defaults processing options */ PMODE_KINEMA,SOLTYPE_FORWARD, /* mode,soltype */ - 2,SYS_GPS|SYS_GLO|SYS_GAL, /* nf, navsys */ + 2,SYS_GPS|SYS_GLO|SYS_GAL|SYS_BDS, /* nf, navsys */ 15.0*D2R,{{0,0}}, /* elmin,snrmask */ 0,3,3,1,0,1, /* sateph,modear,glomodear,gpsmodear,bdsmodear,arfilter */ 20,0,4,5,10,20, /* maxout,minlock,minfixsats,minholdsats,mindropsats,minfix */ From ca3befa0432a451a67f34bcd86e5117af530133b Mon Sep 17 00:00:00 2001 From: oaq Date: Sat, 25 Apr 2026 19:39:47 +1000 Subject: [PATCH 06/25] rnx2rtkp: default nav sys to include Galileo and BDS --- app/consapp/rnx2rtkp/rnx2rtkp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/app/consapp/rnx2rtkp/rnx2rtkp.c b/app/consapp/rnx2rtkp/rnx2rtkp.c index 95d2e49be..de3234bd7 100644 --- a/app/consapp/rnx2rtkp/rnx2rtkp.c +++ b/app/consapp/rnx2rtkp/rnx2rtkp.c @@ -193,7 +193,7 @@ int main(int argc, char **argv) else if (n Date: Tue, 28 Apr 2026 00:25:52 +1000 Subject: [PATCH 07/25] readsp3: handle arbitrary comment lines Common product files are using more that the example 4 lines, so rework the code to handle this without generating error messages. --- src/preceph.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/preceph.c b/src/preceph.c index 2bb2a90b9..3576e1473 100644 --- a/src/preceph.c +++ b/src/preceph.c @@ -146,7 +146,7 @@ static int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats, bfact[0]=str2num(buff, 3,10); bfact[1]=str2num(buff,14,12); } - else if (i==2*nl+11){ + else if (i==2*nl+7){ break; /* at end of header */ } i=i+1; /* line counter */ @@ -187,6 +187,8 @@ static void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, if (!strncmp(buff,"EOF",3)) break; + if (buff[0] == '/' && buff[1] == '*') continue; // Comment. + if (buff[0]!='*'||str2time(buff,3,28,&time)) { trace(2,"sp3 invalid epoch %31.31s\n",buff); continue; From d3dc227eeb0681ea9cb8dca68f5cffa9c045e5df Mon Sep 17 00:00:00 2001 From: oaq Date: Mon, 27 Apr 2026 10:15:31 +1000 Subject: [PATCH 08/25] rinex header: fix clock file system parsing It uses the 'flag' to indicated that a clock file is expected and to handle the format changes in clock files from version 3.04, but could incorrectly parse the type and system if this expectation was not met. Rework this to use the flag to bias the search but to fallback to the alternative if not met. --- src/rinex.c | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/src/rinex.c b/src/rinex.c index 1bcd288b0..2385a0736 100644 --- a/src/rinex.c +++ b/src/rinex.c @@ -671,11 +671,31 @@ static int readrnxh(FILE *fp, double *ver, char *type, int *sys, int *tsys, } else if (strstr(label,"RINEX VERSION / TYPE")) { *ver=str2num(buff,0,9); - // Format change for clock files >=3.04 - *type=(*ver<3.04||flag==0)?*(buff+20):*(buff+21); + char sc; + if (*ver >= 3.04) { + // The format changed for clock files >=3.04. + if (flag == 1) { // Expecting a clock file. + *type = buff[21]; + sc = buff[42]; + if (*type != 'C') { + *type = buff[20]; + sc = buff[40]; + } + } else { // Not expecting a clock file. + *type = buff[20]; + sc = buff[40]; + if (*type == ' ' && buff[21] == 'C') { + *type = buff[21]; + sc = buff[42]; + } + } + } else { + *type = buff[20]; + sc = buff[40]; + } // Satellite system - switch ((*ver<3.04||flag==0)?*(buff+40):*(buff+42)) { + switch (sc) { case ' ': case 'G': *sys=SYS_GPS; *tsys=TSYS_GPS; break; case 'R': *sys=SYS_GLO; *tsys=TSYS_UTC; break; @@ -686,7 +706,7 @@ static int readrnxh(FILE *fp, double *ver, char *type, int *sys, int *tsys, case 'I': *sys=SYS_IRN; *tsys=TSYS_IRN; break; /* v.3.03 */ case 'M': *sys=SYS_NONE; *tsys=TSYS_GPS; break; /* mixed */ default : - trace(2,"not supported satellite system: %c\n",*(buff+40)); + trace(2,"not supported satellite system: %c\n", sc); break; } continue; From 0bba414473086096ec17964a165cf2211855e0ef Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Mon, 27 Apr 2026 10:28:11 -0600 Subject: [PATCH 09/25] Fix wrong constant in recent commits: SYS_BDS->SYS_CMP --- app/consapp/rnx2rtkp/rnx2rtkp.c | 2 +- src/rtkcmn.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/app/consapp/rnx2rtkp/rnx2rtkp.c b/app/consapp/rnx2rtkp/rnx2rtkp.c index de3234bd7..7cabfd3cd 100644 --- a/app/consapp/rnx2rtkp/rnx2rtkp.c +++ b/app/consapp/rnx2rtkp/rnx2rtkp.c @@ -193,7 +193,7 @@ int main(int argc, char **argv) else if (n Date: Tue, 28 Apr 2026 11:25:36 -0600 Subject: [PATCH 10/25] Modify iono-free option to use iono-free combination of freq slots 1 and 2 for all constellations if freqs="L1+L2", else use L1/L2 for GLONASS and freq slots 1 and 3 for all other constellations --- src/rtkcmn.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rtkcmn.c b/src/rtkcmn.c index 55958e2df..4443e9b0f 100644 --- a/src/rtkcmn.c +++ b/src/rtkcmn.c @@ -3740,8 +3740,8 @@ extern double ionppp(const double *pos, const double *azel, double re, /* select iono-free linear combination (L1/L2 or L1/L5) ----------------------*/ extern int seliflc(int optnf,int sys) { - /* use L1/L5 for Galileo if L5 is enabled */ - return((optnf==2||sys!=SYS_GAL)?1:2); + /* use L1/L5 for GPS,GAL,BDS if L5 is enabled */ + return((optnf==2||sys==SYS_GLO)?1:2); } /* troposphere model ----------------------------------------------------------- * compute tropospheric delay by standard atmosphere and saastamoinen model From a67d8a8798698558d27e2393c9aef7b70f5dc3c0 Mon Sep 17 00:00:00 2001 From: oaq Date: Mon, 27 Apr 2026 23:57:11 +1000 Subject: [PATCH 11/25] code2bias: guard code being zero, and code_ix not found --- src/preceph.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/preceph.c b/src/preceph.c index 2bb2a90b9..2d839f158 100644 --- a/src/preceph.c +++ b/src/preceph.c @@ -445,10 +445,12 @@ extern double code2bias(const nav_t *nav, int sys, int sat, int code, int mode) int sys_ix,frq_ix,code_ix; double bias=0; + if (code <= CODE_NONE) return 0; sys_ix=sys2ix(sys); frq_ix=code2idx(sys,code); if (sys_ix >= 0 && sys_ix < MAX_BIAS_SYS && frq_ix >= 0 && sat <= MAXSAT) { code_ix = code_bias_ix[sys_ix][code]; + if (code_ix < 0) return 0; bias=nav->cbias[sat-1][frq_ix][code_ix]; // absolute bias if (mode==0) bias-=nav->cbias[sat-1][frq_ix][0]; // difference with reference From f542de90425010781d0b52040492e00b9f30684f Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Fri, 1 May 2026 15:52:41 -0600 Subject: [PATCH 12/25] Make Beidou SSR clock and orbit correction message parsing functional - Fix field length errors - Derive IODEs per Beidou ICD --- src/rcvraw.c | 4 ++-- src/rtcm3.c | 13 ++++++++++--- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/rcvraw.c b/src/rcvraw.c index 99ce3ad92..17c1b2263 100644 --- a/src/rcvraw.c +++ b/src/rcvraw.c @@ -568,7 +568,6 @@ static int decode_bds_d1_eph(const uint8_t *buff, eph_t *eph) eph_bds.f2 =getbits (buff,i+214,11)*P2_66; eph_bds.f0 =getbits2(buff,i+225, 7,i+240,17)*P2_33; eph_bds.f1 =getbits2(buff,i+257, 5,i+270,17)*P2_50; - eph_bds.iode =getbitu (buff,i+287, 5); /* AODE */ i=8*38*1; /* subframe 2 */ frn2 =getbitu (buff,i+ 15, 3); @@ -611,6 +610,7 @@ static int decode_bds_d1_eph(const uint8_t *buff, eph_t *eph) toc_bds); return 0; } + eph_bds.iode =((int)(toc_bds/720.0))%240; /* per BeiDou ICD */ eph_bds.ttr=bdt2gpst(bdt2time(eph_bds.week,sow1)); /* bdt -> gpst */ if (eph_bds.toes>sow1+302400.0) eph_bds.week++; else if (eph_bds.toesbuff,i,13); i+=13; eph.sva =getbitu(rtcm->buff,i, 4); i+= 4; eph.idot =getbits(rtcm->buff,i,14)*P2_43*SC2RAD; i+=14; - eph.iode =getbitu(rtcm->buff,i, 5); i+= 5; /* AODE */ + i+= 5; /* AODE */ toc =getbitu(rtcm->buff,i,17)*8.0; i+=17; eph.f2 =getbits(rtcm->buff,i,11)*P2_66; i+=11; eph.f1 =getbits(rtcm->buff,i,22)*P2_50; i+=22; @@ -1479,6 +1479,7 @@ static int decode_type1042(rtcm_t *rtcm) } eph.sat=sat; eph.week=adjbdtweek(week); + eph.iode =((int)(toc/720.0))%240; /* per BeiDou ICD */ if (rtcm->time.time==0) rtcm->time=utc2gpst(timeget()); tt=timediff(bdt2gpst(bdt2time(eph.week,eph.toes)),rtcm->time); if (tt<-302400.0) eph.week++; @@ -1616,7 +1617,7 @@ static int decode_ssr1(rtcm_t *rtcm, int sys, int subtype) case SYS_GLO: np=5; ni= 8; nj= 0; offp= 0; break; case SYS_GAL: np=6; ni=10; nj= 0; offp= 0; break; case SYS_QZS: np=4; ni= 8; nj= 0; offp=192; break; - case SYS_CMP: np=6; ni=10; nj=24; offp= 1; break; + case SYS_CMP: np=6; ni=10; nj=8; offp= 0; break; case SYS_SBS: np=6; ni= 9; nj=24; offp=120; break; default: return sync?0:10; } @@ -1643,6 +1644,9 @@ static int decode_ssr1(rtcm_t *rtcm, int sys, int subtype) rtcm->ssr[sat-1].t0 [0]=rtcm->time; rtcm->ssr[sat-1].udi[0]=udint; rtcm->ssr[sat-1].iod[0]=iod; + if (sys==SYS_CMP && subtype==0) { + iode=iodcrc; /* BDS per-satellite SSR IOD */ + } rtcm->ssr[sat-1].iode=iode; /* SBAS/BDS: toe/t0 modulo */ rtcm->ssr[sat-1].iodcrc=iodcrc; /* SBAS/BDS: IOD CRC */ rtcm->ssr[sat-1].refd=refd; @@ -1776,7 +1780,7 @@ static int decode_ssr4(rtcm_t *rtcm, int sys, int subtype) case SYS_GLO: np=5; ni= 8; nj= 0; offp= 0; break; case SYS_GAL: np=6; ni=10; nj= 0; offp= 0; break; case SYS_QZS: np=4; ni= 8; nj= 0; offp=192; break; - case SYS_CMP: np=6; ni=10; nj=24; offp= 1; break; + case SYS_CMP: np=6; ni=10; nj=8; offp= 0; break; case SYS_SBS: np=6; ni= 9; nj=24; offp=120; break; default: return sync?0:10; } @@ -1807,6 +1811,9 @@ static int decode_ssr4(rtcm_t *rtcm, int sys, int subtype) rtcm->ssr[sat-1].t0 [0]=rtcm->ssr[sat-1].t0 [1]=rtcm->time; rtcm->ssr[sat-1].udi[0]=rtcm->ssr[sat-1].udi[1]=udint; rtcm->ssr[sat-1].iod[0]=rtcm->ssr[sat-1].iod[1]=iod; + if (sys==SYS_CMP && subtype==0) { + iode=iodcrc; /* BDS per-satellite SSR IOD */ + } rtcm->ssr[sat-1].iode=iode; rtcm->ssr[sat-1].iodcrc=iodcrc; rtcm->ssr[sat-1].refd=refd; From 5b59da32e47bb930d73de9bd5426a0dec9d39eee Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Fri, 1 May 2026 15:55:04 -0600 Subject: [PATCH 13/25] Some .BIA bias files leave SVN field blank, modify file parsing to handle this case. --- src/preceph.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/preceph.c b/src/preceph.c index e62e493d0..9269d46dc 100644 --- a/src/preceph.c +++ b/src/preceph.c @@ -466,7 +466,7 @@ static int readbiaf(const char *file, nav_t *nav) { FILE *fp; double cbias; - char buff[256],bias[6]="",svn[6]="",prn[6]="",obs1[6]="",obs2[6]; + char buff[256],bias[4]="",svn[4]="",prn[4]="",obs1[4]="",obs2[4]; int sat,sys_ix,frq_ix,code1,code2,bias_ix1,bias_ix2,sys; trace(3,"readbiaf: file=%s\n",file); @@ -476,7 +476,11 @@ static int readbiaf(const char *file, nav_t *nav) return 0; } while (fgets(buff,sizeof(buff),fp)) { - if (sscanf(buff,"%4s %5s %4s %4s %4s",bias,svn,prn,obs1,obs2)<5) continue; + if ((int)strlen(buff)<91) continue; + strncpy(bias, buff+1, 3); bias[3] ='\0'; + strncpy(prn, buff+11, 3); prn[3] ='\0'; + strncpy(obs1, buff+25, 3); obs1[3] ='\0'; + strncpy(obs2, buff+29, 3); obs2[3] ='\0'; if (obs1[0]!='C') continue; /* skip phase biases for now */ if ((cbias=str2num(buff,70,21))==0.0) continue; sat=satid2no(prn); From 9ada3b7ed49ae4019b5ef047e5805d3745a315a4 Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Fri, 1 May 2026 15:56:43 -0600 Subject: [PATCH 14/25] Apply code biases for PPP solutions as absolute instead of differential for SSR and post-processing solutions --- src/ppp.c | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/ppp.c b/src/ppp.c index 81d813db0..9f2509004 100644 --- a/src/ppp.c +++ b/src/ppp.c @@ -426,18 +426,11 @@ static void corr_meas(const obsd_t *obs, const nav_t *nav, const double *azel, P[i]=obs->P[i] -dants[i]-dantr[i]; double P_nobias = P[i]; if (opt->sateph==EPHOPT_SSRAPC||opt->sateph==EPHOPT_SSRCOM) { - /* select SSR code correction based on code */ - if (sys==SYS_GPS) - ix=(i==0?CODE_L1W-1:CODE_L2W-1); - else if (sys==SYS_GLO) - ix=(i==0?CODE_L1P-1:CODE_L2P-1); - else if (sys==SYS_GAL) - ix=(i==0?CODE_L1X-1:CODE_L7X-1); /* apply SSR correction */ - P[i]+=(nav->ssr[obs->sat-1].cbias[obs->code[i]-1]-nav->ssr[obs->sat-1].cbias[ix]); + P[i]-=nav->ssr[obs->sat-1].cbias[obs->code[i]-1]; } else { /* apply code bias corrections from file */ - P[i]-=code2bias(nav,sys,obs->sat,obs->code[i],0); /* differential bias*/ + P[i]-=code2bias(nav,sys,obs->sat,obs->code[i],1); /* absolute bias*/ } trace(4,"sys=%d sat=%d frq=%d, P: %.3f->%.3f, dt=%.3f\n",sys,obs->sat,i,P_nobias,P[i],(P[i]-P_nobias)/(1E-9*CLIGHT)); } @@ -450,6 +443,7 @@ static void corr_meas(const obsd_t *obs, const nav_t *nav, const double *azel, if (L[0]!=0.0&&L[frq2]!=0.0) *Lc=C1*L[0]+C2*L[frq2]; if (P[0]!=0.0&&P[frq2]!=0.0) *Pc=C1*P[0]+C2*P[frq2]; + trace(4,"corr_meas: sat=%d f2=%d, Lc=%.3f Pc=%.3f\n",obs->sat,frq2,*Lc,*Pc); } /* detect cycle slip by LLI --------------------------------------------------*/ static void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) From 28ad77c06ffc16b215858d6d5184d85333d44db9 Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Fri, 1 May 2026 16:04:01 -0600 Subject: [PATCH 15/25] - Initialize timestamps for SSR corrections in rtksvrinit - Add reset of rtksvr structure when starting RTKNAVI as a commented out line, uncommenting this line makes RTKNAVI solutions repeatable when postprocessing data from files. --- app/winapp/rtknavi/navimain.cpp | 1 + src/rtksvr.c | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/app/winapp/rtknavi/navimain.cpp b/app/winapp/rtknavi/navimain.cpp index ee31a2f11..41e193c62 100644 --- a/app/winapp/rtknavi/navimain.cpp +++ b/app/winapp/rtknavi/navimain.cpp @@ -1320,6 +1320,7 @@ void __fastcall TMainForm::SvrStart(void) strsetopt(stropt); strcpy(rtksvr.cmd_reset,ResetCmd.c_str()); rtksvr.bl_reset=MaxBL; + //rtksvrinit(&rtksvr); // uncomment this to make RTKNAVI repeatable from run to run (useful for debug) // start rtk server if (!rtksvrstart(&rtksvr,SvrCycle,SvrBuffSize,strs,(const char **)paths,Format, diff --git a/src/rtksvr.c b/src/rtksvr.c index ac45da1c1..cacbf0996 100644 --- a/src/rtksvr.c +++ b/src/rtksvr.c @@ -832,6 +832,10 @@ extern int rtksvrinit(rtksvr_t *svr) memset(svr->rtcm+i,0,sizeof(rtcm_t)); } for (i=0;istream+i); + + for (i=0;inav.ssr[i].t0[j]=time0; + } for (i=0;i<3;i++) *svr->cmds_periodic[i]='\0'; *svr->cmd_reset='\0'; From bfd6491205cc5e7c3a7b613f8c8aa987fd30f120 Mon Sep 17 00:00:00 2001 From: oaq Date: Fri, 8 May 2026 13:29:10 +1000 Subject: [PATCH 16/25] readrnxclk: interpolate the standard deviations The standard deviations can be supplied at a lower rate than the clock biases, e.g. 30 sec biases with 5 minute standard deviations seen in the CODE precise clock bias products. --- src/rinex.c | 55 ++++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 44 insertions(+), 11 deletions(-) diff --git a/src/rinex.c b/src/rinex.c index 2385a0736..6dc53ad5b 100644 --- a/src/rinex.c +++ b/src/rinex.c @@ -1558,34 +1558,33 @@ static int readrnxnav(FILE *fp, const char *opt, double ver, int sys, /* read RINEX clock ----------------------------------------------------------*/ static int readrnxclk(FILE *fp, const char *opt, double ver, int index, nav_t *nav) { - pclk_t *nav_pclk; - gtime_t time; - double data[2]; - int i,j,sat,mask,off; - char buff[MAXRNXLEN],satid[8]=""; - trace(3,"readrnxclk: index=%d\n", index); if (!nav) return 0; + pclk_t *nav_pclk; + char buff[MAXRNXLEN]; /* set system mask */ - mask=set_sysmask(opt); - off=ver>=3.04?5:0; /* format change for ver>=3.04 */ + int mask=set_sysmask(opt); + int off=ver>=3.04?5:0; /* format change for ver>=3.04 */ while (fgets(buff,sizeof(buff),fp)) { - + gtime_t time; if (str2time(buff,8+off,26,&time)) { trace(2,"rinex clk invalid epoch: %34.34s\n",buff); continue; } + char satid[8]=""; memcpy(satid,buff+3,4); /* only read AS (satellite clock) record */ + int sat; if (strncmp(buff,"AS",2)||!(sat=satid2no(satid))) continue; if (!(satsys(sat,NULL)&mask)) continue; - for (i=0,j=40+off;i<2;i++,j+=20) data[i]=str2num(buff,j,19); + double data[2]; + for (int i=0,j=40+off;i<2;i++,j+=20) data[i]=str2num(buff,j,19); if (nav->nc>=nav->ncmax) { nav->ncmax+=1024; @@ -1600,7 +1599,7 @@ static int readrnxclk(FILE *fp, const char *opt, double ver, int index, nav_t *n nav->nc++; nav->pclk[nav->nc-1].time =time; nav->pclk[nav->nc-1].index=index; - for (i=0;ipclk[nav->nc-1].clk[i][0]=0.0; nav->pclk[nav->nc-1].std[i][0]=0.0f; } @@ -1608,6 +1607,40 @@ static int readrnxclk(FILE *fp, const char *opt, double ver, int index, nav_t *n nav->pclk[nav->nc-1].clk[sat-1][0]=data[0]; nav->pclk[nav->nc-1].std[sat-1][0]=(float)data[1]; } + + // Interpolate the standard deviations. The standard deviations can be + // supplied at a lower rate than the clock biases, e.g. 30 sec biases with + // 5 minute standard deviations. + for (int k = 0; k < MAXSAT; k++) { + int last_std_idx = -1; + for (int i = 0; i < nav->nc; i++) { + double std = nav->pclk[i].std[k][0]; + if (std > 0) { + if (last_std_idx < 0) { + for (int j = 0; j < i; j++) + if (nav->pclk[j].clk[k][0] != 0) nav->pclk[j].std[k][0] = std; + } else { + // Linear interpolation of the variance. + for (int j = last_std_idx + 1; j < i; j++) { + if (nav->pclk[j].clk[k][0] != 0) { + double last_std = nav->pclk[last_std_idx].std[k][0]; + double t0 = timediff(nav->pclk[j].time, nav->pclk[last_std_idx].time); + double t1 = timediff(nav->pclk[j].time, nav->pclk[i].time); + double var = (SQR(std) * t0 - SQR(last_std) * t1) / (t0 - t1); + nav->pclk[j].std[k][0] = (float)sqrt(var); + } + } + } + last_std_idx = i; + } + } + if (last_std_idx >= 0) { + double last_std = nav->pclk[last_std_idx].std[k][0]; + for (int j = last_std_idx + 1; j < nav->nc; j++) + if (nav->pclk[j].clk[k][0] != 0) nav->pclk[j].std[k][0] = last_std; + } + } + return nav->nc>0; } /* read RINEX file -----------------------------------------------------------*/ From 5c9cd9de9fdf46b6e1b96e5aae5ae4e1d20a7e75 Mon Sep 17 00:00:00 2001 From: oaq Date: Thu, 7 May 2026 20:07:49 +1000 Subject: [PATCH 17/25] satposs: fix to work with precise clocks from sp3 files satposs() was failing when precise clocks were configured and supplied in only an sp3 file and without broadcast ephemeris. Rework the code to fix this and a few related issues noted. pephclk() - rework to search both the precise clocks and the precise ephemeris data, giving preference to the precise clocks which are typically at a shorter period and more accurate. It was searching only the precise clocks and failing to make use of the satellite clock biases in sp3 files in some paths. Return 0 on all failure return paths, so the caller can test the return value for success or failure, and rework the callers as necessary. satposs() use the updated pephclk() to work with only satellite clock biases in sp3 files, and without broadcast ephemeris to give the satellite clock bias. pephpos() correct the returned clock variance which was not being scaled by CLIGHT to return it in units of m^2. It was effectively only applying the interpolation variance. Lower the precise clock extrapolation error from 1mm/s to 0.4mm/s. This gives a midpoint standard deviation of around 6mm for 30s data and 60mm for 5 minute data which seems more appropriate for current satellite clocks and still conservative. --- src/ephemeris.c | 9 +++++++-- src/preceph.c | 40 +++++++++++++++++++++++++++++++--------- 2 files changed, 38 insertions(+), 11 deletions(-) diff --git a/src/ephemeris.c b/src/ephemeris.c index 56c8e6ea4..da56ad17e 100644 --- a/src/ephemeris.c +++ b/src/ephemeris.c @@ -808,8 +808,13 @@ extern void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, time[i]=timeadd(obs[i].time,-pr/CLIGHT); /* satellite clock offset from precise products or broadcast ephemeris */ - if (ephopt==EPHOPT_PREC&&nav->nc>0) { - if(!pephclk(time[i],obs[i].sat,nav,&dt,NULL)) { + + // Note: this uses as input the estimated satellite clock time without + // correction but the precise clock corrections are wrt GPST. The + // satellite clock drift over this small period is considered + // negligible to the clock offset lookup here. + if (ephopt == EPHOPT_PREC) { + if (!pephclk(time[i], obs[i].sat, nav, &dt, NULL)) { trace(3,"no precise clock %s sat=%2d\n",time2str(time[i],tstr,3),obs[i].sat); continue; } diff --git a/src/preceph.c b/src/preceph.c index 9269d46dc..19a4676d3 100644 --- a/src/preceph.c +++ b/src/preceph.c @@ -55,7 +55,7 @@ #define NMAX 10 /* order of polynomial interpolation */ #define MAXDTE 900.0 /* max time difference to ephem time (s) */ -#define EXTERR_CLK 1E-3 /* extrapolation error for clock (m/s) */ +#define EXTERR_CLK 0.4E-3 /* extrapolation error for clock (m/s) */ #define EXTERR_EPH 5E-7 /* extrapolation error for ephem (m/s^2) */ #define MAX_BIAS_SYS 6 /* # of constellations supported */ @@ -639,7 +639,7 @@ static int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs, else if (c[0]!=0.0&&c[1]!=0.0) { dts[0]=(c[1]*t[0]-c[0]*t[1])/(t[0]-t[1]); i=t[0]<-t[1]?0:1; - std=nav->peph[index+i].std[sat-1][3]+EXTERR_CLK*fabs(t[i]); + std = nav->peph[index+i].std[sat-1][3] * CLIGHT + EXTERR_CLK * fabs(t[i]); } else { dts[0]=0.0; @@ -647,9 +647,10 @@ static int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs, if (varc) *varc=SQR(std); return 1; } + /* satellite clock by precise clock ------------------------------------------*/ -extern int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, - double *varc) +static int pephclk1(gtime_t time, int sat, const nav_t *nav, double *dts, + double *varc) { double t[2],c[2],std; int i,j,k,index; @@ -661,7 +662,7 @@ extern int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, timediff(time,nav->pclk[0].time)<-MAXDTE|| timediff(time,nav->pclk[nav->nc-1].time)>MAXDTE) { trace(3,"no prec clock %s sat=%2d\n",time2str(time,tstr,0),sat); - return 1; + return 0; } /* binary search */ for (i=0,j=nav->nc-1;i Date: Fri, 8 May 2026 16:50:17 -0600 Subject: [PATCH 18/25] Move $SAT output code to stat file for PPP solutions from rtkpos.c to ppp.c to avoid using wrong macro define for state indices --- src/ppp.c | 26 ++++++++++++++++---------- src/rtklib.h | 2 +- src/rtkpos.c | 35 +++++++++++++++++------------------ 3 files changed, 34 insertions(+), 29 deletions(-) diff --git a/src/ppp.c b/src/ppp.c index 9f2509004..5c6a5a91c 100644 --- a/src/ppp.c +++ b/src/ppp.c @@ -124,7 +124,7 @@ static double STD(rtk_t *rtk, int i) return SQRT(rtk->P[i+i*rtk->nx]); } /* write solution status for PPP ---------------------------------------------*/ -extern int pppoutstat(rtk_t *rtk, char *buff) +extern int pppoutstat(rtk_t *rtk, char *buff, int level) { ssat_t *ssat; double tow,pos[3],vel[3],acc[3],*x; @@ -184,17 +184,23 @@ extern int pppoutstat(rtk_t *rtk, char *buff) rtk->ssat[i].azel[1]*R2D,x[j],STD(rtk,j)); } } -#ifdef OUTSTAT_AMB - /* ambiguity parameters */ - int k; - for (i=0;iopt);j++) { - k=IB(i+1,j,&rtk->opt); - if (rtk->x[k]==0.0) continue; + if (level <= 1) return (int)(p-buff); + + /* Write residuals and status */ + for (int i=0;issat+i; + if (!ssat->vs) continue; satno2id(i+1,id); - p+=sprintf(p,"$AMB,%d,%.3f,%d,%s,%d,%.4f,%.4f\n",week,tow, - rtk->sol.stat,id,j+1,x[k],STD(rtk,k)); + for (int j=0;jopt);j++) { + int k=IB(i+1,j,&rtk->opt); + p+=sprintf(p,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%u,%u,%u,%.2f,%.6f,%.5f\n", + week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, + ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr_rover[j], + ssat->fix[j],ssat->slip[j]&(LLI_SLIP|LLI_HALFC),ssat->lock[j],ssat->outc[j], + ssat->slipc[j],ssat->rejc[j],knx?rtk->x[k]:0, + knx?rtk->P[k+k*rtk->nx]:0,ssat->icbias[j]); + } } -#endif return (int)(p-buff); } /* exclude meas of eclipsing satellite (block IIA) ---------------------------*/ diff --git a/src/rtklib.h b/src/rtklib.h index 55149be8e..9a95e7987 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -1859,7 +1859,7 @@ EXPORT int rtkoutstat(rtk_t *rtk, int level, char *buff); /* precise point positioning -------------------------------------------------*/ EXPORT void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); EXPORT int pppnx(const prcopt_t *opt); -EXPORT int pppoutstat(rtk_t *rtk, char *buff); +EXPORT int pppoutstat(rtk_t *rtk, char *buff, int level); EXPORT int ppp_ar(rtk_t *rtk, const obsd_t *obs, int n, int *exc, const nav_t *nav, const double *azel, double *x, double *P); diff --git a/src/rtkpos.c b/src/rtkpos.c index 8ebc15939..ac9611a4a 100644 --- a/src/rtkpos.c +++ b/src/rtkpos.c @@ -231,7 +231,7 @@ extern int rtkoutstat(rtk_t *rtk, int level, char *buff) if (rtk->opt.mode>=PMODE_PPP_KINEMA) { /* Write ppp solution status to buffer */ - p+=pppoutstat(rtk,buff); + p+=pppoutstat(rtk,buff,level); } else { /* Receiver position */ if (est) { @@ -300,23 +300,22 @@ extern int rtkoutstat(rtk_t *rtk, int level, char *buff) rtk->sol.stat,i+1,rtk->x[j],xa[0]); } } - } - - if (level <= 1) return (int)(p-buff); - - /* Write residuals and status */ - for (int i=0;issat+i; - if (!ssat->vs) continue; - satno2id(i+1,id); - for (int j=0;jopt); - p+=sprintf(p,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%u,%u,%u,%.2f,%.6f,%.5f\n", - week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, - ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr_rover[j], - ssat->fix[j],ssat->slip[j]&(LLI_SLIP|LLI_HALFC),ssat->lock[j],ssat->outc[j], - ssat->slipc[j],ssat->rejc[j],knx?rtk->x[k]:0, - knx?rtk->P[k+k*rtk->nx]:0,ssat->icbias[j]); + if (level <= 1) return (int)(p-buff); + + /* Write residuals and status */ + for (int i=0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1,id); + for (int j=0;jopt); + p+=sprintf(p,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%u,%u,%u,%.2f,%.6f,%.5f\n", + week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, + ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr_rover[j], + ssat->fix[j],ssat->slip[j]&(LLI_SLIP|LLI_HALFC),ssat->lock[j],ssat->outc[j], + ssat->slipc[j],ssat->rejc[j],knx?rtk->x[k]:0, + knx?rtk->P[k+k*rtk->nx]:0,ssat->icbias[j]); + } } } From 17efff8042e755bfe46b19890811898110d151c3 Mon Sep 17 00:00:00 2001 From: oaq Date: Fri, 8 May 2026 20:36:41 +1000 Subject: [PATCH 19/25] rtcm3 ssr: update types, codes, BDS offset, remove std-dev SSR7 types are in common uses, updated from the tentative allocation. Allows these to be processed with current SSR streams. Change SSR7 BDS prn offset of 1 to 0 which appears consistent with current SSR streams, fixing the BDS PRN mapping. The SSR7 phase bias standard-deviation field do not appear in current SSR streams, so remove it. Otherwise the phase biases read as garbage. The RTCM-SSR and IGS-SSR signal mapping has diverged, so split these into separate tables. Update these tables with current common signal mappings. Include the subtype in trace messages in the ssr code paths. --- src/rtcm3.c | 167 +++++++++++++++++++++++++++++------------------- src/rtcm3e.c | 174 ++++++++++++++++++++++++++++++--------------------- src/rtklib.h | 1 - 3 files changed, 206 insertions(+), 136 deletions(-) diff --git a/src/rtcm3.c b/src/rtcm3.c index 29da0b11d..471c204f4 100644 --- a/src/rtcm3.c +++ b/src/rtcm3.c @@ -110,7 +110,7 @@ const char *msm_sig_sbs[32] = { "", "1C", "", "", "", "", "", "", "", "", "", "", // 1-12 "", "", "", "", "", "", "", "", "", "5I", "5Q", "5X", // 13-24 "", "", "", "", "", "", "", ""}; // 25-32 -const char *msm_sig_cmp[32] = { +const char *msm_sig_bds[32] = { // BeiDou: ref [17] table 3.5-108. // 1S, 1L, 1Z, 6D, 6P, 6Z, 7P, 7Z, 8D, 8P, 8X are tentative from the PocketSDR extensions "", "2I", "2Q", "2X", "1S", "1L", "1Z", "6I", "6Q", "6X", "6D", "6P", // 1-12 @@ -124,33 +124,65 @@ const char *msm_sig_irn[32] = { "", "", "", "", "", "", "", "", "", "5A", "5B", "5C", // 13-24 "5X", "", "", "", "", "", "", ""}; // 25-32 -/* SSR signal and tracking mode IDs ------------------------------------------*/ +/* RTCM SSR signal and tracking mode IDs --------------------------------------*/ +// Signals L1S and L1L have intentional redundant entries, as producers +// currently vary in usage. const uint8_t ssr_sig_gps[32]={ CODE_L1C,CODE_L1P,CODE_L1W,CODE_L1S,CODE_L1L,CODE_L2C,CODE_L2D,CODE_L2S, - CODE_L2L,CODE_L2X,CODE_L2P,CODE_L2W, 0, 0,CODE_L5I,CODE_L5Q + CODE_L2L,CODE_L2X,CODE_L2P,CODE_L2W,CODE_L2Y,CODE_L2M,CODE_L5I,CODE_L5Q, + CODE_L5X,CODE_L1S,CODE_L1L,CODE_L1X }; const uint8_t ssr_sig_glo[32]={ + CODE_L1C,CODE_L1P,CODE_L2C,CODE_L2P,CODE_L4A,CODE_L4B,CODE_L4X,CODE_L6A, + CODE_L6B,CODE_L6X,CODE_L3I,CODE_L3Q,CODE_L3X +}; +const uint8_t ssr_sig_gal[32]={ + CODE_L1A,CODE_L1B,CODE_L1C,CODE_L1X,CODE_L1Z,CODE_L5I,CODE_L5Q,CODE_L5X, + CODE_L7I,CODE_L7Q,CODE_L7X,CODE_L8I,CODE_L8Q,CODE_L8X,CODE_L6A,CODE_L6B, + CODE_L6C,CODE_L6X,CODE_L6Z +}; +const uint8_t ssr_sig_qzs[32]={ + CODE_L1C,CODE_L1S,CODE_L1L,CODE_L2S,CODE_L2L,CODE_L2X,CODE_L5I,CODE_L5Q, + CODE_L5X,CODE_L6S,CODE_L6L,CODE_L6X,CODE_L1X,CODE_L1Z,CODE_L5D,CODE_L5P, + CODE_L5Z,CODE_L6E,CODE_L6Z +}; +const uint8_t ssr_sig_bds[32]={ + CODE_L2I,CODE_L2Q,CODE_L2X,CODE_L6I,CODE_L6Q,CODE_L6X,CODE_L7I,CODE_L7Q, + CODE_L7X,CODE_L1D,CODE_L1P,CODE_L1X,CODE_L5D,CODE_L5P,CODE_L5X,CODE_L1A, + 0, 0,CODE_L6A +}; +const uint8_t ssr_sig_sbs[32]={ + CODE_L1C,CODE_L5I,CODE_L5Q +}; + +/* IGS SSR signal and tracking mode IDs ---------------------------------------*/ +const uint8_t ssr_igs_sig_gps[32]={ + CODE_L1C,CODE_L1P,CODE_L1W,CODE_L1S,CODE_L1L,CODE_L2C,CODE_L2D,CODE_L2S, + CODE_L2L, 0,CODE_L2P,CODE_L2W, 0, 0,CODE_L5I,CODE_L5Q +}; +const uint8_t ssr_igs_sig_glo[32]={ CODE_L1C,CODE_L1P,CODE_L2C,CODE_L2P,CODE_L4A,CODE_L4B,CODE_L6A,CODE_L6B, CODE_L3I,CODE_L3Q }; -const uint8_t ssr_sig_gal[32]={ +const uint8_t ssr_igs_sig_gal[32]={ CODE_L1A,CODE_L1B,CODE_L1C, 0, 0,CODE_L5I,CODE_L5Q, 0, - CODE_L7I,CODE_L7Q, 0,CODE_L8I,CODE_L8Q, 0,CODE_L6A,CODE_L6B, + CODE_L7I,CODE_L7Q, 0, 0, 0, 0,CODE_L6A,CODE_L6B, CODE_L6C }; -const uint8_t ssr_sig_qzs[32]={ +const uint8_t ssr_igs_sig_qzs[32]={ CODE_L1C,CODE_L1S,CODE_L1L,CODE_L2S,CODE_L2L, 0,CODE_L5I,CODE_L5Q, 0,CODE_L6S,CODE_L6L, 0, 0, 0, 0, 0, 0,CODE_L6E }; -const uint8_t ssr_sig_cmp[32]={ +const uint8_t ssr_igs_sig_bds[32]={ CODE_L2I,CODE_L2Q, 0,CODE_L6I,CODE_L6Q, 0,CODE_L7I,CODE_L7Q, 0,CODE_L1D,CODE_L1P, 0,CODE_L5D,CODE_L5P, 0,CODE_L1A, 0, 0,CODE_L6A }; -const uint8_t ssr_sig_sbs[32]={ +const uint8_t ssr_igs_sig_sbs[32]={ CODE_L1C,CODE_L5I,CODE_L5Q }; + /* SSR update intervals ------------------------------------------------------*/ static const double ssrudint[16]={ 1,2,5,10,15,30,60,120,240,300,600,900,1800,3600,7200,10800 @@ -1609,7 +1641,7 @@ static int decode_ssr1(rtcm_t *rtcm, int sys, int subtype) type=getbitu(rtcm->buff,24,12); if ((nsat=decode_ssr1_head(rtcm,sys,subtype,&sync,&iod,&udint,&refd,&i))<0) { - trace(2,"rtcm3 %d length error: len=%d\n",type,rtcm->len); + trace(2,"rtcm3 %d/%d length error: len=%d\n",type,subtype,rtcm->len); return -1; } switch (sys) { @@ -1623,8 +1655,7 @@ static int decode_ssr1(rtcm_t *rtcm, int sys, int subtype) } if (subtype>0) { /* IGS SSR */ np=6; ni=8; nj=0; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i,np)+offp; i+=np; @@ -1638,7 +1669,7 @@ static int decode_ssr1(rtcm_t *rtcm, int sys, int subtype) ddeph[2]=getbits(rtcm->buff,i,19)*4E-6; i+=19; if (!(sat=satno(sys,prn))) { - trace(2,"rtcm3 %d satellite number error: prn=%d\n",type,prn); + trace(2,"rtcm3 %d/%d satellite number error: prn=%d\n",type,subtype,prn); continue; } rtcm->ssr[sat-1].t0 [0]=rtcm->time; @@ -1668,7 +1699,7 @@ static int decode_ssr2(rtcm_t *rtcm, int sys, int subtype) type=getbitu(rtcm->buff,24,12); if ((nsat=decode_ssr2_head(rtcm,sys,subtype,&sync,&iod,&udint,&i))<0) { - trace(2,"rtcm3 %d length error: len=%d\n",type,rtcm->len); + trace(2,"rtcm3 %d/%d length error: len=%d\n",type,subtype,rtcm->len); return -1; } switch (sys) { @@ -1676,14 +1707,13 @@ static int decode_ssr2(rtcm_t *rtcm, int sys, int subtype) case SYS_GLO: np=5; offp= 0; break; case SYS_GAL: np=6; offp= 0; break; case SYS_QZS: np=4; offp=192; break; - case SYS_CMP: np=6; offp= 1; break; + case SYS_CMP: np=6; offp= 0; break; case SYS_SBS: np=6; offp=120; break; default: return sync?0:10; } if (subtype>0) { /* IGS SSR */ np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i,np)+offp; i+=np; @@ -1692,7 +1722,7 @@ static int decode_ssr2(rtcm_t *rtcm, int sys, int subtype) dclk[2]=getbits(rtcm->buff,i,27)*2E-8; i+=27; if (!(sat=satno(sys,prn))) { - trace(2,"rtcm3 %d satellite number error: prn=%d\n",type,prn); + trace(2,"rtcm3 %d/%d satellite number error: prn=%d\n",type,subtype,prn); continue; } rtcm->ssr[sat-1].t0 [1]=rtcm->time; @@ -1716,22 +1746,29 @@ static int decode_ssr3(rtcm_t *rtcm, int sys, int subtype) type=getbitu(rtcm->buff,24,12); if ((nsat=decode_ssr2_head(rtcm,sys,subtype,&sync,&iod,&udint,&i))<0) { - trace(2,"rtcm3 %d length error: len=%d\n",type,rtcm->len); + trace(2,"rtcm3 %d/%d length error: len=%d\n",type,subtype,rtcm->len); return -1; } - switch (sys) { + if (subtype>0) { /* IGS SSR */ + switch (sys) { + case SYS_GPS: np=6; offp= 0; sigs=ssr_igs_sig_gps; break; + case SYS_GLO: np=6; offp= 0; sigs=ssr_igs_sig_glo; break; + case SYS_GAL: np=6; offp= 0; sigs=ssr_igs_sig_gal; break; + case SYS_QZS: np=6; offp=192; sigs=ssr_igs_sig_qzs; break; + case SYS_CMP: np=6; offp= 0; sigs=ssr_igs_sig_bds; break; + case SYS_SBS: np=6; offp=119; sigs=ssr_igs_sig_sbs; break; + default: return sync?0:10; + } + } else { + switch (sys) { case SYS_GPS: np=6; offp= 0; sigs=ssr_sig_gps; break; case SYS_GLO: np=5; offp= 0; sigs=ssr_sig_glo; break; case SYS_GAL: np=6; offp= 0; sigs=ssr_sig_gal; break; case SYS_QZS: np=4; offp=192; sigs=ssr_sig_qzs; break; - case SYS_CMP: np=6; offp= 1; sigs=ssr_sig_cmp; break; + case SYS_CMP: np=6; offp= 0; sigs=ssr_sig_bds; break; case SYS_SBS: np=6; offp=120; sigs=ssr_sig_sbs; break; default: return sync?0:10; - } - if (subtype>0) { /* IGS SSR */ - np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + } } for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i,np)+offp; i+=np; @@ -1745,11 +1782,11 @@ static int decode_ssr3(rtcm_t *rtcm, int sys, int subtype) cbias[sigs[mode]-1]=bias; } else { - trace(2,"rtcm3 %d not supported mode: mode=%d\n",type,mode); + trace(2,"rtcm3 %d/%d not supported mode: mode=%d\n",type,subtype,mode); } } if (!(sat=satno(sys,prn))) { - trace(2,"rtcm3 %d satellite number error: prn=%d\n",type,prn); + trace(2,"rtcm3 %d/%d satellite number error: prn=%d\n",type,subtype,prn); continue; } rtcm->ssr[sat-1].t0 [4]=rtcm->time; @@ -1772,7 +1809,7 @@ static int decode_ssr4(rtcm_t *rtcm, int sys, int subtype) type=getbitu(rtcm->buff,24,12); if ((nsat=decode_ssr1_head(rtcm,sys,subtype,&sync,&iod,&udint,&refd,&i))<0) { - trace(2,"rtcm3 %d length error: len=%d\n",type,rtcm->len); + trace(2,"rtcm3 %d/%d length error: len=%d\n",type,subtype,rtcm->len); return -1; } switch (sys) { @@ -1780,14 +1817,13 @@ static int decode_ssr4(rtcm_t *rtcm, int sys, int subtype) case SYS_GLO: np=5; ni= 8; nj= 0; offp= 0; break; case SYS_GAL: np=6; ni=10; nj= 0; offp= 0; break; case SYS_QZS: np=4; ni= 8; nj= 0; offp=192; break; - case SYS_CMP: np=6; ni=10; nj=8; offp= 0; break; + case SYS_CMP: np=6; ni=10; nj= 8; offp= 0; break; case SYS_SBS: np=6; ni= 9; nj=24; offp=120; break; default: return sync?0:10; } if (subtype>0) { /* IGS SSR */ np=6; ni=8; nj=0; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i,np)+offp; i+=np; @@ -1805,7 +1841,7 @@ static int decode_ssr4(rtcm_t *rtcm, int sys, int subtype) dclk [2]=getbits(rtcm->buff,i,27)*2E-8; i+=27; if (!(sat=satno(sys,prn))) { - trace(2,"rtcm3 %d satellite number error: prn=%d\n",type,prn); + trace(2,"rtcm3 %d/%d satellite number error: prn=%d\n",type,subtype,prn); continue; } rtcm->ssr[sat-1].t0 [0]=rtcm->ssr[sat-1].t0 [1]=rtcm->time; @@ -1836,7 +1872,7 @@ static int decode_ssr5(rtcm_t *rtcm, int sys, int subtype) type=getbitu(rtcm->buff,24,12); if ((nsat=decode_ssr2_head(rtcm,sys,subtype,&sync,&iod,&udint,&i))<0) { - trace(2,"rtcm3 %d length error: len=%d\n",type,rtcm->len); + trace(2,"rtcm3 %d/%d length error: len=%d\n",type,subtype,rtcm->len); return -1; } switch (sys) { @@ -1844,21 +1880,20 @@ static int decode_ssr5(rtcm_t *rtcm, int sys, int subtype) case SYS_GLO: np=5; offp= 0; break; case SYS_GAL: np=6; offp= 0; break; case SYS_QZS: np=4; offp=192; break; - case SYS_CMP: np=6; offp= 1; break; + case SYS_CMP: np=6; offp= 0; break; case SYS_SBS: np=6; offp=120; break; default: return sync?0:10; } if (subtype>0) { /* IGS SSR */ np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } for (j=0;jlen*8;j++) { prn=getbitu(rtcm->buff,i,np)+offp; i+=np; ura=getbitu(rtcm->buff,i, 6); i+= 6; if (!(sat=satno(sys,prn))) { - trace(2,"rtcm3 %d satellite number error: prn=%d\n",type,prn); + trace(2,"rtcm3 %d/%d satellite number error: prn=%d\n",type,subtype,prn); continue; } rtcm->ssr[sat-1].t0 [3]=rtcm->time; @@ -1878,7 +1913,7 @@ static int decode_ssr6(rtcm_t *rtcm, int sys, int subtype) type=getbitu(rtcm->buff,24,12); if ((nsat=decode_ssr2_head(rtcm,sys,subtype,&sync,&iod,&udint,&i))<0) { - trace(2,"rtcm3 %d length error: len=%d\n",type,rtcm->len); + trace(2,"rtcm3 %d/%d length error: len=%d\n",type,subtype,rtcm->len); return -1; } switch (sys) { @@ -1886,21 +1921,20 @@ static int decode_ssr6(rtcm_t *rtcm, int sys, int subtype) case SYS_GLO: np=5; offp= 0; break; case SYS_GAL: np=6; offp= 0; break; case SYS_QZS: np=4; offp=192; break; - case SYS_CMP: np=6; offp= 1; break; + case SYS_CMP: np=6; offp= 0; break; case SYS_SBS: np=6; offp=120; break; default: return sync?0:10; } if (subtype>0) { /* IGS SSR */ np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i,np)+offp; i+=np; hrclk=getbits(rtcm->buff,i,22)*1E-4; i+=22; if (!(sat=satno(sys,prn))) { - trace(2,"rtcm3 %d satellite number error: prn=%d\n",type,prn); + trace(2,"rtcm3 %d/%d satellite number error: prn=%d\n",type,subtype,prn); continue; } rtcm->ssr[sat-1].t0 [2]=rtcm->time; @@ -1954,7 +1988,7 @@ static int decode_ssr7_head(rtcm_t *rtcm, int sys, int subtype, int *sync, static int decode_ssr7(rtcm_t *rtcm, int sys, int subtype) { const uint8_t *sigs; - double udint,bias,std=0.0,pbias[MAXCODE],stdpb[MAXCODE]; + double udint,bias,pbias[MAXCODE]; int i,j,k,type,mode,sync,iod,nsat,prn,sat,nbias,np,mw,offp,sii,swl; int dispe,sdc,yaw_ang,yaw_rate; @@ -1962,21 +1996,29 @@ static int decode_ssr7(rtcm_t *rtcm, int sys, int subtype) if ((nsat=decode_ssr7_head(rtcm,sys,subtype,&sync,&iod,&udint,&dispe,&mw, &i))<0) { - trace(2,"rtcm3 %d length error: len=%d\n",type,rtcm->len); + trace(2,"rtcm3 %d/%d length error: len=%d\n",type,subtype,rtcm->len); return -1; } - switch (sys) { + if (subtype>0) { /* IGS SSR */ + switch (sys) { + case SYS_GPS: np=6; offp= 0; sigs=ssr_igs_sig_gps; break; + case SYS_GLO: np=6; offp= 0; sigs=ssr_igs_sig_glo; break; + case SYS_GAL: np=6; offp= 0; sigs=ssr_igs_sig_gal; break; + case SYS_QZS: np=6; offp=192; sigs=ssr_igs_sig_qzs; break; + case SYS_CMP: np=6; offp= 0; sigs=ssr_igs_sig_bds; break; + case SYS_SBS: np=6; offp=119; sigs=ssr_igs_sig_sbs; break; + default: return sync?0:10; + } + } else { + switch (sys) { case SYS_GPS: np=6; offp= 0; sigs=ssr_sig_gps; break; case SYS_GLO: np=5; offp= 0; sigs=ssr_sig_glo; break; case SYS_GAL: np=6; offp= 0; sigs=ssr_sig_gal; break; case SYS_QZS: np=4; offp=192; sigs=ssr_sig_qzs; break; - case SYS_CMP: np=6; offp= 1; sigs=ssr_sig_cmp; break; + case SYS_CMP: np=6; offp= 0; sigs=ssr_sig_bds; break; + case SYS_SBS: np=6; offp=120; sigs=ssr_sig_sbs; break; default: return sync?0:10; - } - if (subtype>0) { /* IGS SSR */ - np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + } } for (j=0;jlen*8;j++) { prn =getbitu(rtcm->buff,i,np)+offp; i+=np; @@ -1984,26 +2026,22 @@ static int decode_ssr7(rtcm_t *rtcm, int sys, int subtype) yaw_ang =getbitu(rtcm->buff,i, 9); i+= 9; yaw_rate=getbits(rtcm->buff,i, 8); i+= 8; - for (k=0;klen*8;k++) { mode=getbitu(rtcm->buff,i, 5); i+= 5; sii =getbitu(rtcm->buff,i, 1); i+= 1; /* integer-indicator */ swl =getbitu(rtcm->buff,i, 2); i+= 2; /* WL integer-indicator */ sdc =getbitu(rtcm->buff,i, 4); i+= 4; /* discontinuity counter */ bias=getbits(rtcm->buff,i,20); i+=20; /* phase bias (m) */ - if (subtype==0) { - std =getbitu(rtcm->buff,i,17); i+=17; /* phase bias std-dev (m) */ - } if (sigs[mode]) { pbias[sigs[mode]-1]=bias*0.0001; /* (m) */ - stdpb[sigs[mode]-1]=std *0.0001; /* (m) */ } else { - trace(2,"rtcm3 %d not supported mode: mode=%d\n",type,mode); + trace(2,"rtcm3 %d/%d not supported mode: mode=%d\n",type,subtype,mode); } } if (!(sat=satno(sys,prn))) { - trace(2,"rtcm3 %d satellite number error: prn=%d\n",type,prn); + trace(2,"rtcm3 %d/%d satellite number error: prn=%d\n",type,subtype,prn); continue; } rtcm->ssr[sat-1].t0 [5]=rtcm->time; @@ -2014,7 +2052,6 @@ static int decode_ssr7(rtcm_t *rtcm, int sys, int subtype) for (k=0;kssr[sat-1].pbias[k]=pbias[k]; - rtcm->ssr[sat-1].stdpb[k]=(float)stdpb[k]; } } return 20; @@ -2088,7 +2125,7 @@ static void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, case SYS_GAL: sig[i]=msm_sig_gal[h->sigs[i]-1]; break; case SYS_QZS: sig[i]=msm_sig_qzs[h->sigs[i]-1]; break; case SYS_SBS: sig[i]=msm_sig_sbs[h->sigs[i]-1]; break; - case SYS_CMP: sig[i]=msm_sig_cmp[h->sigs[i]-1]; break; + case SYS_CMP: sig[i]=msm_sig_bds[h->sigs[i]-1]; break; case SYS_IRN: sig[i]=msm_sig_irn[h->sigs[i]-1]; break; default: sig[i]=""; break; } @@ -2760,10 +2797,12 @@ extern int decode_rtcm3(rtcm_t *rtcm) case 1261: ret=decode_ssr4(rtcm,SYS_CMP,0); break; /* draft */ case 1262: ret=decode_ssr5(rtcm,SYS_CMP,0); break; /* draft */ case 1263: ret=decode_ssr6(rtcm,SYS_CMP,0); break; /* draft */ - case 11: ret=decode_ssr7(rtcm,SYS_GPS,0); break; /* tentative */ - case 12: ret=decode_ssr7(rtcm,SYS_GAL,0); break; /* tentative */ - case 13: ret=decode_ssr7(rtcm,SYS_QZS,0); break; /* tentative */ - case 14: ret=decode_ssr7(rtcm,SYS_CMP,0); break; /* tentative */ + case 1265: ret=decode_ssr7(rtcm,SYS_GPS,0); break; /* draft */ + case 1266: ret=decode_ssr7(rtcm,SYS_GLO,0); break; /* draft */ + case 1267: ret=decode_ssr7(rtcm,SYS_GAL,0); break; /* draft */ + case 1268: ret=decode_ssr7(rtcm,SYS_QZS,0); break; /* draft */ + case 1269: ret=decode_ssr7(rtcm,SYS_SBS,0); break; /* draft */ + case 1270: ret=decode_ssr7(rtcm,SYS_CMP,0); break; /* draft */ case 4073: ret=decode_type4073(rtcm); break; case 4076: ret=decode_type4076(rtcm); break; } diff --git a/src/rtcm3e.c b/src/rtcm3e.c index 6f68a4410..7a57f7899 100644 --- a/src/rtcm3e.c +++ b/src/rtcm3e.c @@ -71,7 +71,7 @@ extern const char *msm_sig_glo[32]; extern const char *msm_sig_gal[32]; extern const char *msm_sig_qzs[32]; extern const char *msm_sig_sbs[32]; -extern const char *msm_sig_cmp[32]; +extern const char *msm_sig_bds[32]; extern const char *msm_sig_irn[32]; /* SSR signal and tracking mode IDs ------------------------------------------*/ @@ -79,7 +79,7 @@ extern const uint8_t ssr_sig_gps[32]; extern const uint8_t ssr_sig_glo[32]; extern const uint8_t ssr_sig_gal[32]; extern const uint8_t ssr_sig_qzs[32]; -extern const uint8_t ssr_sig_cmp[32]; +extern const uint8_t ssr_sig_bds[32]; extern const uint8_t ssr_sig_sbs[32]; /* SSR update intervals ------------------------------------------------------*/ @@ -1447,29 +1447,26 @@ static int encode_ssr_head(int type, rtcm_t *rtcm, int sys, int subtype, if (subtype==0) { /* RTCM SSR */ ns=(sys==SYS_QZS)?4:6; - switch (sys) { - case SYS_GPS: msgno=(type==7)?11:1056+type; break; - case SYS_GLO: msgno=(type==7)? 0:1062+type; break; - case SYS_GAL: msgno=(type==7)?12:1239+type; break; /* draft */ - case SYS_QZS: msgno=(type==7)?13:1245+type; break; /* draft */ - case SYS_CMP: msgno=(type==7)?14:1257+type; break; /* draft */ - case SYS_SBS: msgno=(type==7)? 0:1251+type; break; /* draft */ - default: return 0; - } - if (msgno==0) { - return 0; - } + switch (sys) { + case SYS_GPS: msgno=(type==7)?1265:1056+type; break; + case SYS_GLO: msgno=(type==7)?1266:1062+type; break; + case SYS_GAL: msgno=(type==7)?1267:1239+type; break; /* draft */ + case SYS_QZS: msgno=(type==7)?1268:1245+type; break; /* draft */ + case SYS_CMP: msgno=(type==7)?1269:1257+type; break; /* draft */ + case SYS_SBS: msgno=(type==7)?1270:1251+type; break; /* draft */ + default: return 0; + } setbitu(rtcm->buff,i,12,msgno); i+=12; /* message type */ - if (sys==SYS_GLO) { - tow=time2gpst(timeadd(gpst2utc(rtcm->time),10800.0),&week); - epoch=ROUND(tow)%86400; - setbitu(rtcm->buff,i,17,epoch); i+=17; /* GLONASS epoch time */ - } - else { - tow=time2gpst(rtcm->time,&week); - epoch=ROUND(tow)%604800; - setbitu(rtcm->buff,i,20,epoch); i+=20; /* GPS epoch time */ + if (sys==SYS_GLO) { + tow=time2gpst(timeadd(gpst2utc(rtcm->time),10800.0),&week); + epoch=ROUND(tow)%86400; + setbitu(rtcm->buff,i,17,epoch); i+=17; /* GLONASS epoch time */ + } + else { + tow=time2gpst(rtcm->time,&week); + epoch=ROUND(tow)%604800; + setbitu(rtcm->buff,i,20,epoch); i+=20; /* GPS epoch time */ } } else { /* IGS SSR */ @@ -1502,31 +1499,59 @@ static int encode_ssr_head(int type, rtcm_t *rtcm, int sys, int subtype, setbitu(rtcm->buff,i,ns,nsat ); i+=ns; /* no of satellites */ return i; } -/* SSR signal and tracking mode IDs ------------------------------------------*/ +/* RTCM SSR signal and tracking mode IDs --------------------------------------*/ static const int codes_gps[32]={ - CODE_L1C,CODE_L1P,CODE_L1W,CODE_L1S,CODE_L1L,CODE_L2C,CODE_L2D,CODE_L2S, - CODE_L2L,CODE_L2X,CODE_L2P,CODE_L2W, 0, 0,CODE_L5I,CODE_L5Q + CODE_L1C,CODE_L1P,CODE_L1W, 0, 0,CODE_L2C,CODE_L2D,CODE_L2S, + CODE_L2L,CODE_L2X,CODE_L2P,CODE_L2W,CODE_L2Y,CODE_L2M,CODE_L5I,CODE_L5Q, + CODE_L5X,CODE_L1S,CODE_L1L,CODE_L1X }; static const int codes_glo[32]={ + CODE_L1C,CODE_L1P,CODE_L2C,CODE_L2P,CODE_L4A,CODE_L4B,CODE_L4X,CODE_L6A, + CODE_L6B,CODE_L6X,CODE_L3I,CODE_L3Q,CODE_L3X +}; +static const int codes_gal[32]={ + CODE_L1A,CODE_L1B,CODE_L1C,CODE_L1X,CODE_L1Z,CODE_L5I,CODE_L5Q,CODE_L5X, + CODE_L7I,CODE_L7Q,CODE_L7X,CODE_L8I,CODE_L8Q,CODE_L8X,CODE_L6A,CODE_L6B, + CODE_L6C,CODE_L6X,CODE_L6Z +}; +static const int codes_qzs[32]={ + CODE_L1C,CODE_L1S,CODE_L1L,CODE_L2S,CODE_L2L,CODE_L2X,CODE_L5I,CODE_L5Q, + CODE_L5X,CODE_L6S,CODE_L6L,CODE_L6X,CODE_L1X,CODE_L1Z,CODE_L5D,CODE_L5P, + CODE_L5Z,CODE_L6E,CODE_L6Z +}; +static const int codes_bds[32]={ + CODE_L2I,CODE_L2Q,CODE_L2X,CODE_L6I,CODE_L6Q,CODE_L6X,CODE_L7I,CODE_L7Q, + CODE_L7X,CODE_L1D,CODE_L1P,CODE_L1X,CODE_L5D,CODE_L5P,CODE_L5X,CODE_L1A, + 0, 0,CODE_L6A +}; +static const int codes_sbs[32]={ + CODE_L1C,CODE_L5I,CODE_L5Q +}; +/* IGS SSR signal and tracking mode IDs ---------------------------------------*/ +static const int codes_igs_gps[32]={ + CODE_L1C,CODE_L1P,CODE_L1W,CODE_L1S,CODE_L1L,CODE_L2C,CODE_L2D,CODE_L2S, + CODE_L2L, 0,CODE_L2P,CODE_L2W, 0, 0,CODE_L5I,CODE_L5Q +}; +static const int codes_igs_glo[32]={ CODE_L1C,CODE_L1P,CODE_L2C,CODE_L2P,CODE_L4A,CODE_L4B,CODE_L6A,CODE_L6B, CODE_L3I,CODE_L3Q }; -static const int codes_gal[32]={ +static const int codes_igs_gal[32]={ CODE_L1A,CODE_L1B,CODE_L1C, 0, 0,CODE_L5I,CODE_L5Q, 0, - CODE_L7I,CODE_L7Q, 0,CODE_L8I,CODE_L8Q, 0,CODE_L6A,CODE_L6B, + CODE_L7I,CODE_L7Q, 0, 0, 0, 0,CODE_L6A,CODE_L6B, CODE_L6C }; -static const int codes_qzs[32]={ +static const int codes_igs_qzs[32]={ CODE_L1C,CODE_L1S,CODE_L1L,CODE_L2S,CODE_L2L, 0,CODE_L5I,CODE_L5Q, 0,CODE_L6S,CODE_L6L, 0, 0, 0, 0, 0, 0,CODE_L6E }; -static const int codes_bds[32]={ +static const int codes_igs_bds[32]={ CODE_L2I,CODE_L2Q, 0,CODE_L6I,CODE_L6Q, 0,CODE_L7I,CODE_L7Q, 0,CODE_L1D,CODE_L1P, 0,CODE_L5D,CODE_L5P, 0,CODE_L1A, 0, 0,CODE_L6A }; -static const int codes_sbs[32]={ +static const int codes_igs_sbs[32]={ CODE_L1C,CODE_L5I,CODE_L5Q }; /* encode SSR 1: orbit corrections -------------------------------------------*/ @@ -1542,14 +1567,13 @@ static int encode_ssr1(rtcm_t *rtcm, int sys, int subtype, int sync) case SYS_GLO: np=5; ni= 8; nj= 0; offp= 0; break; case SYS_GAL: np=6; ni=10; nj= 0; offp= 0; break; case SYS_QZS: np=4; ni= 8; nj= 0; offp=192; break; - case SYS_CMP: np=6; ni=10; nj=24; offp= 1; break; + case SYS_CMP: np=6; ni=10; nj=24; offp= 0; break; case SYS_SBS: np=6; ni= 9; nj=24; offp=120; break; default: return 0; } if (subtype>0) { /* IGS SSR */ np=6; ni=8; nj=0; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } /* number of satellites */ for (j=nsat=0;j0) { /* IGS SSR */ np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } /* number of satellites */ for (j=nsat=0;j0) { /* IGS SSR */ + switch (sys) { + case SYS_GPS: np=6; offp= 0; codes=codes_igs_gps; break; + case SYS_GLO: np=6; offp= 0; codes=codes_igs_glo; break; + case SYS_GAL: np=6; offp= 0; codes=codes_igs_gal; break; + case SYS_QZS: np=6; offp=192; codes=codes_igs_qzs; break; + case SYS_CMP: np=6; offp= 0; codes=codes_igs_bds; break; + case SYS_SBS: np=6; offp=119; codes=codes_igs_sbs; break; + default: return 0; + } + } else { + switch (sys) { case SYS_GPS: np=6; offp= 0; codes=codes_gps; break; case SYS_GLO: np=5; offp= 0; codes=codes_glo; break; case SYS_GAL: np=6; offp= 0; codes=codes_gal; break; case SYS_QZS: np=4; offp=192; codes=codes_qzs; break; - case SYS_CMP: np=6; offp= 1; codes=codes_bds; break; + case SYS_CMP: np=6; offp= 0; codes=codes_bds; break; case SYS_SBS: np=6; offp=120; codes=codes_sbs; break; default: return 0; - } - if (subtype>0) { /* IGS SSR */ - np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + } } /* number of satellites */ for (j=nsat=0;j0) { /* IGS SSR */ np=6; ni=8; nj=0; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } /* number of satellites */ for (j=nsat=0;j0) { /* IGS SSR */ np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } /* number of satellites */ for (j=nsat=0;j0) { /* IGS SSR */ np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + if (sys==SYS_SBS) offp=119; } /* number of satellites */ for (j=nsat=0;j0) { /* IGS SSR */ + switch (sys) { + case SYS_GPS: np=6; offp= 0; codes=codes_igs_gps; break; + case SYS_GLO: np=6; offp= 0; codes=codes_igs_glo; break; + case SYS_GAL: np=6; offp= 0; codes=codes_igs_gal; break; + case SYS_QZS: np=6; offp=192; codes=codes_igs_qzs; break; + case SYS_CMP: np=6; offp= 0; codes=codes_igs_bds; break; + case SYS_SBS: np=6; offp=119; codes=codes_igs_sbs; break; + default: return 0; + } + } else { + switch (sys) { case SYS_GPS: np=6; offp= 0; codes=codes_gps; break; case SYS_GLO: np=5; offp= 0; codes=codes_glo; break; case SYS_GAL: np=6; offp= 0; codes=codes_gal; break; case SYS_QZS: np=4; offp=192; codes=codes_qzs; break; - case SYS_CMP: np=6; offp= 1; codes=codes_bds; break; + case SYS_CMP: np=6; offp= 0; codes=codes_bds; break; case SYS_SBS: np=6; offp=120; codes=codes_sbs; break; default: return 0; - } - if (subtype>0) { /* IGS SSR */ - np=6; - if (sys==SYS_CMP) offp=0; - else if (sys==SYS_SBS) offp=119; + } } /* number of satellites */ for (j=nsat=0;jssr[j].pbias[codes[k]-1]==0.0) continue; code[nbias]=k; - pbias[nbias ]=ROUND(rtcm->ssr[j].pbias[codes[k]-1]/0.0001); - stdpb[nbias++]=ROUND(rtcm->ssr[j].stdpb[codes[k]-1]/0.0001); + pbias[nbias++]=ROUND(rtcm->ssr[j].pbias[codes[k]-1]/0.0001); } yaw_ang =ROUND(rtcm->ssr[j].yaw_ang /180.0* 256.0); yaw_rate=ROUND(rtcm->ssr[j].yaw_rate/180.0*8192.0); @@ -1901,9 +1934,6 @@ static int encode_ssr7(rtcm_t *rtcm, int sys, int subtype, int sync) setbitu(rtcm->buff,i, 2,0 ); i+= 2; /* WL integer-indicator */ setbitu(rtcm->buff,i, 4,0 ); i+= 4; /* discont counter */ setbits(rtcm->buff,i,20,pbias[k]); i+=20; /* phase bias */ - if (subtype==0) { - setbits(rtcm->buff,i,17,stdpb[k]); i+=17; /* std-dev ph-bias */ - } } } rtcm->nbit=i; @@ -1946,7 +1976,7 @@ static int to_sigid(int sys, uint8_t code) case SYS_GAL: msm_sig=msm_sig_gal; break; case SYS_QZS: msm_sig=msm_sig_qzs; break; case SYS_SBS: msm_sig=msm_sig_sbs; break; - case SYS_CMP: msm_sig=msm_sig_cmp; break; + case SYS_CMP: msm_sig=msm_sig_bds; break; case SYS_IRN: msm_sig=msm_sig_irn; break; default: return 0; } @@ -2786,10 +2816,12 @@ extern int encode_rtcm3(rtcm_t *rtcm, int type, int subtype, int sync) case 1261: ret=encode_ssr4(rtcm,SYS_CMP,0,sync); break; /* draft */ case 1262: ret=encode_ssr5(rtcm,SYS_CMP,0,sync); break; /* draft */ case 1263: ret=encode_ssr6(rtcm,SYS_CMP,0,sync); break; /* draft */ - case 11: ret=encode_ssr7(rtcm,SYS_GPS,0,sync); break; /* tentative */ - case 12: ret=encode_ssr7(rtcm,SYS_GAL,0,sync); break; /* tentative */ - case 13: ret=encode_ssr7(rtcm,SYS_QZS,0,sync); break; /* tentative */ - case 14: ret=encode_ssr7(rtcm,SYS_CMP,0,sync); break; /* tentative */ + case 1265: ret=encode_ssr7(rtcm,SYS_GPS,0,sync); break; /* draft */ + case 1266: ret=encode_ssr7(rtcm,SYS_GLO,0,sync); break; /* draft */ + case 1267: ret=encode_ssr7(rtcm,SYS_GAL,0,sync); break; /* draft */ + case 1268: ret=encode_ssr7(rtcm,SYS_QZS,0,sync); break; /* draft */ + case 1269: ret=encode_ssr7(rtcm,SYS_SBS,0,sync); break; /* draft */ + case 1270: ret=encode_ssr7(rtcm,SYS_CMP,0,sync); break; /* draft */ case 4073: ret=encode_type4073(rtcm,subtype,sync); break; case 4076: ret=encode_type4076(rtcm,subtype,sync); break; } diff --git a/src/rtklib.h b/src/rtklib.h index 9a95e7987..d54a362ca 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -837,7 +837,6 @@ typedef struct { /* SSR correction type */ double hrclk; /* high-rate clock correction (m) */ float cbias[MAXCODE]; /* code biases (m) */ double pbias[MAXCODE]; /* phase biases (m) */ - float stdpb[MAXCODE]; /* std-dev of phase biases (m) */ double yaw_ang,yaw_rate; /* yaw angle and yaw rate (deg,deg/s) */ uint8_t update; /* update flag (0:no update,1:update) */ } ssr_t; From 2c64270ff6ba39bcc7a9cab913d2017f3b692760 Mon Sep 17 00:00:00 2001 From: oaq Date: Sun, 10 May 2026 12:41:25 +1000 Subject: [PATCH 20/25] rtksrv update_ssr: remove check for ephemeris iode here The user of the ssr data is responsible for checking the iode, see the checks in seleph() so the check here is redundant and delays the use of the ssr data until after the correct ephemeris is receiver and new ssr data is received to trigger another update. --- src/rtksvr.c | 39 ++++++++++----------------------------- 1 file changed, 10 insertions(+), 29 deletions(-) diff --git a/src/rtksvr.c b/src/rtksvr.c index cacbf0996..9e672f01f 100644 --- a/src/rtksvr.c +++ b/src/rtksvr.c @@ -312,37 +312,18 @@ static void update_antpos(rtksvr_t *svr, int index) { /* update ssr corrections ----------------------------------------------------*/ static void update_ssr(rtksvr_t *svr, int index) { - int i,sys,prn,iode; - - for (i=0;irtcm[index].ssr[i].update) continue; - - /* check consistency between iods of orbit and clock */ - if (svr->rtcm[index].ssr[i].iod[0]!=svr->rtcm[index].ssr[i].iod[1]) { - continue; - } - svr->rtcm[index].ssr[i].update=0; + for (int i = 0; i < MAXSAT; i++) { + if (!svr->rtcm[index].ssr[i].update) continue; - iode=svr->rtcm[index].ssr[i].iode; - sys=satsys(i+1,&prn); - - /* check corresponding ephemeris exists */ - if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS) { - if (svr->nav.eph[i ].iode!=iode&& - svr->nav.eph[i+MAXSAT].iode!=iode) { - continue; - } - } - else if (sys==SYS_GLO) { - if (svr->nav.geph[prn-1 ].iode!=iode&& - svr->nav.geph[prn-1+MAXPRNGLO].iode!=iode) { - continue; - } - } - svr->nav.ssr[i]=svr->rtcm[index].ssr[i]; - } - svr->nmsg[index][7]++; + // Check consistency between iods of orbit and clock. + if (svr->rtcm[index].ssr[i].iod[0] != svr->rtcm[index].ssr[i].iod[1]) + continue; + + svr->nav.ssr[i] = svr->rtcm[index].ssr[i]; + svr->rtcm[index].ssr[i].update = 0; } + svr->nmsg[index][7]++; +} /* update rtk server struct --------------------------------------------------*/ static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int ephsat, int ephset, sbsmsg_t *sbsmsg, int index, From e31a50373ee6ef4d95651d7f4ce94ee0837d20c1 Mon Sep 17 00:00:00 2001 From: oaq Date: Sat, 9 May 2026 23:33:38 +1000 Subject: [PATCH 21/25] rtknavi mondlg: increase width of ssr biases column --- app/qtapp/rtknavi_qt/mondlg.cpp | 2 +- app/winapp/rtknavi/mondlg.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/app/qtapp/rtknavi_qt/mondlg.cpp b/app/qtapp/rtknavi_qt/mondlg.cpp index 174c69477..158d53547 100644 --- a/app/qtapp/rtknavi_qt/mondlg.cpp +++ b/app/qtapp/rtknavi_qt/mondlg.cpp @@ -2063,7 +2063,7 @@ void MonitorDialog::setRtcmSsr() header << tr("SAT") << tr("Status") << tr("UDI (s)") << tr("UDHR (s)") << tr("IOD") << tr("URA") << tr("Datum") << tr("T0") << tr("D0-A (m)") << tr("D0-C (m)") << tr("D0-R (m)") << tr("D1-A (mm/s)") << tr("D1-C (mm/s)") << tr("D1-R (mm/s)") << tr("C0 (m)") << tr("C1 (mm/s)") << tr("C2 (mm/s²)") << tr("C-HR (m)") << tr("Code Bias (m)") << tr("Phase Bias (m)"); - int i, width[] = { 46, 60, 70, 90, 30, 25, 70, 115, 90, 90, 90, 120, 120, 120, 90, 120, 120, 120, 200, 200 }; + int i, width[] = { 46, 60, 70, 90, 30, 25, 70, 115, 90, 90, 90, 120, 120, 120, 90, 120, 120, 120, 400, 400 }; ui->tWConsole->setColumnCount(20); ui->tWConsole->setRowCount(0); diff --git a/app/winapp/rtknavi/mondlg.cpp b/app/winapp/rtknavi/mondlg.cpp index b9e71bfbe..51897b931 100644 --- a/app/winapp/rtknavi/mondlg.cpp +++ b/app/winapp/rtknavi/mondlg.cpp @@ -1918,7 +1918,7 @@ void __fastcall TMonitorDialog::SetRtcmSsr(void) "Phase Bias(m)" }; int i,width[]={ - 25,30,30,30,30,25,15,115,50,50,50,50,50,50,50,50,50,50,180,180 + 25,30,30,30,30,25,15,115,50,50,50,50,50,50,50,50,50,50,250,250 }; char *code; From 192ea5b8d3fc916ac652b8ced3507580240d8903 Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Tue, 12 May 2026 15:24:12 -0600 Subject: [PATCH 22/25] Extend cycle slip detection for PPP solutions to include freqs beyond L1 and L2 using code from the RTK solutions (rtkpos.c) as a template --- src/ppp.c | 93 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 51 insertions(+), 42 deletions(-) diff --git a/src/ppp.c b/src/ppp.c index 5c6a5a91c..0ef346032 100644 --- a/src/ppp.c +++ b/src/ppp.c @@ -388,28 +388,27 @@ static inline void initx(rtk_t *rtk, double xi, double var, int i) rtk->P[i+i*rtk->nx]=var; } /* geometry-free phase measurement -------------------------------------------*/ -static double gfmeas(const obsd_t *obs, const nav_t *nav) +static double gfmeas(const obsd_t *obs, const nav_t *nav, int f2) { double freq1,freq2; freq1=sat2freq(obs->sat,obs->code[0],nav); - freq2=sat2freq(obs->sat,obs->code[1],nav); - if (freq1==0.0||freq2==0.0||obs->L[0]==0.0||obs->L[1]==0.0) return 0.0; - return (obs->L[0]/freq1-obs->L[1]/freq2)*CLIGHT; + freq2=sat2freq(obs->sat,obs->code[f2],nav); + if (freq1==0.0||freq2==0.0||obs->L[0]==0.0||obs->L[f2]==0.0) return 0.0; + return (obs->L[0]/freq1-obs->L[f2]/freq2)*CLIGHT; } /* Melbourne-Wubbena linear combination --------------------------------------*/ -static double mwmeas(const obsd_t *obs, const nav_t *nav) +static double mwmeas(const obsd_t *obs, const nav_t *nav, int f2) { double freq1,freq2; freq1=sat2freq(obs->sat,obs->code[0],nav); - freq2=sat2freq(obs->sat,obs->code[1],nav); + freq2=sat2freq(obs->sat,obs->code[f2],nav); - if (freq1==0.0||freq2==0.0||obs->L[0]==0.0||obs->L[1]==0.0|| - obs->P[0]==0.0||obs->P[1]==0.0) return 0.0; - trace(3,"mwmeas: %12.1f %12.1f %15.3f %15.3f %15.3f %15.3f %d %d\n",freq1,freq2,obs->L[0],obs->L[1],obs->P[0],obs->P[1],obs->code[0],obs->code[1]); - return (obs->L[0]-obs->L[1])*CLIGHT/(freq1-freq2)- - (freq1*obs->P[0]+freq2*obs->P[1])/(freq1+freq2); + if (freq1==0.0||freq2==0.0||obs->L[0]==0.0||obs->L[f2]==0.0|| + obs->P[0]==0.0||obs->P[f2]==0.0) return 0.0; + return (obs->L[0]-obs->L[f2])*CLIGHT/(freq1-freq2)- + (freq1*obs->P[0]+freq2*obs->P[f2])/(freq1+freq2); } /* antenna corrected measurements --------------------------------------------*/ static void corr_meas(const obsd_t *obs, const nav_t *nav, const double *azel, @@ -470,49 +469,57 @@ static void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) /* detect cycle slip by geometry free phase jump -----------------------------*/ static void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - double g0,g1; - int i,j; + double gf0,gf1; + int i,k,sat; trace(4,"detslp_gf: n=%d\n",n); + if (rtk->opt.thresslip==0) return; /* return if check disabled */ for (i=0;issat[obs[i].sat-1].gf[0]; - rtk->ssat[obs[i].sat-1].gf[0]=g1; - - trace(4,"detslip_gf: sat=%2d gf0=%8.3f gf1=%8.3f\n",obs[i].sat,g0,g1); - - if (g0!=0.0&&fabs(g1-g0)>rtk->opt.thresslip) { - trace(3,"detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n", - obs[i].sat,g0,g1); - - for (j=0;jopt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=LLI_SLIP; + sat=obs[i].sat; + for (k=1;kopt.nf;k++) { + /* skip check if slip already detected */ + if (rtk->ssat[sat-1].slip[k]&LLI_SLIP) continue; + /* calc SD geomotry free LC of phase between freq0 and freqk */ + if ((gf1=gfmeas(obs+i,nav,k))==0.0) continue; + + gf0=rtk->ssat[sat-1].gf[k-1]; /* retrieve previous gf */ + rtk->ssat[sat-1].gf[k-1]=gf1; /* save current gf for next epoch */ + + if (gf0!=0.0&&fabs(gf1-gf0)>rtk->opt.thresslip) { + rtk->ssat[sat-1].slip[0]|=LLI_SLIP; + rtk->ssat[sat-1].slip[k]|=LLI_SLIP; + trace(3,"slip detected GF jump (sat=%2d L1-L%d dGF=%.3f)\n", + sat,k+1,gf0-gf1); + } } } } /* detect slip by Melbourne-Wubbena linear combination jump ------------------*/ static void detslp_mw(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - double w0,w1; - int i,j; + double mw0,mw1; + int i,j,k,sat; trace(4,"detslp_mw: n=%d\n",n); for (i=0;issat[obs[i].sat-1].mw[0]; - rtk->ssat[obs[i].sat-1].mw[0]=w1; - - trace(4,"detslip_mw: sat=%2d mw0=%8.3f mw1=%8.3f\n",obs[i].sat,w0,w1); - - if (w0!=0.0&&fabs(w1-w0)>THRES_MW_JUMP) { - trace(3,"detslip_mw: slip detected sat=%2d mw=%8.3f->%8.3f\n", - obs[i].sat,w0,w1); - - for (j=0;jopt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=LLI_SLIP; + sat=obs[i].sat; + for (k=1;kopt.nf;k++) { + /* skip check if slip already detected */ + if (rtk->ssat[sat-1].slip[k]&LLI_SLIP) continue; + /* calc MW LC of phase between freq0 and freqk */ + if ((mw1=mwmeas(obs+i,nav,k))==0.0) continue; + + mw0=rtk->ssat[sat-1].mw[k-1]; /* retrieve previous mw */ + rtk->ssat[sat-1].mw[k-1]=mw1; /* save current mw for next epoch */ + + if (mw0!=0.0&&fabs(mw1-mw0)>THRES_MW_JUMP) { + rtk->ssat[sat-1].slip[0]|=LLI_SLIP; + rtk->ssat[sat-1].slip[k]|=LLI_SLIP; + trace(3,"slip detected MW jump (sat=%2d L1-L%d dMW=%.3f)\n", + sat,k+1,mw0-mw1); + } } } } @@ -739,7 +746,7 @@ static void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) detslp_mw(rtk,obs,n,nav); for (f=0;fopt);f++) { - + offset=0; /* reset phase-bias if expire obs outage counter */ for (i=0;issat[i].outc[f]>(uint32_t)rtk->opt.maxout|| @@ -757,7 +764,8 @@ static void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) if (rtk->opt.ionoopt==IONOOPT_IFLC) { bias[i]=Lc-Pc; - slip[i]=rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[1]; + int f2=seliflc(rtk->opt.nf,rtk->ssat[sat-1].sys); + slip[i]=rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[f2]; } else if (L[f]!=0.0&&P[f]!=0.0) { freq1=sat2freq(sat,obs[i].code[0],nav); @@ -794,6 +802,7 @@ static void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) /* reinitialize phase-bias if detecting cycle slip */ initx(rtk,bias[i],VAR_BIAS,IB(sat,f,&rtk->opt)); + trace(3,"init bias: sat=%d frq=%d\n", sat,f); /* reset fix flags */ for (k=0;kambc[sat-1].flags[k]=0; From eac7c0cfd80f3ef54682a28f9c80bed0318af472 Mon Sep 17 00:00:00 2001 From: oaq Date: Wed, 20 Nov 2024 20:15:26 +1100 Subject: [PATCH 23/25] rtkplot: avoid interger overflow in intermediate calcs --- app/qtapp/rtkplot_qt/plotmain.cpp | 8 ++++---- app/winapp/rtkplot/plotmain.cpp | 8 +++++--- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/app/qtapp/rtkplot_qt/plotmain.cpp b/app/qtapp/rtkplot_qt/plotmain.cpp index e349cf201..8ea510037 100644 --- a/app/qtapp/rtkplot_qt/plotmain.cpp +++ b/app/qtapp/rtkplot_qt/plotmain.cpp @@ -1532,12 +1532,11 @@ void Plot::mouseDownSolution(int x, int y) graphTriple[0]->getLimits(xl, yl); graphTriple[0]->toPoint(x_pos, yl[1], pnt); - if ((x - pnt.x()) * (x - pnt.x()) + (y - pnt.y()) * (y - pnt.y()) < 5*5) { + double dx = x - pnt.x(), dy = y - pnt.y(); + if (dx * dx + dy * dy < 5 * 5) { setCursor(Qt::SizeHorCursor); - dragState = 20; refresh(); - return; } } @@ -1582,7 +1581,8 @@ void Plot::mouseDownObservation(int x, int y) graphSingle->getLimits(xl, yl); graphSingle->toPoint(x_pos, yl[1], pnt); - if ((x - pnt.x()) * (x - pnt.x()) + (y - pnt.y()) * (y - pnt.y()) < 5*5) { + double dx = x - pnt.x(), dy = y - pnt.y(); + if (dx * dx + dy * dy < 5 * 5) { setCursor(Qt::SizeHorCursor); dragState = 20; refresh(); diff --git a/app/winapp/rtkplot/plotmain.cpp b/app/winapp/rtkplot/plotmain.cpp index a147f6e8c..0d5d05bf5 100644 --- a/app/winapp/rtkplot/plotmain.cpp +++ b/app/winapp/rtkplot/plotmain.cpp @@ -1519,8 +1519,9 @@ void __fastcall TPlot::MouseDownSol(int X, int Y) GraphG[0]->GetLim(xl,yl); GraphG[0]->ToPoint(x,yl[1],pnt); - - if ((X-pnt.x)*(X-pnt.x)+(Y-pnt.y)*(Y-pnt.y)<25) { + + double dx = X - pnt.x, dy = Y - pnt.y; + if (dx * dx + dy * dy < 5 * 5) { Screen->Cursor=crSizeWE; Drag=20; Refresh(); @@ -1568,7 +1569,8 @@ void __fastcall TPlot::MouseDownObs(int X, int Y) GraphR->GetLim(xl,yl); GraphR->ToPoint(x,yl[1],pnt); - if ((X-pnt.x)*(X-pnt.x)+(Y-pnt.y)*(Y-pnt.y)<25) { + double dx = X - pnt.x, dy = Y - pnt.y; + if (dx * dx + dy * dy < 5 * 5) { Screen->Cursor=crSizeWE; Drag=20; Refresh(); From 50f55bcf70978eb24c49ba857e955a731c8ba807 Mon Sep 17 00:00:00 2001 From: oaq Date: Tue, 12 May 2026 21:28:46 +1000 Subject: [PATCH 24/25] cmake include GNUInstallDirs --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index df144105f..2aa958ff7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ message("RTKLIB version: ${VERSION}-${PATCH_LEVEL}") project(rtklib LANGUAGES C CXX VERSION 2.4.3) +include(GNUInstallDirs) include(CTest) option(IERS_MODEL "Use Earth models from IERS" OFF) From 7d13645658833ec25dab60f9d8b47c8a7990e254 Mon Sep 17 00:00:00 2001 From: rtklibexplorer Date: Wed, 13 May 2026 15:57:51 -0600 Subject: [PATCH 25/25] -Add VTEC spherical harmonics ionospheric model -Add support for RTCM3 SSR message 1264 - VTEC ionospheric corrections -Update ionospheric state initialization to use VTEC estimates if available --- src/ionex.c | 85 +++++++++++++++++++++++++++++++++++++++++++++++++++- src/ppp.c | 51 ++++++++++++++++++++++++------- src/rtcm3.c | 66 ++++++++++++++++++++++++++++++++++++++++ src/rtklib.h | 20 +++++++++++-- src/rtksvr.c | 5 ++++ 5 files changed, 213 insertions(+), 14 deletions(-) diff --git a/src/ionex.c b/src/ionex.c index d4cd62385..c5555e139 100644 --- a/src/ionex.c +++ b/src/ionex.c @@ -19,7 +19,9 @@ #include "rtklib.h" #define SQR(x) ((x)*(x)) +#define MIN(x,y) ((x)<=(y)?(x):(y)) #define VAR_NOTEC SQR(30.0) /* variance of no tec */ +#define VAR_SSR_VTEC SQR(10.0); /* variance of SSR VTEC */ #define MIN_EL 0.0 /* min elevation angle (rad) */ #define MIN_HGT -1000.0 /* min user height (m) */ @@ -479,6 +481,87 @@ extern int iontec(gtime_t time, const nav_t *nav, const double *pos, *delay=dels[1]; *var =vars[1]; } - trace(3,"iontec : delay=%5.2f std=%5.2f\n",*delay,sqrt(*var)); + trace(4,"iontec : delay=%5.2f std=%5.2f\n",*delay,sqrt(*var)); + return 1; +} +/* ionosphere model (VTEC spherical harmonics) --------------------------------- +* compute ionospheric delay by VTEC spherical harmonics (RTCM SSR MT1264) +* args : gtime_t time I time (GPST) +* nav_t *nav I navigation data (vtec coefficients) +* double *pos I receiver position {lat,lon,h} (rad,rad,m) +* double *azel I azimuth/elevation {az,el} (rad) +* double freq I signal frequency (Hz) +* double *delay O ionospheric delay (m) +* double *var O ionospheric delay variance (m^2) +* return : status (1:ok, 0:error) +* notes : delay is positive for code (L1 scale), i.e. corrected = meas - delay +*-----------------------------------------------------------------------------*/ +extern int ionvtec(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, double freq, double *delay, double *var) +{ + const double re=6371.0; /* fallback single-layer height (km) */ + double pppos[2],fs,vtec,P[17][17],cosl[17],sinl[17]; + double sinp,cosp,x; + int i,j,k,nmax,mmax; + char tstr[40]; + + trace(4,"ionvtec: time=%s pos=%.3f %.3f azel=%.3f %.3f\n", + time2str(time,tstr,0),pos[0]*R2D,pos[1]*R2D,azel[0]*R2D,azel[1]*R2D); + + *delay=*var=0.0; + + if (nav->vtec.nlay<=0) { + trace(2,"ionvtec: no vtec data\n"); + return 0; + } + if (pos[2]vtec.nlay;i++) { + /* ionospheric pierce point position */ + ionppp(pos,azel,re,nav->vtec.hgt[i],pppos); + + nmax=nav->vtec.nmax[i]; + mmax=nav->vtec.mmax[i]; + sinp=sin(pppos[0]); + cosp=cos(pppos[0]); + + /* fully normalized associated Legendre polynomials */ + P[0][0]=1.0; + for (j=1;j<=nmax;j++) { + /* diagonal term */ + P[j][j]=sqrt((2*j+1)/(2.0*j))*cosp*P[j-1][j-1]; + /* one above diagonal */ + P[j][j-1]=sqrt(2*j+1)*sinp*P[j-1][j-1]; + } + for (j=2;j<=nmax;j++) { + for (k=0;k<=j-2;k++) { + P[j][k]=sqrt((2*j+1)*(2*j-1)/((double)(j-k)*(j+k)))*sinp*P[j-1][k] + -sqrt((2*j+1)*(j+k-1)*(j-k-1)/((double)(j-k)*(j+k)*(2*j-3)))*P[j-2][k]; + } + } + /* cosine and sine of longitude multiples */ + cosl[0]=1.0; sinl[0]=0.0; + for (j=1;j<=mmax;j++) { + cosl[j]=cos(j*pppos[1]); + sinl[j]=sin(j*pppos[1]); + } + /* evaluate spherical harmonic expansion (TECU) */ + x=0.0; + for (j=0;j<=nmax;j++) { + x+=nav->vtec.cosC[i][j][0]*P[j][0]; + for (k=1;k<=MIN(j,mmax);k++) { + x+=nav->vtec.cosC[i][j][k]*P[j][k]*cosl[k]+ + nav->vtec.sinC[i][j][k]*P[j][k]*sinl[k]; + } + } + vtec+=x; + } + /* convert TECU to metres on L1, then scale to signal frequency */ + fs=ionmapf(pos,azel); /* mapping function */ + *delay=40.3E16/SQR(FREQL1)*vtec*SQR(FREQL1/freq)*fs; + *var=VAR_SSR_VTEC; /* use fixed variance for now */ + trace(4,"ionvtec: pppos=%.3f %.3f vtec=%.1f delay=%.3f var=%.2f, freq=%.1f\n", + pppos[0]*R2D,pppos[1]*R2D,vtec,*delay,*var,freq); return 1; } diff --git a/src/ppp.c b/src/ppp.c index 0ef346032..a4a056ec3 100644 --- a/src/ppp.c +++ b/src/ppp.c @@ -666,7 +666,7 @@ static void udtrop_ppp(rtk_t *rtk) /* temporal update of ionospheric parameters ---------------------------------*/ static void udiono_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { - double freq1,freq2,ion,sinel,pos[3],*azel; + double freq1,freq2,ion,sinel,pos[3],*azel,var=VAR_IONO; char *p; int i,j,f2,gap_resion=GAP_RESION,sat; @@ -682,27 +682,56 @@ static void udiono_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) rtk->x[j]=0.0; } } + /* reset ionosphere states if VTEC corrections just became available */ + if (rtk->vtec_used==0&&nav->vtec.nlay>0) { + for (i=0;ix[II(i+1,&rtk->opt)]=0.0; + } + rtk->vtec_used=1; // indicate that vtec coeffs have been used + } for (i=0;iopt); - if (rtk->x[j]==0.0) { + if (rtk->x[j]==0.0&&(int)rtk->ssat[i].outc[0]<=gap_resion) { /* initialize ionosphere delay estimates if zero */ + ecef2pos(rtk->sol.rr,pos); + azel=rtk->ssat[sat-1].azel; f2=seliflc(rtk->opt.nf,satsys(sat,NULL)); + if (testsnr(0,0,azel[1],obs[i].SNR[0],&rtk->opt.snrmask)) continue; freq1=sat2freq(sat,obs[i].code[0],nav); freq2=sat2freq(sat,obs[i].code[f2],nav); - if (obs[i].P[0]==0.0||obs[i].P[f2]==0.0||freq1==0.0||freq2==0.0) { - continue; + if (freq1==0.0) continue; + if (nav->vtec.nlay>0) { /* use VTEC if corrections available */ + ionvtec(obs[i].time,nav,pos,azel,freq1,&ion,&var); + if (var==0.0) continue; + } else { + if (obs[i].P[0]==0.0||obs[i].P[f2]==0.0||freq2==0.0|| + testsnr(0,f2,azel[1],obs[i].SNR[f2],&rtk->opt.snrmask)) { + continue; + } + /* use pseudorange difference adjusted by freq for initial estimate */ + int sys=satsys(sat,NULL); + double P0_corr=obs[i].P[0]; + double Pf_corr=obs[i].P[f2]; + if (&rtk->opt.sateph==EPHOPT_SSRAPC||&rtk->opt.sateph==EPHOPT_SSRCOM) { + /* apply SSR correction */ + P0_corr-=nav->ssr[obs->sat-1].cbias[obs[i].code[0]-1]; + Pf_corr-=nav->ssr[obs->sat-1].cbias[obs[i].code[f2]-1]; + } + else { /* apply code bias corrections from file */ + P0_corr-=code2bias(nav,sys,sat,obs[i].code[0],1); + Pf_corr-=code2bias(nav,sys,sat,obs[i].code[f2],1); + } + ion=(P0_corr-Pf_corr)/(SQR(FREQL1/freq1)-SQR(FREQL1/freq2)); + trace(3,"P1=%.3f P2=%.3f frq1=%.1f frq2=%.1f\n",obs[i].P[0],obs[i].P[f2],freq1,freq2); + var=VAR_IONO; } - /* use pseudorange difference adjusted by freq for initial estimate */ - ion=(obs[i].P[0]-obs[i].P[f2])/(SQR(FREQL1/freq1)-SQR(FREQL1/freq2)); - ecef2pos(rtk->sol.rr,pos); - azel=rtk->ssat[sat-1].azel; /* adjust delay estimate by path length */ ion/=ionmapf(pos,azel); - initx(rtk,ion,VAR_IONO,j); - trace(4,"ion init: sat=%d ion=%.4f\n",sat,ion); + initx(rtk,ion,var,j); + trace(3,"ion init: sat=%d ion=%.4f var=%.1f\n",sat,ion,var); } - else { + else { /* temporal update */ sinel=sin(MAX(rtk->ssat[sat-1].azel[1],5.0*D2R)); /* update variance of delay state */ rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[1]/sinel)*fabs(rtk->tt); diff --git a/src/rtcm3.c b/src/rtcm3.c index 471c204f4..8de486eef 100644 --- a/src/rtcm3.c +++ b/src/rtcm3.c @@ -2056,6 +2056,71 @@ static int decode_ssr7(rtcm_t *rtcm, int sys, int subtype) } return 20; } +/* decode SSR 8: VTEC ionosphere --------------------------------------------- */ +static int decode_ssr8(rtcm_t *rtcm, int subtype) +{ + double udint,hgt,cosC,sinC,tow; + int i=0,j,k,l,type,sync,iod,udi,nlay,nmax,mmax,np,offp,qi; + char *msg,tstr[40]; + + type=getbitu(rtcm->buff,24,12); + + i+=decode_ssr_epoch(rtcm,SYS_GPS,subtype); + udi =getbitu(rtcm->buff,i, 4); i+= 4; + udint=ssrudint[udi]; + sync =getbitu(rtcm->buff,i,1); i+= 1; + iod =getbitu(rtcm->buff,i,4); i+= 4; + getbitu(rtcm->buff,i,16); i+=16; /* provider ID */ + getbitu(rtcm->buff,i, 4); i+= 4; /* solution ID */ + qi =getbitu(rtcm->buff,i,9); i+= 9; /* quality indicator */ + nlay =getbitu(rtcm->buff,i,2)+1; i+= 2; /* number of layers (1-4) */ + + if (rtcm->outtype) { + time2str(rtcm->time,tstr,2); + msg=rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg," %s nsat= iod=%2d udi=%2d sync=%d",tstr,iod,udi,sync); + } + + if (nlay>4) { + trace(2,"rtcm3 %d too many layers: nlay=%d\n",type,nlay); + return -1; + } + + for (j=0;jlen*8;j++) { + hgt =getbitu(rtcm->buff,i,8)*10; i+=8; /* height (km) DF472 */ + nmax =getbitu(rtcm->buff,i, 4)+1; i+= 4; /* degree DF473 */ + mmax =getbitu(rtcm->buff,i, 4)+1; i+= 4; /* order DF474 */ + + rtcm->nav.vtec.hgt[j]=hgt; + rtcm->nav.vtec.nmax[j]=nmax; + rtcm->nav.vtec.mmax[j]=mmax; + + /* clear previous coefficients */ + memset(rtcm->nav.vtec.cosC, 0, sizeof(rtcm->nav.vtec.cosC)); + memset(rtcm->nav.vtec.sinC, 0, sizeof(rtcm->nav.vtec.sinC)); + /* cosine coefficients: for order o=0..mmax, degree n=o..nmax */ + for (k=0;k<=mmax;k++) { + for (l=k;l<=nmax;l++) { + cosC=getbits(rtcm->buff,i,16); i+=16; + rtcm->nav.vtec.cosC[j][l][k]=cosC/200.0; + } + } + /* sine coefficients: for order o=1..mmax, degree n=o..nmax */ + for (k=1;k<=mmax;k++) { + for (l=k;l<=nmax;l++) { + sinC=getbits(rtcm->buff,i,16); i+=16; + rtcm->nav.vtec.sinC[j][l][k]=sinC/200.0; + } + } + } + rtcm->nav.vtec.udint=udint; + rtcm->nav.vtec.iod=iod; + rtcm->nav.vtec.nlay=nlay; + rtcm->nav.vtec.qi=qi*0.05; /* TECU */ + + trace(3,"ssr8 vtec: nlay=%d\n",nlay); + return 10; +} /* get signal index ----------------------------------------------------------*/ static void sigindex(int sys, const uint8_t *code, int n, const char *opt, int *idx) @@ -2797,6 +2862,7 @@ extern int decode_rtcm3(rtcm_t *rtcm) case 1261: ret=decode_ssr4(rtcm,SYS_CMP,0); break; /* draft */ case 1262: ret=decode_ssr5(rtcm,SYS_CMP,0); break; /* draft */ case 1263: ret=decode_ssr6(rtcm,SYS_CMP,0); break; /* draft */ + case 1264: ret=decode_ssr8(rtcm,0); break; /* draft */ case 1265: ret=decode_ssr7(rtcm,SYS_GPS,0); break; /* draft */ case 1266: ret=decode_ssr7(rtcm,SYS_GLO,0); break; /* draft */ case 1267: ret=decode_ssr7(rtcm,SYS_GAL,0); break; /* draft */ diff --git a/src/rtklib.h b/src/rtklib.h index d54a362ca..b11da21c4 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -753,6 +753,18 @@ typedef struct { /* TEC grid type */ float *rms; /* RMS values (tecu) */ } tec_t; +typedef struct { + double udint; + double qi; + int iod; + int nlay; + int nmax[4]; + int mmax[4]; + double hgt[4]; + double cosC[4][16][16]; + double sinC[4][16][16]; +} vtec_t; + typedef struct { /* SBAS message type */ int week,tow; /* reception time */ uint8_t prn,rcv; /* SBAS satellite PRN,receiver number */ @@ -871,6 +883,7 @@ typedef struct { /* navigation data type */ double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ int glo_fcn[32]; /* GLONASS FCN + 8 */ double cbias[MAXSAT][NFREQ][MAX_CODE_BIASES]; /* satellite code biases] (m) */ + vtec_t vtec; /* ionosphere VTEC coefficients */ pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ sbssat_t sbssat; /* SBAS satellite corrections */ sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */ @@ -1221,8 +1234,9 @@ typedef struct { /* RTK control/result type */ prcopt_t opt; /* processing options */ int initial_mode; /* initial positioning mode */ int epoch; /* epoch number */ - int intpres_nb; // Time interpolation of residuals, number of previous base observations. - obsd_t intpres_obsb[MAXOBS]; // Time interpolation of residuals, previous base observations. + int intpres_nb; /* Time interpolation of residuals, number of previous base observations */ + int vtec_used; /* indicates VTEC coeffs have been used to init ion states */ + obsd_t intpres_obsb[MAXOBS]; /* Time interpolation of residuals, previous base observations */ } rtk_t; typedef struct { /* receiver raw data control type */ @@ -1570,6 +1584,8 @@ EXPORT int iontec(gtime_t time, const nav_t *nav, const double *pos, EXPORT void readtec(const char *file, nav_t *nav, int opt); EXPORT int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, const double *azel, int ionoopt, double *ion, double *var); +EXPORT int ionvtec(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, double freq, double *delay, double *var); EXPORT int tropcorr(gtime_t time, const nav_t *nav, const double *pos, const double *azel, int tropopt, double *trp, double *var); EXPORT int seliflc(int optnf, int sys); diff --git a/src/rtksvr.c b/src/rtksvr.c index 9e672f01f..6eca2d5ac 100644 --- a/src/rtksvr.c +++ b/src/rtksvr.c @@ -323,6 +323,9 @@ static void update_ssr(rtksvr_t *svr, int index) svr->rtcm[index].ssr[i].update = 0; } svr->nmsg[index][7]++; + + /* update vtec */ + svr->nav.vtec=svr->rtcm[index].nav.vtec; } /* update rtk server struct --------------------------------------------------*/ static void update_svr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, @@ -993,6 +996,8 @@ extern int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs, svr->rtcm[i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time; } } + svr->rtk.vtec_used=0; + /* sync input streams */ strsync(svr->stream,svr->stream+1); strsync(svr->stream,svr->stream+2);