diff --git a/src/tdcp.cpp b/src/tdcp.cpp index 3c91462cc8283dda59122bfc91060b3cb0bd07e8..e56c28506a70813966adeeba9f81b7e3c1b354f3 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -34,8 +34,9 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, auto nav_k = snapshot_k->getNavigation(); if (tdcp_params.tdcp.use_old_nav) { - snapshot_k->getSatellites().clear(); - snapshot_k->setNavigation(snapshot_r->getNavigation()); + auto new_snapshot_k = std::make_shared<Snapshot>(std::make_shared<Observations>(*snapshot_k->getObservations()), + std::make_shared<Navigation>(*snapshot_k->getNavigation())); + snapshot_k = new_snapshot_k; } // COMPUTE SATELLITES POSITION @@ -44,36 +45,35 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, if (not snapshot_k->satellitesComputed()) snapshot_k->computeSatellites(opt.sateph); + // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR) + snapshot_r->filterObservations(discarded_sats, x_r, false, true, opt); + snapshot_k->filterObservations(discarded_sats, x_r, false, true, opt); + // COMPUTE RANGES if (not snapshot_r->rangesComputed()) snapshot_r->computeRanges(x_r, opt); if (not snapshot_k->rangesComputed()) snapshot_k->computeRanges(x_r, opt); // the x_r is only used to compute Azels -> corrections - // FILTER SATELLITES (ALREADY DISCARDED, CORRUPTED DATA, WRONG POSITION, CONSTELLATION, ELEVATION and SNR) - snapshot_r->filterObservations(discarded_sats, x_r, false, true, opt); - snapshot_k->filterObservations(discarded_sats, x_r, false, true, opt); - // FIND COMMON SATELLITES std::set<int> common_sats = Range::findCommonSatellites(snapshot_r->getRanges(), snapshot_k->getRanges()); - // COMPUTE TDCP - TdcpOutput output = Tdcp(snapshot_r, - snapshot_k, - x_r, - common_sats, - d_0, - tdcp_params); - - // UNDO temporary change navigation - if (tdcp_params.tdcp.use_old_nav) + if (common_sats.empty()) { - snapshot_k->setNavigation(nav_k); - snapshot_k->computeSatellites(opt.sateph); + TdcpOutput output; + output.success = false; + output.msg = "No common satellites after filtering observations."; + return output; } - return output; + // COMPUTE TDCP + return Tdcp(snapshot_r, + snapshot_k, + x_r, + common_sats, + d_0, + tdcp_params); } TdcpOutput Tdcp(SnapshotPtr snapshot_r,