mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-14 03:31:50 +00:00
Code Issues Releases Wiki Activity

Improve carrier phase integrity in the RTK engine

Browse Source
This commit is contained in:
Carles Fernandez
2026-08-15 21:22:41 +02:00
parent f5337b8fc9
commit 87d8a6991e
20 changed files with 748 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/doxygen/other/main_page.dox
+2 -2
View File
@@ -585,11 +585,11 @@ PVT.ntrip_station_id=0
PVT.ntrip_fallback_to_single=true
\endverbatim
The caster address, mountpoint, username, password, and password-environment variable name default to empty strings; the remaining defaults are those shown above, except that <code>PVT.ntrip_client_enabled</code> defaults to <code>false</code>. Set either <code>PVT.ntrip_password</code> or <code>PVT.ntrip_password_env</code>, never both. The latter names an environment variable from which GNSS-SDR reads the password and avoids placing the password in the configuration file. Station ID <code>0</code> accepts any station; otherwise, set the expected RTCM station ID. Setting <code>PVT.ntrip_reconnect_interval_ms=0</code> disables automatic reconnect attempts.
The caster address, mountpoint, username, password, and password-environment variable name default to empty strings; the remaining defaults are those shown above, except that <code>PVT.ntrip_client_enabled</code> defaults to <code>false</code>. Set either <code>PVT.ntrip_password</code> or <code>PVT.ntrip_password_env</code>, never both. The latter names an environment variable from which GNSS-SDR reads the password and avoids placing the password in the configuration file. Station ID <code>0</code> accepts any station; otherwise, set the expected RTCM station ID. Setting <code>PVT.ntrip_reconnect_interval_ms=0</code> disables operational reconnect attempts; the one immediate v2-to-v1 compatibility retry still runs and is not counted as an automatic reconnect.
The correction stream must provide an RTCM 3 base position message 1005 or 1006 and GPS observation messages carrying both L1 and L2, either as message 1004 or as MSM4 through MSM7 (1074 through 1077). Corrections older than <code>PVT.ntrip_max_correction_age_s</code> are not used. Enabling this client forces a separate internal single-point solver for receiver-clock correction, independently of the RTK solver. With <code>PVT.ntrip_fallback_to_single=true</code>, the receiver reports an explicitly labeled <code>SOLQ_SINGLE</code> solution while corrections are unavailable, incomplete, or stale; it does not label that fallback as an RTK solution.
This client negotiates NTRIP version 2 over HTTP/1.1 by default, and falls back automatically to NTRIP version 1 when the caster does not answer with an HTTP status line; <code>PVT.ntrip_version=1</code> forces the legacy v1 protocol. Setting <code>PVT.ntrip_tls_enabled=true</code> encrypts the connection with TLS, validating the caster certificate against the system CA store and its host name. Without TLS, Basic authentication only encodes credentials; it does not encrypt them, so use a trusted network or a suitable secure tunnel, and prefer <code>PVT.ntrip_password_env</code> over a password stored in a file. VRS mountpoints requiring rover GGA messages are not supported.
This client sends an NTRIP version 2 HTTP/1.1 request by default and accepts a legacy v1 reply on that connection. If the fully-sent v2 exchange closes or times out before receiving any response bytes, or returns HTTP 400, 501, or 505, it opens one fresh connection and retries using the v1 HTTP/1.0 request. Once selected, v1 is retained for operational reconnects until the client is stopped and started again. DNS, TCP, TLS, and partial-write failures before the request is sent do not trigger a downgrade; <code>PVT.ntrip_version=1</code> forces v1 from the first connection. Setting <code>PVT.ntrip_tls_enabled=true</code> encrypts both attempts with TLS 1.2 or newer, validating the caster certificate against the system CA store and its host name. Without TLS, Basic authentication only encodes credentials; it does not encrypt them, so use a trusted network or a suitable secure tunnel, and prefer <code>PVT.ntrip_password_env</code> over a password stored in a file. VRS mountpoints requiring rover GGA messages are not supported.
<code>PVT.flag_rtcm_server</code> is unrelated to the NTRIP correction client. It controls an outbound raw TCP server for RTCM messages generated by GNSS-SDR; it neither connects to a caster nor supplies corrections to the RTK solver. The outbound server and inbound NTRIP client can be enabled independently.
docs/protobuf/gnss_synchro.proto
+9
View File
@@ -38,6 +38,15 @@ message GnssSynchro {
double interp_tow_ms = 25; // Interpolated time of week, in ms
bool flag_PLL_180_deg_phase_locked = 26; // PLL lock at 180º
bool flag_cycle_slip = 27; // Carrier cycle slip detection flag
// Carrier phase ambiguity change, reported by the tracking block. Sent
// negated with respect to Gnss_Synchro::Flag_carrier_phase_continuous so that
// the protobuf default for a missing field (false) means "no discontinuity"
bool flag_carrier_phase_discontinuity = 28;
// Half-cycle jump of the carrier phase, caused by a change in the Costas loop
// phase ambiguity resolved by the Telemetry Decoder
bool flag_half_cycle_slip = 29;
}
/* Observables represents a collection of GnssSynchro annotations */
src/algorithms/PVT/libs/rinex_printer.cc
+6 -1
View File
@@ -1835,6 +1835,11 @@ void add_obs_sat_record_line(const Gnss_Synchro& synchro, std::string& line, boo
{
const int32_t ssi = signal_strength(synchro.CN0_dB_hz);
const char lli = synchro.Flag_cycle_slip ? '1' : ' ';
// bit 0: loss of lock or cycle slip, bit 1: half-cycle ambiguity change.
// The half-cycle bit only makes sense for the carrier phase observable
const int32_t phase_lli_bits = (synchro.Flag_cycle_slip ? 1 : 0) +
(synchro.Flag_half_cycle_slip ? 2 : 0);
const char phase_lli = phase_lli_bits != 0 ? static_cast<char>('0' + phase_lli_bits) : ' ';
// PSEUDORANGE
line += rightJustify(asString(synchro.Pseudorange_m, 3), 14);
@@ -1843,7 +1848,7 @@ void add_obs_sat_record_line(const Gnss_Synchro& synchro, std::string& line, boo
// PHASE
line += rightJustify(asString(synchro.Carrier_phase_rads / TWO_PI, 3), 14);
line += std::string(1, lli); // Loss of lock indicator (LLI)
line += std::string(1, phase_lli); // Loss of lock indicator (LLI)
line += rightJustify(asString<int32_t>(ssi), 1); // Signal Strength Indicator (SSI)
// DOPPLER
src/algorithms/libs/rtklib/rtklib_conversions.cc
+3 -1
View File
@@ -170,7 +170,9 @@ obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs,
rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz;
rtklib_obs.P[band] = gnss_synchro.Pseudorange_m;
rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / TWO_PI;
rtklib_obs.LLI[band] = gnss_synchro.Flag_cycle_slip ? 1U : 0U;
// bit 0: loss of lock or cycle slip, bit 1: half-cycle ambiguity change
rtklib_obs.LLI[band] = static_cast<unsigned char>((gnss_synchro.Flag_cycle_slip ? 1U : 0U) |
(gnss_synchro.Flag_half_cycle_slip ? 2U : 0U));
switch (band)
{
src/algorithms/libs/rtklib/rtklib_rtcm3.cc
+5 -1
View File
@@ -3222,8 +3222,12 @@ void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r,
{
rtcm->obs.data[index].D[ind[k]] = static_cast<float>(-(rr[i] + rrf[j]) / wl);
}
/* bit 0: loss of lock, bit 1: half-cycle ambiguity unresolved.
Adding 2 (not 3) keeps both flags in their own bit: a set
half-cycle indicator must not be reported as a cycle slip,
and a real loss of lock must not overflow into bit 2 */
rtcm->obs.data[index].LLI[ind[k]] =
lossoflock(rtcm, sat, ind[k], lock[j]) + (half[j] ? 3 : 0);
lossoflock(rtcm, sat, ind[k], lock[j]) + (half[j] ? 2 : 0);
rtcm->obs.data[index].SNR[ind[k]] = static_cast<unsigned char>(cnr[j] * 4.0);
rtcm->obs.data[index].code[ind[k]] = code[k];
}
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
+85 -16
View File
@@ -49,6 +49,18 @@ static const double AR_POLY_COEFFS[3][5] = {
{6.42237302e-01, -8.39813962e+00, 2.92107285e+01, -2.37577308e+01, -1.14307128e+00},
{-2.22600390e-02, 3.23169103e-01, -1.39837429e+00, 2.19282996e+00, -5.34583971e-02}};
/* number of consecutive innovation rejections after which a phase bias is
considered stale and reinitialized (demo5) */
const unsigned int MAX_REJECTIONS_BEFORE_RESET = 2;
/* factor applied to the innovation threshold when one of the two phase biases
of a double difference has just been initialized (demo5) */
const double INNOVATION_THRESHOLD_INIT_FACTOR = 10.0;
/* variance added to a carrier phase measurement whose half-cycle ambiguity is
not resolved (m^2), which brings its weight down to decimeter level (demo5) */
const double HALF_CYCLE_VARIANCE_M2 = 0.01;
static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)),
const double *azel __attribute((unused))) { return 0; }
@@ -734,8 +746,10 @@ void udpos(rtk_t *rtk, double tt)
}
return;
}
/* initialize position for first epoch */
if (norm_rtk(rtk->x, 3) <= 0.0)
/* initialize position for the first epoch, or whenever the state does not
hold a plausible position on the Earth, which would otherwise keep the
filter from converging (demo5) */
if (norm_rtk(rtk->x, 3) <= RE_WGS84 / 2.0)
{
for (i = 0; i < 3; i++)
{
@@ -802,15 +816,30 @@ void udpos(rtk_t *rtk, double tt)
{
F[i + (i + 3) * rtk->nx] = tt;
}
/* let the acceleration states drive the position only once the filter has
converged: while the position variance is still large the acceleration
estimates are meaningless and would inject noise into the position
(demo5, which reuses its AR position-variance gate for this) */
if (rtk->opt.armaxposvar <= 0.0 || var < rtk->opt.armaxposvar)
{
for (i = 0; i < 3; i++)
{
F[i + (i + 6) * rtk->nx] = (tt >= 0.0 ? 1.0 : -1.0) * tt * tt / 2.0;
}
}
else
{
trace(3, "udpos : position variance too high for the acceleration term: %.4f\n", var);
}
/* x=F*x, P=F*P*F+Q */
matmul("NN", rtk->nx, 1, rtk->nx, 1.0, F, rtk->x, 0.0, xp);
matcpy(rtk->x, xp, rtk->nx, 1);
matmul("NN", rtk->nx, rtk->nx, rtk->nx, 1.0, F, rtk->P, 0.0, FP);
matmul("NT", rtk->nx, rtk->nx, rtk->nx, 1.0, FP, F, 0.0, rtk->P);
/* process noise added to only acceleration */
Q[0] = Q[4] = std::pow(rtk->opt.prn[3], 2.0);
Q[8] = std::pow(rtk->opt.prn[4], 2.0);
/* process noise added to only acceleration, scaled by the elapsed time */
Q[0] = Q[4] = std::pow(rtk->opt.prn[3], 2.0) * fabs(tt);
Q[8] = std::pow(rtk->opt.prn[4], 2.0) * fabs(tt);
ecef2pos(rtk->x, pos);
covecef(pos, Q, Qv);
for (i = 0; i < 3; i++)
@@ -858,7 +887,7 @@ void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns)
/* elevation dependent factor of process noise */
el = rtk->ssat[sat[i] - 1].azel[1];
fact = cos(el);
rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[1] * bl / 1e4 * fact, 2.0) * tt;
rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[1] * bl / 1e4 * fact, 2.0) * fabs(tt);
}
}
}
@@ -891,7 +920,7 @@ void udtrop(rtk_t *rtk, double tt, double bl __attribute((unused)))
}
else
{
rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[2], 2.0) * tt;
rtk->P[j + j * rtk->nx] += std::pow(rtk->opt.prn[2], 2.0) * fabs(tt);
if (rtk->opt.tropopt >= TROPOPT_ESTG)
{
@@ -928,7 +957,7 @@ void udrcvbias(rtk_t *rtk, double tt)
}
else
{
rtk->P[j + j * rtk->nx] += std::pow(PRN_HWBIAS, 2.0) * tt;
rtk->P[j + j * rtk->nx] += std::pow(PRN_HWBIAS, 2.0) * fabs(tt);
}
}
}
@@ -1183,6 +1212,7 @@ void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
int j;
int f;
int slip;
unsigned int rejc;
int reset;
int nf = NF_RTK(&rtk->opt);
@@ -1243,21 +1273,26 @@ void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
rtk->ssat[i - 1].lock[f] = -rtk->opt.minlock;
}
}
/* reset phase-bias if detecting cycle slip */
/* reset phase-bias if detecting cycle slip or repeated outliers */
for (i = 0; i < ns; i++)
{
j = IB_RTK(sat[i], f, &rtk->opt);
rtk->P[j + j * rtk->nx] += rtk->opt.prn[0] * rtk->opt.prn[0] * tt;
rtk->P[j + j * rtk->nx] += rtk->opt.prn[0] * rtk->opt.prn[0] * fabs(tt);
slip = rtk->ssat[sat[i] - 1].slip[f];
rejc = rtk->ssat[sat[i] - 1].rejc[f];
if (rtk->opt.ionoopt == IONOOPT_IFLC)
{
slip |= rtk->ssat[sat[i] - 1].slip[1];
}
if (rtk->opt.modear == ARMODE_INST || !(slip & 1))
/* a bias that keeps producing outliers is stale: reinitialize it
instead of rejecting its measurements forever (demo5) */
if (rtk->opt.modear == ARMODE_INST ||
(!(slip & 1) && rejc < MAX_REJECTIONS_BEFORE_RESET))
{
continue;
}
rtk->x[j] = 0.0;
rtk->ssat[sat[i] - 1].rejc[f] = 0;
rtk->ssat[sat[i] - 1].lock[f] = -rtk->opt.minlock;
}
bias = zeros(ns, 1);
@@ -1720,6 +1755,7 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
double fi;
double fj;
double df;
double threshadj;
double *Hi = nullptr;
int i;
int j;
@@ -1958,8 +1994,23 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
rtk->ssat[sat[j] - 1].resp[f - nf] = v[nv];
}
/* open up the innovation threshold if one of the two phase
biases has just been initialized, otherwise the freshly
reset bias is rejected again and never recovers (demo5) */
threshadj = 1.0;
if (P != nullptr && opt->mode > PMODE_DGPS)
{
const int ii = IB_RTK(sat[i], frq, opt);
const int jj = IB_RTK(sat[j], frq, opt);
if (P[ii + rtk->nx * ii] == std::pow(opt->std[0], 2.0) ||
P[jj + rtk->nx * jj] == std::pow(opt->std[0], 2.0))
{
threshadj = INNOVATION_THRESHOLD_INIT_FACTOR;
}
}
/* test innovation, with separate thresholds for phase and code */
if (opt->maxinno[f < nf ? 0 : 1] > 0.0 && fabs(v[nv]) > opt->maxinno[f < nf ? 0 : 1])
if (opt->maxinno[f < nf ? 0 : 1] > 0.0 &&
fabs(v[nv]) > opt->maxinno[f < nf ? 0 : 1] * threshadj)
{
if (f < nf)
{
@@ -1978,6 +2029,22 @@ int ddres(rtk_t *rtk, const obsd_t *obs, const nav_t *nav, double dt, const doub
rtk->ssat[sat[j] - 1].snr_rover[frq], rtk->ssat[sat[j] - 1].snr_base[frq],
bl, dt, f, opt, &obs[iu[j]]);
/* a carrier phase with an unresolved half-cycle ambiguity is
only worth its half wavelength, so deweight it instead of
feeding it to the filter with full carrier phase accuracy
(demo5, extended here to the base observations, which carry
the flag too) */
if (f < nf)
{
if ((obs[iu[i]].LLI[frq] & 2) || (obs[ir[i]].LLI[frq] & 2))
{
Ri[nv] += HALF_CYCLE_VARIANCE_M2;
}
if ((obs[iu[j]].LLI[frq] & 2) || (obs[ir[j]].LLI[frq] & 2))
{
Rj[nv] += HALF_CYCLE_VARIANCE_M2;
}
}
/* set valid data flags */
if (opt->mode > PMODE_DGPS)
{
@@ -2984,10 +3051,11 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
}
}
}
/* lack of valid satellites */
/* too few valid phase measurements: fall back to the code-differenced
solution instead of dropping the epoch altogether (demo5) */
if (rtk->sol.ns < 4)
{
stat = SOLQ_NONE;
stat = SOLQ_DGPS;
}
}
else
@@ -3011,8 +3079,9 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
stat = SOLQ_FIX;
}
}
/* resolve integer ambiguity by LAMBDA */
else if (stat != SOLQ_NONE && manage_amb_LAMBDA(rtk, bias, xa, sat.data(), nf, ns) > 1)
/* resolve integer ambiguity by LAMBDA (only from a valid float solution: a
code-differenced epoch has no phase biases to fix) */
else if (stat == SOLQ_FLOAT && manage_amb_LAMBDA(rtk, bias, xa, sat.data(), nf, ns) > 1)
{
if (zdres(0, obs, nu, rs, dts, svh.data(), nav, xa, opt, 0, y, e, azel))
{
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
+134 -13
View File
@@ -105,7 +105,11 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_)
d_channel_last_pll_lock = std::vector<bool>(d_nchannels_out, false);
d_channel_last_pseudorange_smooth = std::vector<double>(d_nchannels_out, 0.0);
d_channel_last_carrier_phase_rads = std::vector<double>(d_nchannels_out, 0.0);
d_channel_last_rx_time_valid = std::vector<bool>(d_nchannels_out, false);
d_channel_has_previous_observation = std::vector<bool>(d_nchannels_out, false);
d_channel_phase_discontinuity_pending = std::vector<bool>(d_nchannels_out, false);
d_channel_last_pll_180_locked = std::vector<bool>(d_nchannels_out, false);
d_channel_last_valid_epoch = std::vector<uint64_t>(d_nchannels_out, 0);
d_channel_last_valid_prn = std::vector<uint32_t>(d_nchannels_out, 0);
d_last_trk_data = std::vector<Gnss_Synchro>(d_nchannels_out);
d_SourceTagTimestamps = std::vector<std::queue<GnssTime>>(d_nchannels_out);
@@ -249,7 +253,9 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
{
d_gnss_synchro_history->clear(n);
}
std::fill(d_channel_last_rx_time_valid.begin(), d_channel_last_rx_time_valid.end(), false);
// the carrier phase continuity bookkeeping is deliberately kept:
// realigning the receiver clock discards the interpolation history
// but leaves the accumulated carrier phase of the channels untouched
LOG(INFO) << "Corrected new RX Time offset: " << static_cast<int>(round(new_rx_clock_offset_s * 1000.0)) << "[ms]";
}
@@ -266,7 +272,6 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
{
d_gnss_synchro_history->clear(n);
}
std::fill(d_channel_last_rx_time_valid.begin(), d_channel_last_rx_time_valid.end(), false);
LOG(INFO) << "Received reset observables TOW command from PVT";
break;
default:
@@ -637,9 +642,11 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
wavelength_m = SPEED_OF_LIGHT_M_S / it_freq_map->second;
}
// todo: propagate the PLL lock status in Gnss_Synchro
// 1. check if last PLL lock status was false and initialize last d_channel_last_pseudorange_smooth
if (d_channel_last_pll_lock[it->Channel_ID] == true)
// 1. restart the filter unless the channel was already producing
// observations and its carrier phase is continuous with them:
// a phase discontinuity would otherwise propagate into the
// smoothed pseudorange through the carrier phase term below
if (d_channel_last_pll_lock[it->Channel_ID] == true && !it->Flag_cycle_slip)
{
// 2. Compute the smoothed pseudorange for this channel
// Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/)
@@ -659,6 +666,45 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
}
bool hybrid_observables_gs::phase_stream_is_discontinuous(bool has_previous_observation,
uint64_t last_valid_epoch,
uint64_t current_epoch,
uint32_t last_valid_prn,
uint32_t current_prn,
double epoch_interval_s)
{
// A satellite seen for the first time on this channel always comes with a
// new ambiguity
if (!has_previous_observation || last_valid_prn != current_prn)
{
return true;
}
// Repeated or out-of-order epochs are not expected: assume the worst
if (current_epoch <= last_valid_epoch)
{
return true;
}
const double gap_s = static_cast<double>(current_epoch - last_valid_epoch) * epoch_interval_s;
return gap_s > MIN_REACQUISITION_GAP_S;
}
bool hybrid_observables_gs::half_cycle_ambiguity_changed(bool has_previous_observation,
bool carrier_phase_discontinuous,
uint32_t last_valid_prn,
uint32_t current_prn,
bool last_pll_180_locked,
bool current_pll_180_locked)
{
// nothing to compare against, or the whole ambiguity is new anyway
if (!has_previous_observation || carrier_phase_discontinuous || last_valid_prn != current_prn)
{
return false;
}
return last_pll_180_locked != current_pll_180_locked;
}
void hybrid_observables_gs::detect_cycle_slips(std::vector<Gnss_Synchro> &data, uint64_t rx_clock)
{
constexpr double kCycleSlipThresholdCycles = 0.5;
@@ -671,12 +717,73 @@ void hybrid_observables_gs::detect_cycle_slips(std::vector<Gnss_Synchro> &data,
for (auto &obs : data)
{
obs.Flag_cycle_slip = false;
obs.Flag_half_cycle_slip = false;
}
for (uint32_t n = 0; n < d_nchannels_out; n++)
{
auto &obs = data[n];
if (!obs.Flag_valid_pseudorange || obs.fs == 0LL)
if (!obs.Flag_valid_pseudorange)
{
continue;
}
// A satellite whose observations resume after a loss of lock comes with
// a brand-new carrier phase ambiguity, since the accumulated carrier
// phase of the tracking channel restarts on every (re)acquisition.
// Report the discontinuity and keep the observation out of the
// common-mode estimate: its phase difference is meaningless and would
// bias the median.
const uint64_t previous_epoch = d_channel_last_valid_epoch[n];
// the tracking block reports a restarted phase accumulator on a single
// sample, which interpolation may skip, so the report is latched as it
// enters the block and consumed here; the gap heuristic below covers
// the tracking blocks that do not report it
const bool reported_discontinuity = d_channel_phase_discontinuity_pending[n];
d_channel_phase_discontinuity_pending[n] = false;
const bool discontinuous = reported_discontinuity ||
phase_stream_is_discontinuous(
d_channel_has_previous_observation[n],
d_channel_last_valid_epoch[n],
d_epoch_counter,
d_channel_last_valid_prn[n],
obs.PRN,
d_T_rx_step_s);
const bool half_cycle_step = half_cycle_ambiguity_changed(
d_channel_has_previous_observation[n],
discontinuous,
d_channel_last_valid_prn[n],
obs.PRN,
d_channel_last_pll_180_locked[n],
obs.Flag_PLL_180_deg_phase_locked);
d_channel_last_pll_180_locked[n] = obs.Flag_PLL_180_deg_phase_locked;
d_channel_has_previous_observation[n] = true;
d_channel_last_valid_epoch[n] = d_epoch_counter;
d_channel_last_valid_prn[n] = obs.PRN;
if (half_cycle_step)
{
obs.Flag_half_cycle_slip = true;
LOG(INFO) << "Half-cycle slip on channel " << n
<< " at RX time " << obs.RX_time
<< " s, for satellite " << obs.System << obs.PRN
<< ", signal " << std::string(obs.Signal, 2)
<< " (PLL 180 deg lock is now " << std::boolalpha
<< obs.Flag_PLL_180_deg_phase_locked << ')';
}
if (discontinuous)
{
obs.Flag_cycle_slip = true;
LOG(INFO) << "Carrier phase discontinuity on channel " << n
<< " after " << static_cast<double>(d_epoch_counter - previous_epoch) * d_T_rx_step_s
<< " s without observations, at RX time " << obs.RX_time
<< " s, for satellite " << obs.System << obs.PRN
<< ", signal " << std::string(obs.Signal, 2);
continue;
}
if (obs.fs == 0LL)
{
continue;
}
@@ -783,6 +890,7 @@ void hybrid_observables_gs::set_tag_timestamp_in_sdr_timeframe(const std::vector
}
}
void hybrid_observables_gs::propagate_sensor_data(const std::vector<Gnss_Synchro> &data)
{
static pmt::pmt_t SAMPLE_STAMP_KEY = pmt::mp(SensorIdentifier::to_string(SensorIdentifier::SAMPLE_STAMP));
@@ -908,6 +1016,13 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
// valid time reference can be reported through the Monitor
d_last_trk_data[n] = in[n][m];
}
// Latch a restarted carrier phase accumulator reported by the
// tracking block: it is flagged on a single sample, which may
// not be the one the observation is interpolated from
if (!in[n][m].Flag_carrier_phase_continuous)
{
d_channel_phase_discontinuity_pending[n] = true;
}
// Push the valid tracking Gnss_Synchros to their corresponding deque
if (in[n][m].Flag_valid_word)
{
@@ -924,7 +1039,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
if (d_gnss_synchro_history->front(n).PRN != in[n][m].PRN)
{
d_gnss_synchro_history->clear(n);
d_channel_last_rx_time_valid[n] = false;
d_channel_has_previous_observation[n] = false;
// LOG(INFO) << "Channel " << d_gnss_synchro_history->front(n).Channel_ID << " changed satellite to PRN " << in[n][m].PRN;
}
}
@@ -967,6 +1082,10 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
}
epoch_data[n] = std::move(interpolated_gnss_synchro);
}
// counts every produced epoch, including those with no valid
// observation at all, so that gaps in a channel are always visible
d_epoch_counter++;
if (d_T_rx_TOW_set)
{
update_TOW(epoch_data);
@@ -986,17 +1105,19 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
propagate_sensor_data(epoch_data);
}
// Cycle slip detection runs first: the carrier smoothing filter needs
// to know whether the carrier phase is continuous before using it
if (n_valid > 0)
{
detect_cycle_slips(epoch_data, d_Rx_clock_buffer.front());
}
// Carrier smoothing (optional)
if (d_conf.enable_carrier_smoothing == true)
{
smooth_pseudoranges(epoch_data);
}
if (n_valid > 0)
{
detect_cycle_slips(epoch_data, d_Rx_clock_buffer.front());
}
// output the observables set to the PVT block
for (uint32_t n = 0; n < d_nchannels_out; n++)
{
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
+67 -1
View File
@@ -63,6 +63,66 @@ public:
int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star& input_items, gr_vector_void_star& output_items);
/*!
* \brief Shortest interruption of the observation stream of a channel that
* can be attributed to a loss of lock, in seconds.
*
* Reacquiring a satellite and decoding its telemetry again takes seconds,
* so shorter interruptions are transient failures to interpolate the
* tracking history and leave the carrier phase untouched.
*/
static constexpr double MIN_REACQUISITION_GAP_S = 1.0;
/*!
* \brief Tells whether the carrier phase of a channel can be assumed to be
* continuous with respect to its previous observation.
*
* The accumulated carrier phase of a tracking channel restarts from scratch
* whenever the channel (re)acquires a satellite, so an observation that
* resumes after a loss of lock carries a new ambiguity.
*
* \param has_previous_observation Whether the channel produced a valid
* observation since it was last reset
* \param last_valid_epoch Epoch count when the channel last produced a
* valid observation
* \param current_epoch Epoch count of the observation being checked
* \param last_valid_prn PRN of the last valid observation of the channel
* \param current_prn PRN of the observation being checked
* \param epoch_interval_s Time between consecutive observation epochs
* \return true if the carrier phase ambiguity may have changed
*/
static bool phase_stream_is_discontinuous(bool has_previous_observation,
uint64_t last_valid_epoch,
uint64_t current_epoch,
uint32_t last_valid_prn,
uint32_t current_prn,
double epoch_interval_s);
/*!
* \brief Tells whether the carrier phase has stepped by half a cycle because
* the Costas loop phase ambiguity resolved by the Telemetry Decoder changed.
*
* The Telemetry Decoder compensates that ambiguity by adding half a cycle to
* the carrier phase while the PLL is locked at 180 degrees, so a change of
* its determination steps the reported carrier phase.
*
* \param has_previous_observation Whether the channel produced a valid
* observation since it was last reset
* \param carrier_phase_discontinuous Whether the carrier phase ambiguity has
* already changed for another reason, which supersedes this one
* \param last_valid_prn PRN of the last valid observation of the channel
* \param current_prn PRN of the observation being checked
* \param last_pll_180_locked Ambiguity resolved for the previous observation
* \param current_pll_180_locked Ambiguity resolved for this observation
* \return true if the carrier phase stepped by half a cycle
*/
static bool half_cycle_ambiguity_changed(bool has_previous_observation,
bool carrier_phase_discontinuous,
uint32_t last_valid_prn,
uint32_t current_prn,
bool last_pll_180_locked,
bool current_pll_180_locked);
private:
friend hybrid_observables_gs_sptr hybrid_observables_gs_make(const Obs_Conf& conf_);
@@ -101,7 +161,11 @@ private:
std::vector<bool> d_channel_last_pll_lock;
std::vector<double> d_channel_last_pseudorange_smooth;
std::vector<double> d_channel_last_carrier_phase_rads;
std::vector<bool> d_channel_last_rx_time_valid;
std::vector<bool> d_channel_has_previous_observation; // carrier phase continuity bookkeeping
std::vector<bool> d_channel_phase_discontinuity_pending; // reported by the tracking block
std::vector<bool> d_channel_last_pll_180_locked; // Costas loop ambiguity state
std::vector<uint64_t> d_channel_last_valid_epoch;
std::vector<uint32_t> d_channel_last_valid_prn;
std::vector<Gnss_Synchro> d_last_trk_data; // latest tracking data per channel, for the Monitor
std::string d_dump_filename;
@@ -112,6 +176,8 @@ private:
double d_T_rx_step_s;
double d_last_rx_clock_round20ms_error;
uint64_t d_epoch_counter{0};
uint32_t d_T_rx_TOW_ms;
uint32_t d_T_rx_step_ms;
uint32_t d_T_status_report_timer_ms;
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
+11
View File
@@ -1163,6 +1163,7 @@ void dll_pll_veml_tracking::start_tracking()
d_rem_carr_phase_rad = 0.0;
d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_carrier_phase_discontinuity = true; // the carrier phase ambiguity starts again
d_cn0_estimation_counter = 0;
d_carrier_lock_test = 1.0;
d_CN0_SNV_dB_Hz = 0.0;
@@ -1485,6 +1486,7 @@ void dll_pll_veml_tracking::check_carrier_phase_coherent_initialization()
{
d_acc_carrier_phase_rad = -d_rem_carr_phase_rad;
d_acc_carrier_phase_initialized = true;
d_carrier_phase_discontinuity = true; // the carrier phase ambiguity starts again
}
}
@@ -2504,6 +2506,15 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
}
current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked;
// report a restarted carrier phase accumulator on the first valid
// output that carries it, so that downstream blocks know that the
// ambiguity of this channel is not the one they saw before
current_synchro_data.Flag_carrier_phase_continuous = !d_carrier_phase_discontinuity;
if (current_synchro_data.Flag_valid_symbol_output)
{
d_carrier_phase_discontinuity = false;
}
// generate new tag associated with gnss-synchro object
if (d_timetag_waiting == true)
{
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h
+1
View File
@@ -222,6 +222,7 @@ private:
bool d_use_histogram_bit_sync;
bool d_wait_for_bit_edge{false};
bool d_b1c_prelock_output_pending{false};
bool d_carrier_phase_discontinuity{true}; // pending report of a new carrier phase ambiguity
};
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc
+9
View File
@@ -791,6 +791,7 @@ void dll_pll_veml_tracking_fpga::check_carrier_phase_coherent_initialization()
{
d_acc_carrier_phase_rad = -d_rem_carr_phase_rad;
d_acc_carrier_phase_initialized = true;
d_carrier_phase_discontinuity = true; // the carrier phase ambiguity starts again
}
}
@@ -1407,6 +1408,7 @@ void dll_pll_veml_tracking_fpga::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro)
d_rem_carr_phase_rad = 0.0;
d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_carrier_phase_discontinuity = true; // the carrier phase ambiguity starts again
d_cn0_estimation_counter = 0;
d_carrier_lock_test = 1.0;
d_CN0_SNV_dB_Hz = 0.0;
@@ -2032,6 +2034,13 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
current_synchro_data.Tracking_sample_counter = d_sample_counter_next; // d_sample_counter;
current_synchro_data.Flag_valid_symbol_output = !loss_of_lock;
current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked;
// report a restarted carrier phase accumulator on the first valid
// output that carries it
current_synchro_data.Flag_carrier_phase_continuous = !d_carrier_phase_discontinuity;
if (current_synchro_data.Flag_valid_symbol_output)
{
d_carrier_phase_discontinuity = false;
}
*out[0] = std::move(current_synchro_data);
return 1;
}
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h
+1
View File
@@ -238,6 +238,7 @@ private:
bool d_corrected_doppler;
bool d_interchange_iq;
bool d_acc_carrier_phase_initialized;
bool d_carrier_phase_discontinuity{true}; // pending report of a new carrier phase ambiguity
bool d_worker_is_done;
bool d_extended_correlation_in_fpga;
bool d_current_extended_correlation_in_fpga;
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
+6
View File
@@ -832,6 +832,7 @@ void kf_tracking::start_tracking()
d_rem_carr_phase_rad = 0.0;
d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_carrier_phase_discontinuity = true; // the carrier phase ambiguity starts again
d_cn0_estimation_counter = 0;
d_carrier_lock_test = 1.0;
d_CN0_SNV_dB_Hz = 0.0;
@@ -1220,6 +1221,7 @@ void kf_tracking::check_carrier_phase_coherent_initialization()
{
d_acc_carrier_phase_rad = -d_rem_carr_phase_rad;
d_acc_carrier_phase_initialized = true;
d_carrier_phase_discontinuity = true; // the carrier phase ambiguity starts again
}
}
@@ -2019,6 +2021,10 @@ int kf_tracking::general_work(int noutput_items __attribute__((unused)), gr_vect
{
current_synchro_data.fs = static_cast<int64_t>(d_trk_parameters.fs_in);
current_synchro_data.Tracking_sample_counter = d_sample_counter;
// report a restarted carrier phase accumulator on the first valid
// output that carries it
current_synchro_data.Flag_carrier_phase_continuous = !d_carrier_phase_discontinuity;
d_carrier_phase_discontinuity = false;
*out[0] = std::move(current_synchro_data);
return 1;
}
src/algorithms/tracking/gnuradio_blocks/kf_tracking.h
+1
View File
@@ -229,6 +229,7 @@ private:
bool d_dump;
bool d_dump_mat;
bool d_acc_carrier_phase_initialized;
bool d_carrier_phase_discontinuity{true}; // pending report of a new carrier phase ambiguity
bool d_enable_extended_integration;
};
src/core/monitor/serdes_gnss_synchro.h
+4
View File
@@ -114,6 +114,8 @@ public:
obs->set_flag_valid_pseudorange(gs.Flag_valid_pseudorange);
obs->set_flag_pll_180_deg_phase_locked(gs.Flag_PLL_180_deg_phase_locked);
obs->set_flag_cycle_slip(gs.Flag_cycle_slip);
obs->set_flag_carrier_phase_discontinuity(!gs.Flag_carrier_phase_continuous);
obs->set_flag_half_cycle_slip(gs.Flag_half_cycle_slip);
obs->set_interp_tow_ms(gs.interp_TOW_ms);
}
if (!observables.SerializeToString(&data))
@@ -163,6 +165,8 @@ public:
gs.Flag_valid_pseudorange = gs_read.flag_valid_pseudorange();
gs.Flag_PLL_180_deg_phase_locked = gs_read.flag_pll_180_deg_phase_locked();
gs.Flag_cycle_slip = gs_read.flag_cycle_slip();
gs.Flag_carrier_phase_continuous = !gs_read.flag_carrier_phase_discontinuity();
gs.Flag_half_cycle_slip = gs_read.flag_half_cycle_slip();
gs.interp_TOW_ms = gs_read.interp_tow_ms();
vgs.push_back(gs);
src/core/system_parameters/gnss_synchro.h
+18
View File
@@ -81,6 +81,16 @@ public:
bool Flag_PLL_180_deg_phase_locked{}; //!< Set by Telemetry Decoder processing block
bool Flag_cycle_slip{}; //!< Set by Observables processing block
//! Set by Observables processing block. Signals that the carrier phase has
//! jumped by half a cycle because the Costas loop phase ambiguity resolved
//! by the Telemetry Decoder changed
bool Flag_half_cycle_slip{};
//! Set by Tracking processing block. False on the first output after the
//! accumulated carrier phase has been (re)initialized, that is, whenever
//! the carrier phase ambiguity of the channel has changed
bool Flag_carrier_phase_continuous{true};
/// Copy constructor
Gnss_Synchro(const Gnss_Synchro& other) noexcept = default;
@@ -119,6 +129,8 @@ public:
this->Flag_valid_pseudorange = rhs.Flag_valid_pseudorange;
this->Flag_PLL_180_deg_phase_locked = rhs.Flag_PLL_180_deg_phase_locked;
this->Flag_cycle_slip = rhs.Flag_cycle_slip;
this->Flag_half_cycle_slip = rhs.Flag_half_cycle_slip;
this->Flag_carrier_phase_continuous = rhs.Flag_carrier_phase_continuous;
}
return *this;
};
@@ -160,6 +172,8 @@ public:
this->Flag_valid_pseudorange = other.Flag_valid_pseudorange;
this->Flag_PLL_180_deg_phase_locked = other.Flag_PLL_180_deg_phase_locked;
this->Flag_cycle_slip = other.Flag_cycle_slip;
this->Flag_half_cycle_slip = other.Flag_half_cycle_slip;
this->Flag_carrier_phase_continuous = other.Flag_carrier_phase_continuous;
// Leave the source object in a valid but unspecified state
other.Signal[0] = '\0';
@@ -191,6 +205,8 @@ public:
other.Flag_valid_pseudorange = false;
other.Flag_PLL_180_deg_phase_locked = false;
other.Flag_cycle_slip = false;
other.Flag_half_cycle_slip = false;
other.Flag_carrier_phase_continuous = true;
}
return *this;
};
@@ -239,6 +255,8 @@ public:
ar& BOOST_SERIALIZATION_NVP(Flag_valid_pseudorange);
ar& BOOST_SERIALIZATION_NVP(Flag_PLL_180_deg_phase_locked);
ar& BOOST_SERIALIZATION_NVP(Flag_cycle_slip);
ar& BOOST_SERIALIZATION_NVP(Flag_half_cycle_slip);
ar& BOOST_SERIALIZATION_NVP(Flag_carrier_phase_continuous);
}
};
tests/unit-tests/control-plane/protobuf_test.cc
+72
View File
@@ -96,3 +96,75 @@ TEST(Protobuf, Works)
EXPECT_EQ(prn_read, prn_read3);
EXPECT_EQ(2, obs_size);
}
TEST(Protobuf, CarrierPhaseContinuitySurvivesSerialization)
{
Serdes_Gnss_Synchro serdes = Serdes_Gnss_Synchro();
Gnss_Synchro gs = Gnss_Synchro();
EXPECT_TRUE(gs.Flag_carrier_phase_continuous) << "a fresh Gnss_Synchro must not report a discontinuity";
std::vector<Gnss_Synchro> vgs;
vgs.push_back(gs);
gs.Flag_carrier_phase_continuous = false;
vgs.push_back(gs);
gnss_sdr::Observables obs;
ASSERT_TRUE(obs.ParseFromString(serdes.createProtobuffer(vgs)));
const std::vector<Gnss_Synchro> vgs_read = serdes.readProtobuffer(obs);
ASSERT_EQ(2U, vgs_read.size());
EXPECT_TRUE(vgs_read[0].Flag_carrier_phase_continuous);
EXPECT_FALSE(vgs_read[1].Flag_carrier_phase_continuous);
// the flag travels negated, so that a message written by a version that does
// not know the field is read back as "no discontinuity"
EXPECT_FALSE(obs.observable(0).flag_carrier_phase_discontinuity());
EXPECT_TRUE(obs.observable(1).flag_carrier_phase_discontinuity());
gnss_sdr::Observables without_field;
without_field.add_observable();
EXPECT_TRUE(serdes.readProtobuffer(without_field)[0].Flag_carrier_phase_continuous);
EXPECT_FALSE(serdes.readProtobuffer(without_field)[0].Flag_half_cycle_slip);
}
TEST(Protobuf, HalfCycleSlipSurvivesSerialization)
{
Serdes_Gnss_Synchro serdes = Serdes_Gnss_Synchro();
Gnss_Synchro gs = Gnss_Synchro();
EXPECT_FALSE(gs.Flag_half_cycle_slip);
std::vector<Gnss_Synchro> vgs;
vgs.push_back(gs);
gs.Flag_half_cycle_slip = true;
vgs.push_back(gs);
gnss_sdr::Observables obs;
ASSERT_TRUE(obs.ParseFromString(serdes.createProtobuffer(vgs)));
const std::vector<Gnss_Synchro> vgs_read = serdes.readProtobuffer(obs);
ASSERT_EQ(2U, vgs_read.size());
EXPECT_FALSE(vgs_read[0].Flag_half_cycle_slip);
EXPECT_TRUE(vgs_read[1].Flag_half_cycle_slip);
}
TEST(Protobuf, CarrierPhaseContinuitySurvivesCopyAndMove)
{
Gnss_Synchro gs = Gnss_Synchro();
gs.Flag_carrier_phase_continuous = false;
Gnss_Synchro copy_constructed(gs);
EXPECT_FALSE(copy_constructed.Flag_carrier_phase_continuous);
Gnss_Synchro copy_assigned = Gnss_Synchro();
copy_assigned = gs;
EXPECT_FALSE(copy_assigned.Flag_carrier_phase_continuous);
Gnss_Synchro move_assigned = Gnss_Synchro();
move_assigned = std::move(copy_constructed);
EXPECT_FALSE(move_assigned.Flag_carrier_phase_continuous);
}
tests/unit-tests/signal-processing-blocks/pvt/ntrip_rtcm_client_test.cc
+67 -6
View File
@@ -625,7 +625,9 @@ std::vector<unsigned char> make_mt1002()
}
std::vector<unsigned char> make_mt1074()
std::vector<unsigned char> make_mt1074(bool half_cycle_ambiguity = false,
unsigned int lock_indicator = 5,
double tow_s = TEST_TOW_S)
{
// One GPS satellite with L1 C/A (signal ID 2) and L2C(M) (signal ID 15).
// Nonzero fine code and phase fields make both decoded bands usable.
@@ -640,7 +642,7 @@ std::vector<unsigned char> make_mt1074()
setbitu(frame.data(), bit, 12, TEST_STATION_ID);
bit += 12;
setbitu(frame.data(), bit, 30,
static_cast<unsigned int>(TEST_TOW_S * 1000.0));
static_cast<unsigned int>(tow_s * 1000.0));
bit += 30;
setbitu(frame.data(), bit, 1, 0); // Last message in the epoch
bit += 1;
@@ -680,12 +682,12 @@ std::vector<unsigned char> make_mt1074()
bit += 22;
setbits(frame.data(), bit, 22, 2500);
bit += 22;
setbitu(frame.data(), bit, 4, 5);
setbitu(frame.data(), bit, 4, lock_indicator); // Lock time indicator, signal 1
bit += 4;
setbitu(frame.data(), bit, 4, 5);
setbitu(frame.data(), bit, 4, lock_indicator); // Lock time indicator, signal 2
bit += 4;
setbitu(frame.data(), bit++, 1, 0);
setbitu(frame.data(), bit++, 1, 0);
setbitu(frame.data(), bit++, 1, half_cycle_ambiguity ? 1U : 0U);
setbitu(frame.data(), bit++, 1, half_cycle_ambiguity ? 1U : 0U);
setbitu(frame.data(), bit, 6, 45);
bit += 6;
setbitu(frame.data(), bit, 6, 45);
@@ -1556,6 +1558,65 @@ TEST(NtripRtcmClientTest, GeneratedMsm4FixtureIsValidDualBandRtcm3)
}
TEST(NtripRtcmClientTest, Msm4HalfCycleAmbiguityDoesNotFlagACycleSlip)
{
using namespace ntrip_rtcm_client_test;
rtcm_t decoder = {};
ASSERT_EQ(1, init_rtcm(&decoder));
decoder.time = gpst2time(TEST_GPS_WEEK, TEST_TOW_S);
int decode_result = 0;
const std::vector<unsigned char> frame = make_mt1074(true);
for (const unsigned char byte : frame)
{
decode_result = input_rtcm3(&decoder, byte);
}
ASSERT_EQ(1, decode_result);
ASSERT_EQ(1, decoder.obs.n);
// An unresolved half-cycle ambiguity sets bit 1 only. Reporting bit 0 as
// well would make detslp_ll() declare a cycle slip on every epoch the base
// keeps the flag raised, resetting the ambiguity and preventing a fix.
EXPECT_EQ(2, decoder.obs.data[0].LLI[0]);
EXPECT_EQ(2, decoder.obs.data[0].LLI[1]);
free_rtcm(&decoder);
}
TEST(NtripRtcmClientTest, Msm4LossOfLockAndHalfCycleAmbiguityKeepSeparateBits)
{
using namespace ntrip_rtcm_client_test;
rtcm_t decoder = {};
ASSERT_EQ(1, init_rtcm(&decoder));
decoder.time = gpst2time(TEST_GPS_WEEK, TEST_TOW_S);
int decode_result = 0;
const std::vector<unsigned char> locked = make_mt1074(false, 9, TEST_TOW_S);
for (const unsigned char byte : locked)
{
decode_result = input_rtcm3(&decoder, byte);
}
ASSERT_EQ(1, decode_result);
ASSERT_EQ(1, decoder.obs.n);
EXPECT_EQ(0, decoder.obs.data[0].LLI[0]);
// Lock indicator decreases (loss of lock) while the half-cycle ambiguity is
// unresolved: both flags must survive, one per bit. Adding them as 1 + 3
// would yield 4, silently dropping the slip.
const std::vector<unsigned char> reacquired = make_mt1074(true, 1, TEST_TOW_S + 1.0);
for (const unsigned char byte : reacquired)
{
decode_result = input_rtcm3(&decoder, byte);
}
ASSERT_EQ(1, decode_result);
ASSERT_EQ(1, decoder.obs.n);
EXPECT_EQ(3, decoder.obs.data[0].LLI[0]);
EXPECT_EQ(3, decoder.obs.data[0].LLI[1]);
free_rtcm(&decoder);
}
TEST(NtripRtcmClientTest, DisabledAndInvalidConfigurationsDoNotStartAWorker)
{
Ntrip_Rtcm_Client_Config disabled_config;
tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc
+65
View File
@@ -532,6 +532,71 @@ TEST_F(RinexPrinterTest, Rinex4GpsNavAndObs)
}
TEST_F(RinexPrinterTest, ObservationRecordsReportLossOfLockIndicatorBits)
{
Pvt_Conf conf;
conf.use_e6_for_pvt = false;
const auto signal_enabled_flags = GPS_1C;
auto pvt_solution = std::make_shared<Rtklib_Solver>(rtk, conf, "filename", signal_enabled_flags, false, false);
Gps_Ephemeris eph;
eph.PRN = 1;
eph.WN = 512;
pvt_solution->gps_ephemeris_map[1] = eph;
Gnss_Synchro gs{};
gs.System = 'G';
std::memcpy(static_cast<void*>(gs.Signal), "1C", 3);
gs.Pseudorange_m = 22000000.0;
gs.Carrier_phase_rads = 23.4;
gs.Carrier_Doppler_hz = 1534.0;
gs.CN0_dB_hz = 42.0;
gs.Flag_valid_pseudorange = true;
std::map<int, Gnss_Synchro> gnss_observables_map;
gs.PRN = 1; // clean
gnss_observables_map[1] = gs;
gs.PRN = 2; // cycle slip
gs.Flag_cycle_slip = true;
gnss_observables_map[2] = gs;
gs.PRN = 3; // half-cycle slip
gs.Flag_cycle_slip = false;
gs.Flag_half_cycle_slip = true;
gnss_observables_map[3] = gs;
gs.PRN = 4; // both
gs.Flag_cycle_slip = true;
gnss_observables_map[4] = gs;
for (int prn = 2; prn <= 4; prn++)
{
pvt_solution->gps_ephemeris_map[prn] = eph;
}
auto rp = std::make_shared<Rinex_Printer>(signal_enabled_flags, 3);
rp->print_rinex_annotation(pvt_solution.get(), gnss_observables_map, 42.0, true);
const std::string obsfile = rp->get_obsfilename();
const std::string navfile = rp->get_navfilename()[0];
rp = nullptr; // close the RINEX files so we can inspect them
// the LLI of the carrier phase observation sits right after its 14-character
// value, which follows the 3-character satellite id and the pseudorange field
const std::size_t phase_lli_position = 3 + 16 + 14;
const auto phase_lli_of = [&obsfile](const std::string& satellite) {
const std::string record = find_line_starting_with(obsfile, satellite);
EXPECT_FALSE(record.empty()) << "no observation record for " << satellite;
return record.size() > phase_lli_position ? record[phase_lli_position] : '?';
};
EXPECT_EQ(' ', phase_lli_of("G01"));
EXPECT_EQ('1', phase_lli_of("G02"));
EXPECT_EQ('2', phase_lli_of("G03"));
EXPECT_EQ('3', phase_lli_of("G04"));
fs::remove(obsfile);
fs::remove(navfile);
}
TEST_F(RinexPrinterTest, Rinex4GalileoNav)
{
Pvt_Conf conf;
tests/unit-tests/signal-processing-blocks/pvt/rtklib_fixed_base_test.cc
+182
View File
@@ -680,6 +680,188 @@ TEST_F(RtklibFixedBaseTest, ConsistentDualFrequencyBaseProducesRelativeSolution)
}
TEST_F(RtklibFixedBaseTest, RepeatedPhaseOutliersResetTheStaleAmbiguity)
{
using namespace rtklib_fixed_base_test_detail;
const double base_position_geodetic[3] = {45.0 * D2R, 8.0 * D2R, 100.0};
double base_position_ecef[3]{};
pos2ecef(base_position_geodetic, base_position_ecef);
const double baseline_enu_m[3] = {8.0, 4.0, 1.0};
double baseline_ecef_m[3]{};
enu2ecef(base_position_geodetic, baseline_enu_m, baseline_ecef_m);
const double rover_position_ecef[3] = {
base_position_ecef[0] + baseline_ecef_m[0],
base_position_ecef[1] + baseline_ecef_m[1],
base_position_ecef[2] + baseline_ecef_m[2]};
/* a long outage counter keeps the satellite from being rescued by the
maxout path, so the recovery below can only come from the reject counter */
prcopt_t options = fixed_base_options();
options.maxout = 50;
auto solver = make_solver_with_options(options);
const std::vector<unsigned int> satellites = select_relative_satellites(
base_position_ecef, rover_position_ecef);
ASSERT_GE(satellites.size(), 6U);
/* a carrier-phase jump larger than the innovation threshold (100 m here)
makes the double differences of one satellite outliers from then on */
const unsigned int corrupted_prn = satellites.back();
constexpr double PHASE_JUMP_M = 300.0;
constexpr int FIRST_CORRUPTED_EPOCH = 4;
const double wavelengths_m[2] = {
SPEED_OF_LIGHT_M_S / FREQ1,
SPEED_OF_LIGHT_M_S / FREQ2};
std::vector<unsigned int> valid_satellites;
for (int epoch_index = 0; epoch_index < 12; ++epoch_index)
{
Synthetic_Relative_Epoch epoch = make_relative_epoch(
*solver, epoch_index, base_position_ecef, rover_position_ecef, satellites);
if (epoch_index >= FIRST_CORRUPTED_EPOCH)
{
for (unsigned int band = 0; band < 2U; ++band)
{
const auto observation = epoch.rover_observations.find(
static_cast<int>(corrupted_prn * 2U + band));
ASSERT_NE(epoch.rover_observations.end(), observation);
observation->second.Carrier_phase_rads +=
PHASE_JUMP_M / wavelengths_m[band] * 2.0 * GNSS_PI;
}
}
SCOPED_TRACE(::testing::Message() << "epoch=" << epoch_index);
ASSERT_TRUE(solve(*solver, epoch.rover_observations, &epoch.base_snapshot));
valid_satellites.push_back(solver->pvt_sol.ns);
}
const unsigned int all_satellites = static_cast<unsigned int>(satellites.size());
EXPECT_EQ(all_satellites, valid_satellites[FIRST_CORRUPTED_EPOCH - 1]);
/* the jump first takes the satellite out of the solution ... */
EXPECT_LT(valid_satellites[FIRST_CORRUPTED_EPOCH], all_satellites);
/* ... and the stale bias is then reinitialized instead of being rejected
forever, so the satellite comes back on its own */
EXPECT_EQ(all_satellites, valid_satellites.back());
}
TEST_F(RtklibFixedBaseTest, AnUnresolvedHalfCycleAmbiguityIsDeweighted)
{
using namespace rtklib_fixed_base_test_detail;
const double base_position_geodetic[3] = {45.0 * D2R, 8.0 * D2R, 100.0};
double base_position_ecef[3]{};
pos2ecef(base_position_geodetic, base_position_ecef);
const double baseline_enu_m[3] = {8.0, 4.0, 1.0};
double baseline_ecef_m[3]{};
enu2ecef(base_position_geodetic, baseline_enu_m, baseline_ecef_m);
const double rover_position_ecef[3] = {
base_position_ecef[0] + baseline_ecef_m[0],
base_position_ecef[1] + baseline_ecef_m[1],
base_position_ecef[2] + baseline_ecef_m[2]};
/* keep the float solution: a satellite flagged as half-cycle ambiguous is
excluded from ambiguity resolution on its own, and this exercises the
measurement weighting instead */
prcopt_t options = fixed_base_options();
options.modear = ARMODE_OFF;
const auto position_variance_with_flag = [&](bool report_half_cycle) {
auto solver = make_solver_with_options(options);
const std::vector<unsigned int> satellites = select_relative_satellites(
base_position_ecef, rover_position_ecef);
EXPECT_GE(satellites.size(), 6U);
for (int epoch_index = 0; epoch_index < 6; ++epoch_index)
{
Synthetic_Relative_Epoch epoch = make_relative_epoch(
*solver, epoch_index, base_position_ecef, rover_position_ecef, satellites);
for (auto& rover_observation : epoch.rover_observations)
{
rover_observation.second.Flag_half_cycle_slip = report_half_cycle;
}
EXPECT_TRUE(solve(*solver, epoch.rover_observations, &epoch.base_snapshot));
}
return static_cast<double>(solver->pvt_sol.qr[0]) +
static_cast<double>(solver->pvt_sol.qr[1]) +
static_cast<double>(solver->pvt_sol.qr[2]);
};
const double variance_unflagged = position_variance_with_flag(false);
const double variance_flagged = position_variance_with_flag(true);
// the flagged carrier phases are worth their half wavelength rather than a
// few millimeters, and the reported solution says so
EXPECT_GT(variance_flagged, 1.001 * variance_unflagged)
<< "flagged: " << variance_flagged << " m^2, unflagged: " << variance_unflagged << " m^2";
}
TEST_F(RtklibFixedBaseTest, TooFewPhaseObservationsFallBackToACodeDifferencedSolution)
{
using namespace rtklib_fixed_base_test_detail;
const double base_position_geodetic[3] = {45.0 * D2R, 8.0 * D2R, 100.0};
double base_position_ecef[3]{};
pos2ecef(base_position_geodetic, base_position_ecef);
const double baseline_enu_m[3] = {8.0, 4.0, 1.0};
double baseline_ecef_m[3]{};
enu2ecef(base_position_geodetic, baseline_enu_m, baseline_ecef_m);
const double rover_position_ecef[3] = {
base_position_ecef[0] + baseline_ecef_m[0],
base_position_ecef[1] + baseline_ecef_m[1],
base_position_ecef[2] + baseline_ecef_m[2]};
auto solver = make_solver(false);
const std::vector<unsigned int> satellites = select_relative_satellites(
base_position_ecef, rover_position_ecef);
ASSERT_GE(satellites.size(), 6U);
/* keep the code observations of every satellite but leave carrier phase on
only two of them, so fewer than four phase measurements survive */
const std::vector<unsigned int> phase_satellites(satellites.cbegin(), satellites.cbegin() + 2);
for (int epoch_index = 0; epoch_index < 4; ++epoch_index)
{
Synthetic_Relative_Epoch epoch = make_relative_epoch(
*solver, epoch_index, base_position_ecef, rover_position_ecef, satellites);
for (const unsigned int prn : satellites)
{
if (std::find(phase_satellites.cbegin(), phase_satellites.cend(), prn) !=
phase_satellites.cend())
{
continue;
}
for (unsigned int band = 0; band < 2U; ++band)
{
const auto observation = epoch.rover_observations.find(
static_cast<int>(prn * 2U + band));
ASSERT_NE(epoch.rover_observations.end(), observation);
observation->second.Carrier_phase_rads = 0.0;
}
for (obsd_t& base_observation : epoch.base_snapshot.observations)
{
if (base_observation.sat == satno(SYS_GPS, static_cast<int>(prn)))
{
base_observation.L[0] = base_observation.L[1] = 0.0;
}
}
}
SCOPED_TRACE(::testing::Message() << "epoch=" << epoch_index);
/* the epoch is not dropped: the code-differenced solution is kept */
ASSERT_TRUE(solve(*solver, epoch.rover_observations, &epoch.base_snapshot));
EXPECT_EQ(Rtklib_Fixed_Base_Status::APPLIED, solver->get_fixed_base_status());
EXPECT_EQ(SOLQ_DGPS, solver->pvt_sol.stat);
}
const double position_error_m = std::sqrt(
std::pow(solver->pvt_sol.rr[0] - rover_position_ecef[0], 2.0) +
std::pow(solver->pvt_sol.rr[1] - rover_position_ecef[1], 2.0) +
std::pow(solver->pvt_sol.rr[2] - rover_position_ecef[2], 2.0));
EXPECT_LT(position_error_m, 10.0);
}
TEST_F(RtklibFixedBaseTest, Demo5ArManagementProducesFixedSolutionWithFixAndHold)
{
using namespace rtklib_fixed_base_test_detail;