Merge branch 'joebre-tracked-satellite-healthy-flag' into next

docs/CHANGELOG.md

@@ -371,7 +371,8 @@ All notable changes to GNSS-SDR will be documented in this file.
   ionosphere-free combination), and a `used` flag telling whether it contributed
   to the reported fix. Satellites that were tracked but left out of the solution
   (below `PVT.elevation_mask`, or excluded by RAIM) are listed with
-  `used = false`. Contributed by @joebre.
+  `used = false`. Unhealthy satellites are listed with `healthy = false`.
+  Contributed by @joebre.
 
 ### Improvements in Maintainability:
 

docs/protobuf/monitor_pvt.proto

@@ -60,7 +60,8 @@ message MonitorPvt {
   // appears as two entries, both with combined = true. A satellite can have
   // azimuth_deg/elevation_deg populated with used = false when it was
   // tracked and had az/el computed but excluded from the fix itself (e.g.
-  // below PVT.elevation_mask, or by RAIM FDE).
+  // below PVT.elevation_mask, or by RAIM FDE). The healthy flag carries the
+  // broadcast health of the signal, independently of used.
   message TrackedSatellite {
     uint32 prn = 1;
     string system = 2;  // "G" GPS, "E" Galileo, "R" GLONASS, "C" BeiDou, "S" SBAS, "J" QZSS
@@ -69,6 +70,7 @@ message MonitorPvt {
     double elevation_deg = 5;
     bool combined = 6;
     bool used = 7;
+    bool healthy = 8;
   }
   repeated TrackedSatellite tracked_satellites = 37;
 }

src/algorithms/PVT/libs/monitor_pvt.h

@@ -54,6 +54,9 @@ public:
         // false when tracked (az/el valid) but excluded from the solve, e.g.
         // below PVT.elevation_mask or by RAIM FDE.
         bool used{true};
+        // Broadcast health status, independent of `used` (which alone can't
+        // tell a monitor client *why* a satellite was excluded).
+        bool healthy{true};
 
         template <class Archive>
         void serialize(Archive& ar, const unsigned int version)
@@ -68,6 +71,7 @@ public:
             ar& BOOST_SERIALIZATION_NVP(elevation_deg);
             ar& BOOST_SERIALIZATION_NVP(combined);
             ar& BOOST_SERIALIZATION_NVP(used);
+            ar& BOOST_SERIALIZATION_NVP(healthy);
         }
     };
 

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -1419,6 +1419,121 @@ bool Rtklib_Solver::get_galileo_signal_health(uint32_t prn, const std::string &s
 }
 
 
+bool Rtklib_Solver::get_broadcast_signal_health(char system, uint32_t prn, const std::string &signal,
+    uint32_t observation_tow, bool &healthy) const
+{
+    const auto prn_key = static_cast<int>(prn);
+    switch (system)
+        {
+        case 'G':
+        case 'J':
+            {
+                // L2C and L5 health comes from the CNAV message that carries them:
+                // bits 52-54 of message type 10 are the L1, L2 and L5 signal health
+                // (0 = OK, IS-GPS-200 30.3.3.1.1.2).
+                if (signal == "2S" || signal == "L5" || signal == "J5")
+                    {
+                        const auto cnav_it = gps_cnav_ephemeris_map.find(prn_key);
+                        if (cnav_it != gps_cnav_ephemeris_map.cend())
+                            {
+                                const int32_t signal_bit = (signal == "2S") ? 0x2 : 0x1;
+                                healthy = ((cnav_it->second.signal_health & signal_bit) == 0);
+                                return true;
+                            }
+                    }
+                // L1 C/A: per-satellite SV health from LNAV subframe 1, falling back
+                // to the almanac (broadcast by every satellite) when no ephemeris has
+                // been decoded for this satellite yet.
+                const auto eph_it = gps_ephemeris_map.find(prn_key);
+                if (eph_it != gps_ephemeris_map.cend())
+                    {
+                        healthy = (eph_it->second.SV_health == 0);
+                        return true;
+                    }
+                const auto alm_it = gps_almanac_map.find(prn_key);
+                if (alm_it != gps_almanac_map.cend())
+                    {
+                        healthy = (alm_it->second.SV_health == 0);
+                        return true;
+                    }
+                return false;
+            }
+        case 'E':
+            {
+                if (get_galileo_signal_health(prn, signal, observation_tow, healthy))
+                    {
+                        return true;
+                    }
+                // No ephemeris decoded for this signal's service yet -- fall
+                // back to the almanac, broadcast by every satellite.
+                const auto alm_it = galileo_almanac_map.find(prn_key);
+                if (alm_it != galileo_almanac_map.cend())
+                    {
+                        healthy = (alm_it->second.E1B_HS == 0);
+                        return true;
+                    }
+                return false;
+            }
+        case 'R':
+            {
+                // Same predicate as the ephemeris handed to RTKLIB (see
+                // eph_to_rtklib()), so a GLONASS satellite excluded from the solve
+                // for health reasons is reported with used = false, healthy = false.
+                const auto eph_it = glonass_gnav_ephemeris_map.find(prn_key);
+                if (eph_it != glonass_gnav_ephemeris_map.cend())
+                    {
+                        healthy = glonass_gnav_is_healthy(eph_it->second, d_conf.glonass_strict_health);
+                        return true;
+                    }
+                return false;
+            }
+        case 'C':
+            {
+                if (signal == "1D")
+                    {
+                        // B1C: B-CNAV1 subframe 3 health status (HS, 0 = healthy). The
+                        // latest page data is preferred over the ephemeris record, as
+                        // when the ephemeris is handed to RTKLIB.
+                        const auto page_it = beidou_cnav1_page_data_map.find(prn_key);
+                        if (page_it != beidou_cnav1_page_data_map.cend())
+                            {
+                                healthy = (page_it->second.common.hs == 0);
+                                return true;
+                            }
+                        const auto cnav1_it = beidou_cnav1_ephemeris_map.find(prn_key);
+                        if (cnav1_it != beidou_cnav1_ephemeris_map.cend())
+                            {
+                                healthy = (cnav1_it->second.hs == 0);
+                                return true;
+                            }
+                        return false;
+                    }
+                if (signal == "5D")
+                    {
+                        // B2a: B-CNAV2 health status (HS, 0 = healthy)
+                        const auto cnav2_it = beidou_cnav2_ephemeris_map.find(prn_key);
+                        if (cnav2_it != beidou_cnav2_ephemeris_map.cend())
+                            {
+                                healthy = (cnav2_it->second.hs == 0);
+                                return true;
+                            }
+                        return false;
+                    }
+                // B1I / B3I: DNAV satellite health (SatH1, 0 = healthy)
+                const auto dnav_it = beidou_dnav_ephemeris_map.find(prn_key);
+                if (dnav_it != beidou_dnav_ephemeris_map.cend())
+                    {
+                        healthy = (dnav_it->second.SV_health == 0);
+                        return true;
+                    }
+                return false;
+            }
+        default:
+            return false;
+        }
+}
+
+
 std::map<int, Galileo_Ephemeris> Rtklib_Solver::get_galileo_ephemeris_map_for_pvt() const
 {
     auto result = galileo_ephemeris_store.combined_view(d_galileo_nav_message_type_for_pvt);
@@ -3072,6 +3187,15 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                     info.elevation_deg = pvt_ssat[sat_idx].azel[1] * R2D;
                                     info.combined = combined;
                                     info.used = used;
+                                    // Broadcast health of this signal, as reported by the
+                                    // navigation message that carries it (true when no health
+                                    // information is available). Independent of `used`.
+                                    const auto observation_tow = static_cast<uint32_t>(synchro->interp_TOW_ms / 1000.0);
+                                    bool healthy = true;
+                                    if (get_broadcast_signal_health(sys_char, static_cast<uint32_t>(prn), info.signal, observation_tow, healthy))
+                                        {
+                                            info.healthy = healthy;
+                                        }
                                     d_monitor_pvt.tracked_satellites.push_back(info);
                                 }
                         }

src/algorithms/PVT/libs/rtklib_solver.h

@@ -161,6 +161,17 @@ public:
     Galileo_Nav_Message_Type galileo_nav_message_type_for_pvt() const;
     bool is_galileo_signal_used_in_pvt(const std::string& signal) const;
     bool get_galileo_signal_health(uint32_t prn, const std::string& signal, uint32_t observation_tow, bool& healthy) const;
+    /*!
+     * \brief Broadcast health of one tracked signal, as reported by the navigation
+     * message that carries it: GPS/QZSS L1 C/A from the LNAV SV health (almanac
+     * when no ephemeris has been decoded yet), GPS/QZSS L2C and L5 from the CNAV
+     * per-signal health bits, Galileo per signal (see get_galileo_signal_health(),
+     * almanac E1B_HS when no ephemeris is available), GLONASS from the GNAV ln/Bn
+     * flags, BeiDou B1I/B3I from the DNAV SV health and B1C/B2a from the
+     * B-CNAV1/B-CNAV2 health status. Independent of PVT.use_unhealthy_sats.
+     * \return true if health information is available for that signal (healthy is then set)
+     */
+    bool get_broadcast_signal_health(char system, uint32_t prn, const std::string& signal, uint32_t observation_tow, bool& healthy) const;
     std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map_for_pvt() const;
     bool select_galileo_ephemeris(uint32_t prn, const std::string& signal, uint32_t observation_tow,
         Galileo_Ephemeris& ephemeris, bool& from_reduced_ced) const;

src/algorithms/PVT/libs/serdes_monitor_pvt.h

@@ -130,6 +130,7 @@ public:
                 pb_sat->set_elevation_deg(sat.elevation_deg);
                 pb_sat->set_combined(sat.combined);
                 pb_sat->set_used(sat.used);
+                pb_sat->set_healthy(sat.healthy);
             }
 
         if (!monitor_.SerializeToString(&data))
@@ -190,6 +191,7 @@ public:
                 sat.elevation_deg = pb_sat.elevation_deg();
                 sat.combined = pb_sat.combined();
                 sat.used = pb_sat.used();
+                sat.healthy = pb_sat.healthy();
                 monitor.tracked_satellites.push_back(sat);
             }
 

src/algorithms/libs/rtklib/rtklib_conversions.cc

@@ -402,22 +402,28 @@ obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs,
 }
 
 
+bool glonass_gnav_is_healthy(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, bool glonass_strict_health)
+{
+    // The ln flag (string 3) always marks a satellite malfunction. The MSB of
+    // the Bn word is a stricter health indicator that is only applied when
+    // glonass_strict_health is enabled
+    const bool bn_msb_unhealthy = glonass_strict_health && ((static_cast<int32_t>(glonass_gnav_eph.d_B_n) & 4) != 0);
+    return !(bn_msb_unhealthy || glonass_gnav_eph.d_l3rd_n);
+}
+
+
 geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glonass_Gnav_Utc_Model& gnav_clock_model, bool glonass_strict_health)
 {
     int week;
     double sec;
     int adj_week;
     geph_t rtklib_sat = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0, 0.0, 0.0};
-
-    // The ln flag (string 3) always marks a satellite malfunction. The MSB of
-    // the Bn word is a stricter health indicator that is only applied when
-    // glonass_strict_health is enabled
-    const bool bn_msb_unhealthy = glonass_strict_health && ((static_cast<int32_t>(glonass_gnav_eph.d_B_n) & 4) != 0);
+    const bool healthy = glonass_gnav_is_healthy(glonass_gnav_eph, glonass_strict_health);
 
     rtklib_sat.sat = satno(SYS_GLO, glonass_gnav_eph.i_satellite_slot_number);       /* satellite number */
     rtklib_sat.iode = static_cast<int>(std::lround(glonass_gnav_eph.d_t_b / 900.0)); /* IODE (tb interval index) */
     rtklib_sat.frq = glonass_gnav_eph.i_satellite_freq_channel;                      /* satellite frequency number */
-    rtklib_sat.svh = (bn_msb_unhealthy || glonass_gnav_eph.d_l3rd_n) ? 1 : 0;        /* satellite health from the ln flag (and optionally the Bn MSB) */
+    rtklib_sat.svh = healthy ? 0 : 1;                                                /* satellite health from the ln flag (and optionally the Bn MSB) */
     rtklib_sat.sva = static_cast<int>(glonass_gnav_eph.d_F_T);                       /* satellite accuracy*/
     rtklib_sat.age = static_cast<int>(glonass_gnav_eph.d_E_n);                       /* satellite age*/
     rtklib_sat.pos[0] = glonass_gnav_eph.d_Xn * 1000;                                /* satellite position (ecef) (m) */

src/algorithms/libs/rtklib/rtklib_conversions.h

@@ -127,6 +127,15 @@ alm_t alm_to_rtklib(const Gps_Almanac& gps_alm, int ref_week = 0);
 alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm, int ref_week = 0);
 alm_t alm_to_rtklib(const Beidou_Dnav_Almanac& bei_alm);
 
+/*!
+ * \brief Broadcast health of a GLONASS satellite according to its GNAV ephemeris
+ * \param glonass_gnav_eph GLONASS GNAV Ephemeris structure
+ * \param glonass_strict_health when true (default), the MSB of the Bn word also marks the
+ * satellite as unhealthy; when false, only the ln flag is used
+ * \return true if the satellite does not report a malfunction
+ */
+bool glonass_gnav_is_healthy(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, bool glonass_strict_health = true);
+
 /*!
  * \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart
  * \param glonass_gnav_eph GLONASS GNAV Ephemeris structure