implementing SBAS3 and SBAS4

include/gnss/processor/processor_tracker_gnss.h

 #ifndef PROCESSOR_TRACKER_GNSS_H_
 #define PROCESSOR_TRACKER_GNSS_H_
 
+#include "gnss/internal/config.h"
 #include "core/common/wolf.h"
 #include "core/processor/processor_tracker_feature.h"
 #include "gnss/sensor/sensor_gnss.h"
@@ -23,6 +24,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
     double outlier_residual_th;
     bool init_frames, pseudo_ranges;
     double enu_map_fix_dist;
+    int min_sbas_sats;
 
     ParamsProcessorTrackerGnss() = default;
     ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
@@ -35,6 +37,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
         max_time_span               = _server.getParam<double>  (prefix + _unique_name + "/keyframe_vote/max_time_span");
         enu_map_fix_dist            = _server.getParam<double>  (prefix + _unique_name + "/enu_map_fix_dist");
         pseudo_ranges               = _server.getParam<bool>    (prefix + _unique_name + "/pseudo_ranges");
+        min_sbas_sats               = _server.getParam<int>     (prefix + _unique_name + "/gnss/min_sbas_sats");
 
         // 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

@@ -88,6 +88,7 @@ void ProcessorTrackerGnss::preProcess()
     WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
 
     // filter observations (available ephemeris, constellations and elevation&SNR)
+    auto init_obs = std::make_shared<GnssUtils::Observations>(*inc_snapshot->getObservations());
     #ifdef _WOLF_DEBUG
         auto discarded_gnssutils =
     #endif
@@ -102,6 +103,57 @@ void ProcessorTrackerGnss::preProcess()
                                 fix_incoming_.lat_lon,
                                 params_tracker_gnss_->gnss_opt);
 
+    /* NOT ENOUGH SATS/RANGES in case of:
+     *     eph:  EPHOPT_SBAS3 and EPHOPT_SBAS4
+     *     iono: IONOOPT_SBAS3 and IONOOPT_SBAS4
+     */
+    if (inc_snapshot->getObservations()->size() < params_tracker_gnss_->min_sbas_sats and
+        (params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS3 or
+         params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS4 or
+         params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS3 or
+         params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS4))
+    {
+        WOLF_INFO("ProcessorTrackerGnss::preProcess: with SBAS3/SBAS4, not enough ranges: ", inc_snapshot->getObservations()->size(), " Computing with SBAS2/BRDC");
+
+        int eph_opt_old  = params_tracker_gnss_->gnss_opt.sateph;
+        int iono_opt_old = params_tracker_gnss_->gnss_opt.ionoopt;
+
+        if (eph_opt_old == EPHOPT_SBAS3)
+            params_tracker_gnss_->gnss_opt.sateph = EPHOPT_SBAS2;
+        if (eph_opt_old == EPHOPT_SBAS4)
+            params_tracker_gnss_->gnss_opt.sateph = EPHOPT_BRDC;
+
+        if (iono_opt_old == IONOOPT_SBAS3)
+            params_tracker_gnss_->gnss_opt.ionoopt = IONOOPT_SBAS2;
+        if (iono_opt_old == IONOOPT_SBAS4)
+            params_tracker_gnss_->gnss_opt.ionoopt = IONOOPT_BRDC;
+
+        // reset observations
+        inc_snapshot->setObservations(init_obs);
+        // recompute satellites
+        inc_snapshot ->computeSatellites(params_tracker_gnss_->gnss_opt.sateph);
+        // filter
+        #ifdef _WOLF_DEBUG
+            discarded_gnssutils =
+        #endif
+        inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats
+                                         fix_incoming_.sat_azel,
+                                         false, // check code
+                                         false, // check carrier phase
+                                         params_tracker_gnss_->gnss_opt);
+
+
+        // recompute corrected Ranges
+        inc_snapshot->computeRanges(fix_incoming_.sat_azel,
+                                    fix_incoming_.lat_lon,
+                                    params_tracker_gnss_->gnss_opt);
+
+        WOLF_INFO("ProcessorTrackerGnss::preProcess: with SBAS2/BRDC, obtained ranges: ", inc_snapshot->getObservations()->size());
+
+        params_tracker_gnss_->gnss_opt.sateph  = eph_opt_old;
+        params_tracker_gnss_->gnss_opt.ionoopt = iono_opt_old;
+    }
+
     // create features pseudorange
     for (auto obs_sat : inc_snapshot->getObservations()->getObservations())
     {