From 40215027c3860c831c84492bb42b30edd963e70b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 28 Apr 2021 14:45:43 +0200
Subject: [PATCH] wip
---
.../gnss/processor/processor_tracker_gnss.h | 2 +
src/processor/processor_tracker_gnss.cpp | 44 +++++++++++++------
2 files changed, 33 insertions(+), 13 deletions(-)
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index ab710f73b..5a544d800 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 detect_cycle_slips;
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");
+ detect_cycle_slips = _server.getParam<bool> (prefix + _unique_name + "/detect_cycle_slips");
// 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)
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 7687491ba..23d4ab9a8 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -214,6 +214,8 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
assert(_capture == incoming_ptr_);
int common_sats = 0;
+ int wrong_carrier = 0;
+ int cycle_slips = 0;
for (auto feat_in : _features_in)
{
@@ -224,29 +226,45 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
WOLF_DEBUG("tracking " , feat_in->trackId() , ", sat number ", sat_num);
+ // satellite exists
if (untracked_incoming_features_.count(sat_num) != 0)
+ {
common_sats++;
- if (untracked_incoming_features_.count(sat_num) != 0 and
- std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12) // Track only carrier phase valid
- {
- auto ftr = untracked_incoming_features_[sat_num];
- untracked_incoming_features_.erase(sat_num);
- _features_out.push_back(ftr);
- _feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+ // valid carrier phase data
+ if (std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12)
+ {
+ // cycle slip
+ if (params_tracker_gnss_->detect_cycle_slips and
+ std::abs(untracked_incoming_features_.at(sat_num)->getRange().L - feat_in_gnss->getRange().L) > 0.1)
+ {
+ cycle_slips++;
+ WOLF_DEBUG("\tnot tracked, cycle slip detected" );
+ continue;
+ }
+
+ auto ftr = untracked_incoming_features_[sat_num];
+ untracked_incoming_features_.erase(sat_num);
+ _features_out.push_back(ftr);
+ _feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
- WOLF_DEBUG("\ttracked: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
+ WOLF_DEBUG("\ttracked: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
+ }
+ else
+ {
+ wrong_carrier++;
+ WOLF_DEBUG("\tnot tracked, wrong carrier phase value" );
+ }
}
else
- {
- WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 0, "\tnot tracked, missing satellite" );
- WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 1, "\tnot tracked, wrong carrier phase value" );
- }
+ WOLF_DEBUG("\tnot tracked, missing satellite" );
}
WOLF_WARN_COND(_features_out.empty() and
_features_out.begin() == known_features_incoming_.begin(),
"ProcessorTrackerGnss::trackFeatures: LOST TRACK OF ALL SATELLITES of ", _features_in.size(),
- " - ", common_sats, " due to wrong Carrier Phase data.");
+ " - common sats: ", common_sats,
+ " - wrong carrier data: ", wrong_carrier,
+ " - cycle slips: ", cycle_slips);
WOLF_DEBUG_COND(_features_out.begin() == new_features_incoming_.begin(),
"ProcessorTrackerGnss::trackFeatures: tracked " ,
_features_out.size(), " new features (of ", _features_in.size(), ")");
--
GitLab