Use E6 channels in PVT

src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc

@@ -651,6 +651,11 @@ rtklib_pvt_gs::~rtklib_pvt_gs()
                                 {
                                     ofs.open(file_name.c_str(), std::ofstream::trunc | std::ofstream::out);
                                     boost::archive::xml_oarchive xml(ofs);
+                                    // Annotate as full GPS week number
+                                    for (auto& gal_eph_iter : d_internal_pvt_solver->galileo_ephemeris_map)
+                                        {
+                                            gal_eph_iter.second.WN += 1024;
+                                        }
                                     xml << boost::serialization::make_nvp("GNSS-SDR_gal_ephemeris_map", d_internal_pvt_solver->galileo_ephemeris_map);
                                     LOG(INFO) << "Saved Galileo E1 Ephemeris map data";
                                 }
@@ -2049,6 +2054,10 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
                                             store_valid_observable = true;
                                         }
                                 }
+                            if (std::string(in[i][epoch].Signal) == std::string("E6"))
+                                {
+                                    store_valid_observable = true;
+                                }
 
                             if (store_valid_observable)
                                 {

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -407,6 +407,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
     bool band1 = false;
     bool band2 = false;
     bool gal_e5_is_e5b = false;
+    bool gal_e6 = false;
     for (gnss_observables_iter = gnss_observables_map.cbegin();
          gnss_observables_iter != gnss_observables_map.cend();
          ++gnss_observables_iter)
@@ -514,6 +515,48 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                         gal_e5_is_e5b = true;
                                     }
                             }
+                        if (sig_ == "E6")
+                            {
+                                gal_e6 = true;
+                                galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
+                                if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
+                                    {
+                                        bool found_E1_obs = false;
+                                        for (int i = 0; i < valid_obs; i++)
+                                            {
+                                                if (eph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO)))
+                                                    {
+                                                        d_obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(d_obs_data[i + glo_valid_obs],
+                                                            gnss_observables_iter->second,
+                                                            galileo_ephemeris_iter->second.WN,
+                                                            2);  // Band E6
+                                                        found_E1_obs = true;
+                                                        break;
+                                                    }
+                                            }
+                                        if (!found_E1_obs)
+                                            {
+                                                // insert Galileo E6 obs as new obs and also insert its ephemeris
+                                                // convert ephemeris from GNSS-SDR class to RTKLIB structure
+                                                eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
+                                                // convert observation from GNSS-SDR class to RTKLIB structure
+                                                const auto default_code_ = static_cast<unsigned char>(CODE_NONE);
+                                                obsd_t newobs = {{0, 0}, '0', '0', {}, {},
+                                                    {default_code_, default_code_, default_code_},
+                                                    {}, {0.0, 0.0, 0.0}, {}};
+                                                d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
+                                                    gnss_observables_iter->second,
+                                                    galileo_ephemeris_iter->second.WN,
+                                                    2);  // Band E6
+                                                // std::cout << "Week " << galileo_ephemeris_iter->second.WN << '\n';
+                                                valid_obs++;
+                                            }
+                                    }
+                                else  // the ephemeris are not available for this SV
+                                    {
+                                        DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
+                                    }
+                            }
                         break;
                     }
                 case 'G':
@@ -879,6 +922,11 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                 {
                     for (int j = 0; j < NFREQ; j++)
                         {
+                            if (j == 2 && gal_e6)
+                                {
+                                    // frq = 3 corresponds to E6 in that function
+                                    nav_data.lam[i][j] = satwavelen(i + 1, 3, &nav_data);
+                                }
                             if (j == 2 && gal_e5_is_e5b)
                                 {
                                     // frq = 4 corresponds to E5B in that function

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc

@@ -42,6 +42,7 @@
 #include <cstddef>                   // for size_t
 #include <exception>                 // for std::exception
 #include <iostream>                  // for std::cout
+#include <limits>                    // for std::numeric_limits
 #include <map>                       // for std::map
 #include <stdexcept>                 // for std::out_of_range
 #include <typeinfo>                  // for typeid
@@ -79,6 +80,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
                       d_sample_counter(0ULL),
                       d_preamble_index(0ULL),
                       d_last_valid_preamble(0),
+                      d_received_sample_counter(0),
                       d_frame_type(frame_type),
                       d_CRC_error_counter(0),
                       d_channel(0),
@@ -86,6 +88,7 @@ galileo_telemetry_decoder_gs::galileo_telemetry_decoder_gs(
                       d_stat(0),
                       d_TOW_at_Preamble_ms(0),
                       d_TOW_at_current_symbol_ms(0),
+                      d_received_tow(std::numeric_limits<uint32_t>::max()),
                       d_band('1'),
                       d_sent_tlm_failed_msg(false),
                       d_flag_frame_sync(false),
@@ -324,8 +327,11 @@ void galileo_telemetry_decoder_gs::msg_handler_read_galileo_tow_map(const pmt::p
                         {
                             const auto received_tow_map = wht::any_cast<std::shared_ptr<std::map<uint32_t, std::pair<uint32_t, uint64_t>>>>(pmt::any_ref(msg));
                             const std::pair<uint32_t, uint64_t> received_tow_sample = received_tow_map->at(d_satellite.get_PRN());
-                            std::cout << "Received TOW: " << received_tow_sample.first << '\n';
+                            if (received_tow_sample.first < 604800)
-                            std::cout << "Received sample counter: " << received_tow_sample.second << '\n';
+                                {
+                                    d_received_tow = received_tow_sample.first;
+                                    d_received_sample_counter = received_tow_sample.second;
+                                }
                         }
                 }
             catch (const wht::bad_any_cast &e)
@@ -1105,7 +1111,30 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
                     }
                 case 3:  // CNAV
                     {
-                        // TODO
+                        if (!d_valid_timetag)
+                            {
+                                if (d_received_tow < 604800)
+                                    {
+                                        const int64_t diff = d_received_sample_counter - current_symbol.Tracking_sample_counter;
+                                        bool overflowed = (diff < 0) ^ (d_received_sample_counter < current_symbol.Tracking_sample_counter);
+                                        if (!overflowed && (std::abs(diff) / static_cast<int64_t>(current_symbol.fs) < 1.0))
+                                            {
+                                                d_TOW_at_Preamble_ms = d_received_tow;
+                                                d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_CNAV_SYMBOLS_PER_PAGE * GALILEO_CNAV_PAGE_MS);
+                                                d_E6_TOW_set = true;
+                                            }
+                                    }
+                            }
+                        if (d_E6_TOW_set && d_enable_navdata_monitor && !d_nav_msg_packet.nav_message.empty())
+                            {
+                                d_nav_msg_packet.system = std::string(1, current_symbol.System);
+                                d_nav_msg_packet.signal = std::string(current_symbol.Signal);
+                                d_nav_msg_packet.prn = static_cast<int32_t>(current_symbol.PRN);
+                                d_nav_msg_packet.tow_at_current_symbol_ms = static_cast<int32_t>(d_TOW_at_current_symbol_ms);
+                                const std::shared_ptr<Nav_Message_Packet> tmp_obj = std::make_shared<Nav_Message_Packet>(d_nav_msg_packet);
+                                this->message_port_pub(pmt::mp("Nav_msg_from_TLM"), pmt::make_any(tmp_obj));
+                                d_nav_msg_packet.nav_message = "";
+                            }
                     }
                 }
         }

src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h

@@ -113,6 +113,7 @@ private:
     uint64_t d_sample_counter;
     uint64_t d_preamble_index;
     uint64_t d_last_valid_preamble;
+    uint64_t d_received_sample_counter;
 
     int32_t d_mm;
     int32_t d_codelength;
@@ -132,6 +133,7 @@ private:
     uint32_t d_TOW_at_Preamble_ms;
     uint32_t d_TOW_at_current_symbol_ms;
     uint32_t d_max_symbols_without_valid_frame;
+    uint32_t d_received_tow;
 
     char d_band;  // This variable will store which band we are dealing with (Galileo E1 or E5b)
 

src/core/libs/gnss_sdr_supl_client.cc

@@ -414,6 +414,11 @@ bool Gnss_Sdr_Supl_Client::load_gal_ephemeris_xml(const std::string& file_name)
             gal_ephemeris_map.clear();
             xml >> boost::serialization::make_nvp("GNSS-SDR_gal_ephemeris_map", this->gal_ephemeris_map);
             LOG(INFO) << "Loaded Ephemeris map data with " << this->gal_ephemeris_map.size() << " satellites";
+            // Convert to full GPS week number
+            for (auto& gal_eph_iter : this->gal_ephemeris_map)
+                {
+                    gal_eph_iter.second.WN -= 1024;
+                }
         }
     catch (std::exception& e)
         {