From db9612c8dc5ec4d51558b24eb404715ba782277e Mon Sep 17 00:00:00 2001 From: oaq Date: Fri, 22 Nov 2024 01:39:37 +1100 Subject: [PATCH] rtkpos: rework the rolling exclusion of satellites The heuristics in manage_amb_LAMBDA() select a rolling satellite to test for exclusion, to see if it improves the ambiguity ratio. This was intending to note the sats used for AR, but was also iterating over the frequencies and noting sat/freq used for AR, but the array to hold the sat mapping was only large enough for the sats, not sats*freqs, leading to OOB access when there were many observations and frequencies. It was rolling over this combination, not as intended. Further it was sensitive to changes in the set of sats used for AR, the rolling point could jump around, jump back, and not progress smoothly thought the set of sats. Rework to address these issues, to roll through that sats smoothly. --- src/rtkpos.c | 67 ++++++++++++++++++++++++++++++++-------------------- 1 file changed, 42 insertions(+), 25 deletions(-) diff --git a/src/rtkpos.c b/src/rtkpos.c index 7ae6dc72d..e9d3ec821 100644 --- a/src/rtkpos.c +++ b/src/rtkpos.c @@ -1777,12 +1777,11 @@ static int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa,int gps,int glo,in /* resolve integer ambiguity by LAMBDA using partial fix techniques and multiple attempts -----------------------*/ static int manage_amb_LAMBDA(rtk_t *rtk, double *bias, double *xa, const int *sat, int nf, int ns) { - int i,f,lockc[NFREQ],ar=0,excflag=0,arsats[MAXOBS]={0}; int gps1=-1,glo1=-1,sbas1=-1,gps2,glo2,sbas2,nb,rerun,dly; float ratio1,posvar=0; /* calc position variance, will skip AR if too high to avoid false fix */ - for (i=0;i<3;i++) posvar+=rtk->P[i+i*rtk->nx]; + for (int i=0;i<3;i++) posvar+=rtk->P[i+i*rtk->nx]; posvar/=3.0; /* maintain compatibility with previous code */ trace(3,"posvar=%.6f\n",posvar); @@ -1798,23 +1797,41 @@ static int manage_amb_LAMBDA(rtk_t *rtk, double *bias, double *xa, const int *sa rtk->nb_ar=0; return 0; } - /* if no fix on previous sample and enough sats, exclude next sat in list */ - if (rtk->sol.prev_ratio2sol.thres&&rtk->nb_ar>=rtk->opt.mindropsats) { - /* find and count sats used last time for AR */ - for (f=0;fssat[sat[i]-1].vsat[f] && rtk->ssat[sat[i]-1].lock[f]>=0 && rtk->ssat[sat[i]-1].azel[1]>=rtk->opt.elmin) { - arsats[ar++]=i; - } - if (rtk->excsatexcsat]]; - for (f=0;fssat[i-1].lock[f]; /* save lock count */ - /* remove sat from AR long enough to enable hold if stays fixed */ - rtk->ssat[i-1].lock[f]=-rtk->nb_ar; - } - trace(3,"AR: exclude sat %d\n",i); - excflag=1; - } else rtk->excsat=0; /* exclude none and reset to beginning of list */ + // If no fix on previous sample and enough sats, exclude next sat in list. + int lockc[NFREQ], excsat = 0; + if (rtk->sol.prev_ratio2 < rtk->sol.thres && rtk->nb_ar >= rtk->opt.mindropsats) { + // Find the position of the last excluded sat. + int i = 0; + if (rtk->excsat != 0) { + for (; i < ns; i++) { + if (rtk->excsat == sat[i]) { + i++; + break; + } + } + // If not found then restart from the first sat. + if (i >= ns) i = 0; + } + // Find the next sat used last time for AR. + for (; i < ns; i++) { + for (int f = 0; f < nf; f++) { + if (rtk->ssat[sat[i] - 1].vsat[f] && rtk->ssat[sat[i] - 1].lock[f] >= 0 && + rtk->ssat[sat[i] - 1].azel[1] >= rtk->opt.elmin) { + excsat = sat[i]; + break; + } + } + if (excsat) break; + } + if (excsat) { + for (int f = 0; f < nf; f++) { + lockc[f] = rtk->ssat[excsat - 1].lock[f]; // Save lock count. + // Remove sat from AR long enough to enable hold if stays fixed. + rtk->ssat[excsat - 1].lock[f] = -rtk->nb_ar; + } + trace(3, "AR: exclude sat %d\n", excsat); + } + rtk->excsat = excsat; } /* for inital ambiguity resolution attempt, include all enabled sats */ @@ -1832,7 +1849,7 @@ static int manage_amb_LAMBDA(rtk_t *rtk, double *bias, double *xa, const int *sa (rtk->sol.ratioopt.thresar[0]*1.1 && rtk->sol.ratiosol.prev_ratio1/2.0))) { trace(3,"low ratio: check for new sat\n"); dly=2; - for (i=0;issat[sat[i]-1].fix[f]!=2) continue; /* check for new sats */ if (rtk->ssat[sat[i]-1].lock[f]==0) { @@ -1863,11 +1880,11 @@ static int manage_amb_LAMBDA(rtk_t *rtk, double *bias, double *xa, const int *sa if (glo1!=glo2||gps1!=gps2) nb=resamb_LAMBDA(rtk,bias,xa,gps2,glo2,sbas2); } - /* restore excluded sat if still no fix or significant increase in ar ratio */ - if (excflag && (rtk->sol.ratiosol.thres) && (rtk->sol.ratio<(1.5*rtk->sol.prev_ratio2))) { - i=sat[arsats[rtk->excsat++]]; - for (f=0;fssat[i-1].lock[f]=lockc[f]; - trace(3,"AR: restore sat %d\n",i); + /* Restore excluded sat if still no fix or significant increase in ar ratio */ + if (excsat && (rtk->sol.ratio < rtk->sol.thres) && + (rtk->sol.ratio < (1.5 * rtk->sol.prev_ratio2))) { + for (int f = 0; f < nf; f++) rtk->ssat[excsat - 1].lock[f] = lockc[f]; + trace(3, "AR: restore sat %d\n", excsat); } rtk->sol.prev_ratio1=ratio1>0?ratio1:rtk->sol.ratio;