wip

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)

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(), ")");