diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h index 2bfbfb3c1d7c0f1e2e921c89a70cffe98411f274..67495633d1e064215fd854328cfdba5504c20d7c 100644 --- a/include/gnss/processor/processor_tracker_gnss.h +++ b/include/gnss/processor/processor_tracker_gnss.h @@ -25,6 +25,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature bool init_frames, pseudo_ranges, fix; double enu_map_fix_dist; int min_sbas_sats; + bool tdcp_all_against_all; ParamsProcessorTrackerGnss() = default; ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server): @@ -39,6 +40,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature fix = _server.getParam<bool> (prefix + _unique_name + "/fix"); pseudo_ranges = _server.getParam<bool> (prefix + _unique_name + "/pseudo_ranges"); min_sbas_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/min_sbas_sats"); + tdcp_all_against_all = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/all_against_all"); // GNSS OPTIONS (see rtklib.h) gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC) @@ -127,6 +129,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature + "gnss/constellations/IRN: " + std::to_string(gnss_opt.IRN) + "\n" + "gnss/constellations/LEO: " + std::to_string(gnss_opt.LEO) + "\n" + "gnss/tdcp/enabled: " + std::to_string(gnss_opt.tdcp.enabled) + "\n" + + "gnss/tdcp/all_agains_all: " + std::to_string(tdcp_all_against_all) + "\n" + "gnss/tdcp/batch: " + std::to_string(gnss_opt.tdcp.batch) + "\n" + "gnss/tdcp/corr_iono: " + std::to_string(gnss_opt.tdcp.corr_iono) + "\n" + "gnss/tdcp/corr_tropo: " + std::to_string(gnss_opt.tdcp.corr_tropo) + "\n" diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index 7687491ba6627ec90aa4424271efdc1382f97f08..ae16732675aad95f535b0f0d7b540dd27852494e 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -383,7 +383,7 @@ void ProcessorTrackerGnss::establishFactors() // TDCP FACTORS (tracked sats) if (origin_ptr_ != last_ptr_ and params_tracker_gnss_->gnss_opt.tdcp.enabled) { - // FACTOR per pair of KF (FactorGnssTdcp3d) + // Displacement factor from batch TDCP (FactorGnssTdcp3d) if (params_tracker_gnss_->gnss_opt.tdcp.batch) { WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id()); @@ -462,15 +462,25 @@ void ProcessorTrackerGnss::establishFactors() WOLF_DEBUG("TDCP BATCH cov =\n", tdcp_output.cov_d); // EMPLACE FEATURE - auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(tdcp_output.d.head<3>()), Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>())); + auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, + Eigen::Vector3d(tdcp_output.d.head<3>()), + Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>())); // EMPLACE FACTOR - FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this()); + FactorBase::emplace<FactorGnssTdcp3d>(ftr, + ftr, + KF, + sensor_gnss_, + shared_from_this()); } else { WOLF_DEBUG("TDCP BATCH failed with msg: ", tdcp_output.msg); } + + // just one factor + if (not params_tracker_gnss_->tdcp_all_against_all) + break; } } // FACTOR per SATELLITE (FactorGnssTdcp) @@ -533,6 +543,10 @@ void ProcessorTrackerGnss::establishFactors() // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), // " origin: " , feature_in_origin->id() , // " from last: " , feature_in_last->id() ); + + // just one factor + if (not params_tracker_gnss_->tdcp_all_against_all) + break; } WOLF_DEBUG("All TDCP factors emplaced!"); }