mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-10-13 11:11:52 +00:00
Code Issues Releases Wiki Activity
Files
ef928a5e1f90d1988e59c13754532b2993f2e074
gnss-sdr/src/core/receiver/gnss_flowgraph.cc
T

3149 lines
139 KiB
C++
Raw Blame History

/*!
* \file gnss_flowgraph.cc
* \brief Implementation of a GNSS receiver flow graph
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
* Luis Esteve, 2012. luis(at)epsilon-formacion.com
* Carles Fernandez-Prades, 2014-2020. cfernandez(at)cttc.es
* Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
* Javier Arribas, 2018. javiarribas(at)gmail.com
*
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "gnss_flowgraph.h"
#include "Beidou_B1C.h"
#include "Beidou_B1I.h"
#include "Beidou_B2a.h"
#include "GLONASS_L1_L2_CA.h"
#include "GPS_L1_CA.h"
#include "GPS_L2C.h"
#include "GPS_L5.h"
#include "Galileo_E1.h"
#include "Galileo_E5a.h"
#include "Galileo_E5b.h"
#include "Galileo_E6.h"
#include "Galileo_OSNMA.h"
#include "channel.h"
#include "channel_fsm.h"
#include "channel_interface.h"
#include "configuration_interface.h"
#include "gnss_block_factory.h"
#include "gnss_block_interface.h"
#include "gnss_frequencies.h"
#include "gnss_satellite.h"
#include "gnss_sdr_flags.h"
#include "gnss_sdr_make_unique.h"
#include "gnss_synchro_monitor.h"
#include "nav_message_monitor.h"
#include "qzss.h"
#include "signal_source_interface.h"
#include <boost/lexical_cast.hpp> // for boost::lexical_cast
#include <boost/tokenizer.hpp> // for boost::tokenizer
#include <gnuradio/basic_block.h> // for basic_block
#include <gnuradio/filter/firdes.h> // for gr::filter::firdes
#include <gnuradio/io_signature.h> // for io_signature
#include <gnuradio/top_block.h> // for top_block, make_top_block
#include <pmt/pmt_sugar.h> // for mp
#include <algorithm> // for transform, sort, unique
#include <cmath> // for floor
#include <cstddef> // for size_t
#include <cstdlib> // for exit
#include <exception> // for exception
#include <fstream> // for std::ifstream
#include <iomanip> // for std::setprecision
#include <iostream> // for operator<<
#include <iterator> // for insert_iterator, inserter
#include <limits> // for numeric_limits
#include <map> // for std::map
#include <memory> // for std::shared_ptr
#include <set> // for set
#include <sstream> // for std::stringstream
#include <stdexcept> // for invalid_argument
#include <string> // for std::string
#include <thread> // for std::thread
#include <tuple> // for std::tuple
#include <utility> // for std::move
#include <vector> // for std::vector
#if USE_GLOG_AND_GFLAGS
#include <glog/logging.h>
#else
#include <absl/log/log.h>
#endif
#ifdef GR_GREATER_38
#include <gnuradio/filter/fir_filter_blk.h>
#else
#include <gnuradio/filter/fir_filter_ccf.h>
#endif
#define GNSS_SDR_ARRAY_SIGNAL_CONDITIONER_CHANNELS 8
namespace
{
const auto signal_mapping = std::unordered_map<std::string, std::pair<std::string, std::string>>{
{"1C", {"GPS", "L1"}},
{"2S", {"GPS", "L2"}},
{"L5", {"GPS", "L5"}},
{"1B", {"Galileo", "E1"}},
{"5X", {"Galileo", "E5a"}},
{"7X", {"Galileo", "E5b"}},
{"E6", {"Galileo", "E6"}},
{"B1", {"Beidou", "B1"}},
{"1D", {"Beidou", "B1C"}},
{"5D", {"Beidou", "B2a"}},
{"B3", {"Beidou", "B3"}},
{"1G", {"Glonass", "L1"}},
{"2G", {"Glonass", "L2"}},
{"J1", {"QZSS", "L1"}},
{"J5", {"QZSS", "L5"}},
{"S1", {"SBAS", "L1"}},
};
// E6 CNAV consumes Galileo timing for HAS pages, but it does not seed the TOW map.
// Keeping it off the source side prevents reset placeholders from clearing valid entries.
bool is_galileo_tow_source(const std::string& sig)
{
return sig == "1B" || sig == "5X" || sig == "7X";
}
bool is_galileo_tow_consumer(const std::string& sig)
{
return sig == "E6";
}
} // namespace
GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr<ConfigurationInterface> configuration,
std::shared_ptr<Concurrent_Queue<pmt::pmt_t>> queue) // NOLINT(performance-unnecessary-value-param)
: configuration_(std::move(configuration)),
queue_(std::move(queue)),
satellite_visibility_(std::make_unique<SatelliteVisibility>(configuration_)),
connected_(false),
running_(false),
multiband_(GNSSFlowgraph::is_multiband()),
enable_osnma_rx_(false),
enable_e6_has_rx_(false)
{
enable_fpga_offloading_ = configuration_->property("GNSS-SDR.enable_FPGA", false);
// Skip the assisted acquisition of secondary signals reported as not
// available (GPS SV configuration) or unhealthy by the broadcast data.
// Can be disabled so that acquisition does not rely on that data.
enable_secondary_signal_status_gating_ = configuration_->property("GNSS-SDR.enable_secondary_signal_status_gating", true);
const double acquisition_max_retry_rate_hz = configuration_->property("GNSS-SDR.acquisition_max_retry_rate_hz", 0.0);
// Paced by the receiver time reported by the observables block every
// 100 ms (or every epoch, if longer): higher rates act as that rate.
acquisition_retry_min_interval_s_ = (acquisition_max_retry_rate_hz > 0.0) ? (1.0 / acquisition_max_retry_rate_hz) : 0.0;
init();
}
GNSSFlowgraph::~GNSSFlowgraph()
{
DLOG(INFO) << "GNSSFlowgraph destructor called";
if (connected_)
{
GNSSFlowgraph::disconnect();
}
}
void GNSSFlowgraph::init()
{
/*
* Instantiates the receiver blocks
*/
channels_status_ = channel_status_msg_receiver_make();
if (configuration_->property("Channels_E6.count", 0) > 0)
{
enable_e6_has_rx_ = true;
gal_e6_has_rx_ = galileo_e6_has_msg_receiver_make();
galileo_tow_map_ = galileo_tow_map_make();
}
else
{
gal_e6_has_rx_ = nullptr;
galileo_tow_map_ = nullptr;
}
if (configuration_->property("Channels_1B.count", 0) > 0 && configuration_->property("GNSS-SDR.osnma_enable", true))
{
enable_osnma_rx_ = true;
const auto certFilePath = configuration_->property("GNSS-SDR.osnma_public_key", CRTFILE_DEFAULT);
auto merKleTreePath = configuration_->property("GNSS-SDR.osnma_merkletree", MERKLEFILE_DEFAULT);
if (!configuration_->is_present("GNSS-SDR.osnma_merkletree"))
{
std::ifstream default_merkle_tree(MERKLEFILE_DEFAULT);
if (!default_merkle_tree.good())
{
merKleTreePath.clear();
}
}
std::string osnma_mode = configuration_->property("GNSS-SDR.osnma_mode", std::string(""));
const bool strict_mode = osnma_mode == "strict";
const bool replay_mode = osnma_mode == "replay";
if (!osnma_mode.empty() && !strict_mode && !replay_mode)
{
LOG(WARNING) << "Unknown GNSS-SDR.osnma_mode=" << osnma_mode << ". Falling back to default mode.";
}
osnma_rx_ = osnma_msg_receiver_make(certFilePath, merKleTreePath, strict_mode, replay_mode);
}
else
{
osnma_rx_ = nullptr;
}
// 1. read the number of RF front-ends available (one file_source per RF front-end)
int sources_count_deprecated = configuration_->property("Receiver.sources_count", 1);
sources_count_ = configuration_->property("GNSS-SDR.num_sources", sources_count_deprecated);
// Avoid segmentation fault caused by wrong configuration
if (sources_count_ == 2 && configuration_->property("SignalSource.implementation", std::string("")) == "Multichannel_File_Signal_Source")
{
std::cout << " * Please set GNSS-SDR.num_sources=1 in your configuration file\n";
std::cout << " if you are using the Multichannel_File_Signal_Source implementation.\n";
sources_count_ = 1;
}
int signal_conditioner_ID = 0;
for (int i = 0; i < sources_count_; i++)
{
DLOG(INFO) << "Creating source " << i;
auto check_not_nullptr = block_factory::GetSignalSource(configuration_.get(), queue_.get(), i);
if (!check_not_nullptr)
{
std::cout << "GNSS-SDR program ended.\n";
exit(1);
}
sig_source_.push_back(std::move(check_not_nullptr));
if (enable_fpga_offloading_ == false)
{
auto& src = sig_source_.back();
auto RF_Channels = src->getRfChannels();
if (sources_count_ == 1)
{
std::cout << "RF Channels: " << RF_Channels << '\n';
}
for (auto j = 0U; j < RF_Channels; ++j)
{
sig_conditioner_.push_back(block_factory::GetSignalConditioner(configuration_.get(), signal_conditioner_ID));
signal_conditioner_ID++;
}
}
}
if (sources_count_ != 1 && !enable_fpga_offloading_)
{
std::cout << "RF Channels: " << sources_count_ << '\n';
}
if (!sig_conditioner_.empty())
{
signal_conditioner_connected_ = std::vector<bool>(sig_conditioner_.size(), false);
}
observables_ = block_factory::GetObservables(configuration_.get());
pvt_ = block_factory::GetPVT(configuration_.get());
auto channels = block_factory::GetChannels(configuration_.get(), queue_.get());
channels_count_ = static_cast<int>(channels.size());
for (int i = 0; i < channels_count_; i++)
{
std::shared_ptr<GNSSBlockInterface> chan_ = std::move(channels.at(i));
channels_.push_back(std::dynamic_pointer_cast<ChannelInterface>(chan_));
}
top_block_ = gr::make_top_block("GNSSFlowgraph");
mapStringValues_["1C"] = evGPS_1C;
mapStringValues_["2S"] = evGPS_2S;
mapStringValues_["L5"] = evGPS_L5;
mapStringValues_["1B"] = evGAL_1B;
mapStringValues_["5X"] = evGAL_5X;
mapStringValues_["7X"] = evGAL_7X;
mapStringValues_["E6"] = evGAL_E6;
mapStringValues_["1G"] = evGLO_1G;
mapStringValues_["2G"] = evGLO_2G;
mapStringValues_["B1"] = evBDS_B1;
mapStringValues_["1D"] = evBDS_B1C;
mapStringValues_["5D"] = evBDS_B2A;
mapStringValues_["B3"] = evBDS_B3;
mapStringValues_["J1"] = evQZS_J1;
mapStringValues_["J5"] = evQZS_J5;
mapStringValues_["S1"] = evSBAS_1C;
// fill the signals queue with the satellites ID's to be searched by the acquisition
set_signals_list();
set_channels_state();
DLOG(INFO) << "Blocks instantiated. " << channels_count_ << " channels.";
/*
* Instantiate the receiver monitor block, if required
*/
enable_monitor_ = configuration_->property("Monitor.enable_monitor", false);
if (enable_monitor_)
{
// Retrieve monitor properties
bool enable_protobuf = configuration_->property("Monitor.enable_protobuf", true);
if (configuration_->property("PVT.enable_protobuf", false) == true)
{
enable_protobuf = true;
}
std::string address_string = configuration_->property("Monitor.client_addresses", std::string("127.0.0.1"));
std::vector<std::string> udp_addr_vec = split_string(address_string, '_');
std::sort(udp_addr_vec.begin(), udp_addr_vec.end());
udp_addr_vec.erase(std::unique(udp_addr_vec.begin(), udp_addr_vec.end()), udp_addr_vec.end());
std::string udp_port_string = configuration_->property("Monitor.udp_port", std::string("1234"));
std::vector<std::string> udp_port_vec = split_string(udp_port_string, '_');
std::sort(udp_port_vec.begin(), udp_port_vec.end());
udp_port_vec.erase(std::unique(udp_port_vec.begin(), udp_port_vec.end()), udp_port_vec.end());
// Instantiate monitor object
GnssSynchroMonitor_ = gnss_synchro_make_monitor(channels_count_,
configuration_->property("Monitor.decimation_factor", 1),
udp_port_vec,
udp_addr_vec,
enable_protobuf);
}
/*
* Instantiate the receiver acquisition monitor block, if required
*/
enable_acquisition_monitor_ = configuration_->property("AcquisitionMonitor.enable_monitor", false);
if (enable_acquisition_monitor_)
{
// Retrieve monitor properties
bool enable_protobuf = configuration_->property("AcquisitionMonitor.enable_protobuf", true);
if (configuration_->property("PVT.enable_protobuf", false) == true)
{
enable_protobuf = true;
}
std::string address_string = configuration_->property("AcquisitionMonitor.client_addresses", std::string("127.0.0.1"));
std::vector<std::string> udp_addr_vec = split_string(address_string, '_');
std::sort(udp_addr_vec.begin(), udp_addr_vec.end());
udp_addr_vec.erase(std::unique(udp_addr_vec.begin(), udp_addr_vec.end()), udp_addr_vec.end());
std::string udp_port_string = configuration_->property("AcquisitionMonitor.udp_port", std::string("1235"));
std::vector<std::string> udp_port_vec = split_string(udp_port_string, '_');
std::sort(udp_port_vec.begin(), udp_port_vec.end());
udp_port_vec.erase(std::unique(udp_port_vec.begin(), udp_port_vec.end()), udp_port_vec.end());
GnssSynchroAcquisitionMonitor_ = gnss_synchro_make_monitor(channels_count_,
configuration_->property("AcquisitionMonitor.decimation_factor", 1),
udp_port_vec,
udp_addr_vec,
enable_protobuf);
}
/*
* Instantiate the receiver tracking monitor block, if required
*/
enable_tracking_monitor_ = configuration_->property("TrackingMonitor.enable_monitor", false);
if (enable_tracking_monitor_)
{
// Retrieve monitor properties
bool enable_protobuf = configuration_->property("TrackingMonitor.enable_protobuf", true);
if (configuration_->property("PVT.enable_protobuf", false) == true)
{
enable_protobuf = true;
}
std::string address_string = configuration_->property("TrackingMonitor.client_addresses", std::string("127.0.0.1"));
std::vector<std::string> udp_addr_vec = split_string(address_string, '_');
std::sort(udp_addr_vec.begin(), udp_addr_vec.end());
udp_addr_vec.erase(std::unique(udp_addr_vec.begin(), udp_addr_vec.end()), udp_addr_vec.end());
std::string udp_port_string = configuration_->property("TrackingMonitor.udp_port", std::string("1236"));
std::vector<std::string> udp_port_vec = split_string(udp_port_string, '_');
std::sort(udp_port_vec.begin(), udp_port_vec.end());
udp_port_vec.erase(std::unique(udp_port_vec.begin(), udp_port_vec.end()), udp_port_vec.end());
GnssSynchroTrackingMonitor_ = gnss_synchro_make_monitor(channels_count_,
configuration_->property("TrackingMonitor.decimation_factor", 1),
udp_port_vec,
udp_addr_vec,
enable_protobuf);
}
/*
* Instantiate the receiver nav message monitor block, if required
*/
enable_navdata_monitor_ = configuration_->property("NavDataMonitor.enable_monitor", false);
if (enable_navdata_monitor_)
{
// Retrieve monitor properties
std::string address_string = configuration_->property("NavDataMonitor.client_addresses", std::string("127.0.0.1"));
std::vector<std::string> udp_addr_vec = split_string(address_string, '_');
std::sort(udp_addr_vec.begin(), udp_addr_vec.end());
udp_addr_vec.erase(std::unique(udp_addr_vec.begin(), udp_addr_vec.end()), udp_addr_vec.end());
NavDataMonitor_ = nav_message_monitor_make(udp_addr_vec, configuration_->property("NavDataMonitor.port", 1237));
}
}
void GNSSFlowgraph::start()
{
if (running_)
{
LOG(WARNING) << "Already running";
return;
}
try
{
top_block_->start();
}
catch (const std::exception& e)
{
LOG(ERROR) << "Unable to start flowgraph: " << e.what();
print_help();
return;
}
if (enable_fpga_offloading_ == true)
{
// start the DMA if the receiver is in post-processing mode
if (configuration_->property(sig_source_.at(0)->role() + ".switch_position", 0) == 0)
{
sig_source_.at(0)->start();
}
}
running_ = true;
}
void GNSSFlowgraph::stop()
{
for (const auto& chan : channels_)
{
chan->stop_channel(); // stop the acquisition or tracking operation
}
top_block_->stop();
if (enable_fpga_offloading_ == false)
{
top_block_->wait();
}
running_ = false;
}
void GNSSFlowgraph::wait()
{
if (!running_)
{
LOG(WARNING) << "Can't apply wait. Flowgraph is not running";
return;
}
top_block_->wait();
DLOG(INFO) << "Flowgraph finished calculations";
running_ = false;
}
void GNSSFlowgraph::connect()
{
// Connects the blocks in the flow graph
LOG(INFO) << "Connecting flowgraph";
if (connected_)
{
LOG(WARNING) << "flowgraph already connected";
return;
}
#if ENABLE_FPGA
if (enable_fpga_offloading_ == true)
{
if (connect_fpga_flowgraph() != 0)
{
std::cerr << "Unable to connect flowgraph with FPGA off-loading\n";
print_help();
return;
}
}
else
{
if (connect_desktop_flowgraph() != 0)
{
std::cerr << "Unable to connect flowgraph\n";
print_help();
return;
}
}
#else
if (connect_desktop_flowgraph() != 0)
{
std::cerr << "Unable to connect flowgraph\n";
print_help();
return;
}
#endif
connected_ = true;
LOG(INFO) << "Flowgraph connected";
top_block_->dump();
}
void GNSSFlowgraph::disconnect()
{
LOG(INFO) << "Disconnecting flowgraph";
if (!connected_)
{
LOG(INFO) << "Flowgraph was not connected";
return;
}
connected_ = false;
try
{
top_block_->disconnect_all();
}
catch (const std::exception& e)
{
LOG(INFO) << "Problem disconnecting the flowgraph: " << e.what();
}
LOG(INFO) << "Flowgraph disconnected";
}
int GNSSFlowgraph::connect_desktop_flowgraph()
{
// Connect blocks to the top_block
const int max_channels_in_acq = configuration_->property("Channels.in_acquisition", 0);
if (max_channels_in_acq > channels_count_)
{
help_hint_ += " * The maximum number of channels with concurrent signal acquisition is set to Channels.in_acquisition=" + std::to_string(max_channels_in_acq) + ",\n";
help_hint_ += " but the total number of channels is set to " + std::to_string(channels_count_) + ".\n";
help_hint_ += " Please set Channels.in_acquisition to " + std::to_string(channels_count_) + " or lower, or increment the number of channels in your configuration file.\n";
return 1;
}
if (connect_signal_sources() != 0)
{
return 1;
}
if (connect_signal_conditioners() != 0)
{
return 1;
}
if (connect_channels() != 0)
{
return 1;
}
if (connect_observables() != 0)
{
return 1;
}
if (connect_pvt() != 0)
{
return 1;
}
// Connect blocks between them to form the flow graph
if (connect_signal_sources_to_signal_conditioners() != 0)
{
return 1;
}
if (connect_sample_counter() != 0)
{
return 1;
}
if (connect_signal_conditioners_to_channels() != 0)
{
return 1;
}
if (connect_channels_to_observables() != 0)
{
return 1;
}
check_signal_conditioners();
if (assign_channels() != 0)
{
return 1;
}
if (connect_observables_to_pvt() != 0)
{
return 1;
}
if (connect_monitors() != 0)
{
return 1;
}
if (enable_e6_has_rx_)
{
if (connect_gal_e6_has() != 0)
{
return 1;
}
if (connect_galileo_tow_map() != 0)
{
return 1;
}
}
if (enable_osnma_rx_)
{
if (connect_osnma() != 0)
{
return 1;
}
}
// Activate acquisition in enabled channels
std::lock_guard<std::mutex> lock(signal_list_mutex_);
for (int i = 0; i < channels_count_; i++)
{
LOG(INFO) << "Channel " << i << " assigned to " << channels_.at(i)->get_signal();
if (channels_state_[i] == 2)
{
#if ENABLE_FPGA
if (enable_fpga_offloading_)
{
// create a task for the FPGA such that it doesn't stop the flow
std::thread tmp_thread(&ChannelInterface::start_acquisition, channels_[i]);
tmp_thread.detach();
}
else
{
channels_.at(i)->start_acquisition();
}
#else
channels_.at(i)->start_acquisition();
#endif
LOG(INFO) << "Channel " << i << " connected to observables and ready for acquisition";
}
else
{
LOG(INFO) << "Channel " << i << " connected to observables in standby mode";
}
}
LOG(INFO) << "The GNU Radio flowgraph for the current GNSS-SDR configuration has been successfully connected";
return 0;
}
#if ENABLE_FPGA
int GNSSFlowgraph::connect_fpga_flowgraph()
{
const int max_channels_in_acq = configuration_->property("Channels.in_acquisition", 0);
if (max_channels_in_acq != 1)
{
help_hint_ += " * The maximum number of channels with concurrent signal acquisition is set to Channels.in_acquisition=" + std::to_string(max_channels_in_acq) + ",\n";
help_hint_ += " but it must be set to 1 in the FPGA flow graph.\n";
help_hint_ += " Please set Channels.in_acquisition=1 in your configuration file.\n";
return 1;
}
// Check that the Signal Source has been instantiated successfully
for (auto& src : sig_source_)
{
if (src == nullptr)
{
help_hint_ += " * Check implementation name for SignalSource block.\n";
help_hint_ += " Signal Source block implementation for FPGA off-loading should be Ad9361_Signal_Source_Fpga or Fpga_DMA_2Signal_Source\n";
return 1;
}
if (src->item_size() == 0)
{
help_hint_ += " * The global configuration parameter GNSS-SDR.enable_FPGA is set to true,\n";
help_hint_ += " but gnss-sdr does not appear to be executed in an FPGA-equipped platform,\n";
help_hint_ += " or there are some required files that are missing.\n";
return 1;
}
}
// Connect blocks to the top_block
if (connect_channels() != 0)
{
return 1;
}
if (connect_observables() != 0)
{
return 1;
}
if (connect_pvt() != 0)
{
return 1;
}
DLOG(INFO) << "Blocks connected internally to the top_block";
// Connect the counter
if (connect_fpga_sample_counter() != 0)
{
return 1;
}
if (connect_channels_to_observables() != 0)
{
return 1;
}
if (assign_channels() != 0)
{
return 1;
}
if (connect_observables_to_pvt() != 0)
{
return 1;
}
if (connect_monitors() != 0)
{
return 1;
}
if (enable_e6_has_rx_)
{
if (connect_gal_e6_has() != 0)
{
return 1;
}
if (connect_galileo_tow_map() != 0)
{
return 1;
}
}
if (enable_osnma_rx_)
{
if (connect_osnma() != 0)
{
return 1;
}
}
check_desktop_conf_in_fpga_env();
LOG(INFO) << "The GNU Radio flowgraph for the current GNSS-SDR configuration with FPGA off-loading has been successfully connected";
return 0;
}
#endif
int GNSSFlowgraph::connect_signal_sources()
{
for (int i = 0; i < sources_count_; i++)
{
if (sig_source_.at(i) != nullptr)
{
try
{
sig_source_.at(i)->connect(top_block_);
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect signal source block " << i << " internally: " << e.what();
top_block_->disconnect_all();
return 1;
}
}
else
{
help_hint_ += " * Check implementation name for SignalSource" + (i == 0 ? " " : (std::to_string(i) + " ")) + "block\n";
help_hint_ += " Signal Source blocks documentation at https://gnss-sdr.org/docs/sp-blocks/signal-source/\n";
top_block_->disconnect_all();
return 1;
}
}
DLOG(INFO) << "Signal Source blocks successfully connected to the top_block";
return 0;
}
int GNSSFlowgraph::connect_signal_conditioners()
{
int error = 1; // this should be bool (true)
try
{
for (auto& sig : sig_conditioner_)
{
if (sig == nullptr)
{
help_hint_ += " * The Signal_Conditioner implementation set in the configuration file does not exist.\n";
help_hint_ += " Check the Signal Conditioner documentation at https://gnss-sdr.org/docs/sp-blocks/signal-conditioner/\n";
return error;
}
sig->connect(top_block_);
}
DLOG(INFO) << "Signal Conditioner blocks successfully connected to the top_block";
error = 0; // false
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect signal conditioner block internally: " << e.what();
top_block_->disconnect_all();
std::string reported_error(e.what());
if (std::string::npos != reported_error.find(std::string("itemsize mismatch")))
{
std::string replace_me("copy");
size_t pos = reported_error.find(replace_me);
while (pos != std::string::npos)
{
size_t len = replace_me.length();
reported_error.replace(pos, len, "Pass_Through");
pos = reported_error.find(replace_me, pos + 1);
}
help_hint_ += " * Blocks within the Signal Conditioner are connected with mismatched input/output item size\n";
help_hint_ += " Reported error: " + reported_error + '\n';
help_hint_ += " Check the Signal Conditioner documentation at https://gnss-sdr.org/docs/sp-blocks/signal-conditioner/\n";
}
if (std::string::npos != reported_error.find(std::string("DataTypeAdapter")))
{
help_hint_ += " * The DataTypeAdapter implementation set in the configuration file does not exist\n";
help_hint_ += " Check the DataTypeAdapter documentation at https://gnss-sdr.org/docs/sp-blocks/data-type-adapter/\n";
}
if (std::string::npos != reported_error.find(std::string("InputFilter")))
{
if (std::string::npos != reported_error.find(std::string("itemsize mismatch")))
{
help_hint_ += " * The configured InputFilter input/output item types are not well defined.\n";
}
else
{
help_hint_ += " * The InputFilter implementation set in the configuration file does not exist\n";
}
help_hint_ += " Check the InputFilter documentation at https://gnss-sdr.org/docs/sp-blocks/input-filter/\n";
}
if (std::string::npos != reported_error.find(std::string("Resampler")))
{
if (std::string::npos != reported_error.find(std::string("itemsize mismatch")))
{
help_hint_ += " * The configured Resampler item type is not well defined.\n";
}
else
{
help_hint_ += " * The Resampler implementation set in the configuration file does not exist\n";
}
help_hint_ += " Check the Resampler documentation at https://gnss-sdr.org/docs/sp-blocks/resampler/\n";
}
}
return error;
}
int GNSSFlowgraph::connect_channels()
{
if (channels_count_ <= 0)
{
LOG(ERROR) << "No channels have been assigned.";
help_hint_ += " * No channels have been assigned, check your configuration file.\n";
help_hint_ += " At least one of the Channels_XX.count must be > 0.\n";
help_hint_ += " Channels documentation at https://gnss-sdr.org/docs/sp-blocks/channels/\n";
top_block_->disconnect_all();
return 1;
}
for (int i = 0; i < channels_count_; i++)
{
if (channels_.at(i) != nullptr)
{
try
{
channels_.at(i)->connect(top_block_);
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect channel " << i << " internally: " << e.what();
top_block_->disconnect_all();
return 1;
}
}
else
{
LOG(ERROR) << "Can't connect channel " << i << " internally";
help_hint_ += " * Check your configuration for Channel" + std::to_string(i) + " inner blocks.\n";
help_hint_ += " Acquisition blocks documentation at https://gnss-sdr.org/docs/sp-blocks/acquisition/\n";
help_hint_ += " Tracking blocks documentation at https://gnss-sdr.org/docs/sp-blocks/tracking/\n";
help_hint_ += " Telemetry Decoder blocks documentation at https://gnss-sdr.org/docs/sp-blocks/telemetry-decoder/\n";
top_block_->disconnect_all();
return 1;
}
}
DLOG(INFO) << "Channel blocks successfully connected to the top_block";
return 0;
}
int GNSSFlowgraph::connect_observables()
{
if (observables_ == nullptr)
{
help_hint_ += " * Check implementation name for the Observables block\n";
help_hint_ += " Observables block documentation at https://gnss-sdr.org/docs/sp-blocks/observables/\n";
top_block_->disconnect_all();
return 1;
}
try
{
observables_->connect(top_block_);
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect observables block internally: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "Observables block successfully connected to the top_block";
return 0;
}
int GNSSFlowgraph::connect_pvt()
{
if (pvt_ == nullptr)
{
help_hint_ += " * Check implementation name for the PVT block\n";
help_hint_ += " PVT block documentation at https://gnss-sdr.org/docs/sp-blocks/pvt/\n";
top_block_->disconnect_all();
return 1;
}
try
{
pvt_->connect(top_block_);
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect PVT block internally: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "PVT block successfully connected to the top_block";
return 0;
}
int GNSSFlowgraph::connect_galileo_tow_map()
{
try
{
for (int i = 0; i < channels_count_; i++)
{
const std::string sig = channels_.at(i)->get_signal().get_signal_str();
if (is_galileo_tow_source(sig))
{
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("TOW_from_TLM"), galileo_tow_map_, pmt::mp("TOW_from_TLM"));
}
if (is_galileo_tow_consumer(sig))
{
top_block_->msg_connect(galileo_tow_map_, pmt::mp("TOW_to_TLM"), channels_.at(i)->get_right_block(), pmt::mp("TOW_to_TLM"));
}
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect The Galileo TOW map internally: " << e.what();
top_block_->disconnect_all();
return 1;
}
return 0;
}
int GNSSFlowgraph::connect_sample_counter()
{
// connect the sample counter to the Signal Conditioner
// connect the sample counter to Observables
try
{
const double fs = static_cast<double>(configuration_->property("GNSS-SDR.internal_fs_sps", 0));
if (fs == 0.0)
{
LOG(WARNING) << "Set GNSS-SDR.internal_fs_sps in configuration file";
std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file\n";
throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration"));
}
const int observable_interval_ms = configuration_->property("GNSS-SDR.observable_interval_ms", 20);
ch_out_sample_counter_ = gnss_sdr_make_sample_counter(fs, observable_interval_ms, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0));
top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter_, 0);
top_block_->connect(ch_out_sample_counter_, 0, observables_->get_left_block(), channels_count_); // extra port for the sample counter pulse
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect sample counter: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "sample counter successfully connected to Signal Conditioner and Observables blocks";
return 0;
}
#if ENABLE_FPGA
int GNSSFlowgraph::connect_fpga_sample_counter()
{
// create a hardware-defined gnss_synchro pulse for the observables block
try
{
const double fs = static_cast<double>(configuration_->property("GNSS-SDR.internal_fs_sps", 0));
if (fs == 0.0)
{
LOG(WARNING) << "Set GNSS-SDR.internal_fs_sps in configuration file";
std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file\n";
throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration"));
}
const int observable_interval_ms = configuration_->property("GNSS-SDR.observable_interval_ms", 20);
ch_out_fpga_sample_counter_ = gnss_sdr_make_fpga_sample_counter(fs, observable_interval_ms);
top_block_->connect(ch_out_fpga_sample_counter_, 0, observables_->get_left_block(), channels_count_); // extra port for the sample counter pulse
}
catch (const std::exception& e)
{
std::string reported_error(e.what());
if (std::string::npos != reported_error.find(std::string("filesystem")))
{
help_hint_ += " * The global configuration parameter GNSS-SDR.enable_FPGA is set to true,\n";
help_hint_ += " but gnss-sdr does not appear to be executed in an FPGA-equipped platform.\n";
}
else
{
LOG(ERROR) << reported_error;
}
top_block_->disconnect_all();
return 1;
}
LOG(INFO) << "FPGA sample counter successfully connected";
return 0;
}
#endif
int GNSSFlowgraph::connect_signal_sources_to_signal_conditioners()
{
if (enable_fpga_offloading_)
{
help_hint_ += " * The global configuration parameter GNSS-SDR.enable_FPGA is set to true,\n";
help_hint_ += " but gnss-sdr was not compiled with the -DENABLE_FPGA=ON building option.\n";
top_block_->disconnect_all();
return 1;
}
unsigned int signal_conditioner_ID = 0;
for (int i = 0; i < sources_count_; i++)
{
try
{
auto& src = sig_source_.at(i);
// TODO: Remove this array implementation and create generic multistream connector
// (if a signal source has more than 1 stream, then connect it to the multistream signal conditioner)
if (src->implementation() == "Raw_Array_Signal_Source")
{
// Multichannel Array
std::cout << "ARRAY MODE\n";
for (int j = 0; j < GNSS_SDR_ARRAY_SIGNAL_CONDITIONER_CHANNELS; j++)
{
std::cout << "connecting ch " << j << '\n';
top_block_->connect(src->get_right_block(), j, sig_conditioner_.at(i)->get_left_block(), j);
}
}
else
{
auto RF_Channels = src->getRfChannels();
for (auto j = 0U; j < RF_Channels; ++j)
{
// Connect the multichannel signal source to multiple signal conditioners
// GNURADIO max_streams=-1 means infinite ports!
size_t output_size = src->get_right_block()->output_signature()->sizeof_stream_item(0);
size_t input_size = sig_conditioner_.at(signal_conditioner_ID)->get_left_block()->input_signature()->sizeof_stream_item(0);
// Check configuration inconsistencies
if (output_size != input_size)
{
help_hint_ += " * The Signal Source implementation " + src->implementation() + " has an output with a ";
help_hint_ += src->role() + ".item_size of " + std::to_string(output_size);
help_hint_ += " bytes, but it is connected to the Signal Conditioner implementation ";
help_hint_ += sig_conditioner_.at(signal_conditioner_ID)->implementation() + " with input item size of " + std::to_string(input_size) + " bytes.\n";
help_hint_ += " Output ports must be connected to input ports with the same item size.\n";
top_block_->disconnect_all();
return 1;
}
if (src->get_right_block()->output_signature()->max_streams() > 1 || src->get_right_block()->output_signature()->max_streams() == -1)
{
if (sig_conditioner_.size() > signal_conditioner_ID)
{
LOG(INFO) << "connecting sig_source_ " << i << " stream " << j << " to conditioner " << signal_conditioner_ID;
top_block_->connect(src->get_right_block(), j, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0);
}
}
else
{
if (j == 0 || !src->get_right_block(j))
{
// RF_channel 0 backward compatibility with single channel sources
LOG(INFO) << "connecting sig_source_ " << i << " stream " << 0 << " to conditioner " << signal_conditioner_ID;
top_block_->connect(src->get_right_block(), 0, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0);
}
else
{
// Multiple channel sources using multiple output blocks of single channel (requires RF_channel selector in call)
LOG(INFO) << "connecting sig_source_ " << i << " stream " << j << " to conditioner " << signal_conditioner_ID;
top_block_->connect(src->get_right_block(j), 0, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0);
}
}
signal_conditioner_ID++;
}
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect SignalSource" << (i == 0 ? " " : (std::to_string(i) + " ")) << "to SignalConditioner" << (i == 0 ? " " : (std::to_string(i) + " ")) << ": " << e.what();
std::string reported_error(e.what());
if (std::string::npos != reported_error.find(std::string("itemsize mismatch")))
{
std::string replace_me("copy");
size_t pos = reported_error.find(replace_me);
while (pos != std::string::npos)
{
size_t len = replace_me.length();
reported_error.replace(pos, len, "Pass_Through");
pos = reported_error.find(replace_me, pos + 1);
}
help_hint_ += " * The SignalSource output item size and the SignalConditioner input item size are mismatched\n";
help_hint_ += " Reported error: " + reported_error + '\n';
}
top_block_->disconnect_all();
return 1;
}
}
DLOG(INFO) << "Signal source(s) successfully connected to signal conditioner(s)";
return 0;
}
int GNSSFlowgraph::connect_signal_conditioners_to_channels()
{
for (int i = 0; i < channels_count_; i++)
{
int selected_signal_conditioner_ID = 0;
const bool use_acq_resampler = configuration_->property("GNSS-SDR.use_acquisition_resampler", false);
const uint32_t fs = configuration_->property("GNSS-SDR.internal_fs_sps", 0);
try
{
selected_signal_conditioner_ID = configuration_->property("Channels_" + channels_.at(i)->get_signal().get_signal_str() + ".RF_channel_ID", 0);
selected_signal_conditioner_ID = configuration_->property("Channel" + std::to_string(i) + ".RF_channel_ID", selected_signal_conditioner_ID);
}
catch (const std::exception& e)
{
LOG(WARNING) << e.what();
}
try
{
// Enable automatic resampler for the acquisition, if required
if (use_acq_resampler == true)
{
// create acquisition resamplers if required
double resampler_ratio = 1.0;
double acq_fs = fs;
// find the signal associated to this channel
switch (mapStringValues_[channels_.at(i)->get_signal().get_signal_str()])
{
case evGPS_1C:
acq_fs = GPS_L1_CA_OPT_ACQ_FS_SPS;
break;
case evGPS_2S:
acq_fs = GPS_L2C_OPT_ACQ_FS_SPS;
break;
case evGPS_L5:
acq_fs = GPS_L5_OPT_ACQ_FS_SPS;
break;
case evSBAS_1C:
acq_fs = GPS_L1_CA_OPT_ACQ_FS_SPS;
break;
case evGAL_1B:
acq_fs = GALILEO_E1_OPT_ACQ_FS_SPS;
break;
case evGAL_5X:
acq_fs = GALILEO_E5A_OPT_ACQ_FS_SPS;
break;
case evGAL_7X:
acq_fs = GALILEO_E5B_OPT_ACQ_FS_SPS;
break;
case evGAL_E6:
acq_fs = GALILEO_E6_OPT_ACQ_FS_SPS;
break;
case evBDS_B1:
acq_fs = BEIDOU_B1I_OPT_ACQ_FS_SPS;
break;
case evBDS_B1C:
acq_fs = BEIDOU_B1C_OPT_ACQ_FS_SPS;
break;
case evBDS_B2A:
acq_fs = BEIDOU_B2A_OPT_ACQ_FS_SPS;
break;
case evGLO_1G:
case evGLO_2G:
case evBDS_B3:
acq_fs = fs;
break;
case evQZS_J1:
acq_fs = QZSS_L1_OPT_ACQ_FS_SPS;
break;
case evQZS_J5:
acq_fs = QZSS_L5_OPT_ACQ_FS_SPS;
break;
default:
break;
}
if (acq_fs < fs)
{
// check if the resampler is already created for the channel system/signal and for the specific RF Channel
const std::string map_key = channels_.at(i)->get_signal().get_signal_str() + std::to_string(selected_signal_conditioner_ID);
resampler_ratio = static_cast<double>(fs) / acq_fs;
int decimation = floor(resampler_ratio);
while (fs % decimation > 0)
{
decimation--;
};
const double acq_fs_decimated = static_cast<double>(fs) / static_cast<double>(decimation);
if (decimation > 1)
{
// create a FIR low pass filter
std::vector<float> taps = gr::filter::firdes::low_pass(1.0,
fs,
acq_fs_decimated / 2.1,
acq_fs_decimated / 2);
gr::basic_block_sptr fir_filter_ccf_ = gr::filter::fir_filter_ccf::make(decimation, taps);
std::pair<std::map<std::string, gr::basic_block_sptr>::iterator, bool> ret;
ret = acq_resamplers_.insert(std::pair<std::string, gr::basic_block_sptr>(map_key, fir_filter_ccf_));
if (ret.second == true)
{
top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0,
acq_resamplers_.at(map_key), 0);
LOG(INFO) << "Created "
<< channels_.at(i)->get_signal().get_signal_str()
<< " acquisition resampler for RF channel " << std::to_string(selected_signal_conditioner_ID) << " with " << taps.size() << " taps and decimation factor of " << decimation;
}
else
{
LOG(INFO) << "Found existing "
<< channels_.at(i)->get_signal().get_signal_str()
<< " acquisition resampler for RF channel " << std::to_string(selected_signal_conditioner_ID) << " with " << taps.size() << " taps and decimation factor of " << decimation;
}
top_block_->connect(acq_resamplers_.at(map_key), 0,
channels_.at(i)->get_left_block_acq(), 0);
std::shared_ptr<Channel> channel_ptr = std::dynamic_pointer_cast<Channel>(channels_.at(i));
channel_ptr->acquisition()->set_resampler_latency((taps.size() - 1) / 2);
}
else
{
LOG(INFO) << "Disabled acquisition resampler because the input sampling frequency is too low";
// resampler not required!
top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0,
channels_.at(i)->get_left_block_acq(), 0);
}
}
else
{
LOG(INFO) << "Disabled acquisition resampler because the input sampling frequency is too low";
top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0,
channels_.at(i)->get_left_block_acq(), 0);
}
}
else
{
top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0,
channels_.at(i)->get_left_block_acq(), 0);
}
top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0,
channels_.at(i)->get_left_block_trk(), 0);
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect signal conditioner " << selected_signal_conditioner_ID << " to channel " << i << ": " << e.what();
top_block_->disconnect_all();
return 1;
}
signal_conditioner_connected_.at(selected_signal_conditioner_ID) = true; // annotate that this signal conditioner is connected
DLOG(INFO) << "Signal conditioner " << selected_signal_conditioner_ID << " successfully connected to channel " << i;
}
return 0;
}
int GNSSFlowgraph::connect_channels_to_observables()
{
for (int i = 0; i < channels_count_; i++)
{
try
{
top_block_->connect(channels_.at(i)->get_right_block(), 0,
observables_->get_left_block(), i);
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect channel " << i << " to observables: " << e.what();
top_block_->disconnect_all();
return 1;
}
}
DLOG(INFO) << "Channel blocks successfully connected to the Observables block";
return 0;
}
int GNSSFlowgraph::connect_observables_to_pvt()
{
// Connect the observables output of each channel to the PVT block
try
{
for (int i = 0; i < channels_count_; i++)
{
top_block_->connect(observables_->get_right_block(), i, pvt_->get_left_block(), i);
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("telemetry"), pvt_->get_left_block(), pmt::mp("telemetry"));
// experimental Vector Tracking Loop (VTL) messages from PVT to Tracking blocks
// not supported by all tracking algorithms
pmt::pmt_t ports_in = channels_.at(i)->get_left_block_trk()->message_ports_in();
for (size_t n = 0; n < pmt::length(ports_in); n++)
{
// std::cout << "pmt: " << pmt::symbol_to_string(pmt::vector_ref(ports_in, n)) << "\n";
if (pmt::symbol_to_string(pmt::vector_ref(ports_in, n)) == "pvt_to_trk")
{
top_block_->msg_connect(pvt_->get_left_block(), pmt::mp("pvt_to_trk"), channels_.at(i)->get_left_block_trk(), pmt::mp("pvt_to_trk"));
LOG(INFO) << "pvt_to_trk message port connected in " << channels_.at(i)->implementation();
}
}
}
top_block_->msg_connect(observables_->get_right_block(), pmt::mp("status"), channels_status_, pmt::mp("status"));
top_block_->msg_connect(pvt_->get_left_block(), pmt::mp("pvt_to_observables"), observables_->get_right_block(), pmt::mp("pvt_to_observables"));
top_block_->msg_connect(pvt_->get_left_block(), pmt::mp("status"), channels_status_, pmt::mp("status"));
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect observables to PVT: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "Observables successfully connected to the PVT block";
return 0;
}
int GNSSFlowgraph::connect_gnss_synchro_monitor()
{
try
{
for (int i = 0; i < channels_count_; i++)
{
top_block_->connect(observables_->get_right_block(), i, GnssSynchroMonitor_, i);
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect observables to Monitor block: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "gnss_synchro_monitor successfully connected to Observables block";
return 0;
}
int GNSSFlowgraph::connect_acquisition_monitor()
{
try
{
for (int i = 0; i < channels_count_; i++)
{
top_block_->connect(channels_.at(i)->get_right_block_acq(), 0, GnssSynchroAcquisitionMonitor_, i);
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect acquisition intermediate outputs to Monitor block: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "acquisition_monitor successfully connected to Channel blocks";
return 0;
}
int GNSSFlowgraph::connect_tracking_monitor()
{
try
{
for (int i = 0; i < channels_count_; i++)
{
top_block_->connect(channels_.at(i)->get_right_block_trk(), 0, GnssSynchroTrackingMonitor_, i);
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect tracking outputs to Monitor block: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "tracking_monitor successfully connected to Channel blocks";
return 0;
}
int GNSSFlowgraph::connect_navdata_monitor()
{
try
{
for (int i = 0; i < channels_count_; i++)
{
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("Nav_msg_from_TLM"), NavDataMonitor_, pmt::mp("Nav_msg_from_TLM"));
}
if (enable_e6_has_rx_)
{
gal_e6_has_rx_->set_enable_navdata_monitor(true);
top_block_->msg_connect(gal_e6_has_rx_, pmt::mp("Nav_msg_from_TLM"), NavDataMonitor_, pmt::mp("Nav_msg_from_TLM"));
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect TlM outputs to Monitor block: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "navdata monitor successfully connected to Channel blocks";
return 0;
}
int GNSSFlowgraph::connect_monitors()
{
// GNSS SYNCHRO MONITOR
if (enable_monitor_)
{
if (connect_gnss_synchro_monitor() != 0)
{
return 1;
}
}
// GNSS SYNCHRO ACQUISITION MONITOR
if (enable_acquisition_monitor_)
{
if (connect_acquisition_monitor() != 0)
{
return 1;
}
}
// GNSS SYNCHRO TRACKING MONITOR
if (enable_tracking_monitor_)
{
if (connect_tracking_monitor() != 0)
{
return 1;
}
}
// NAVIGATION DATA MONITOR
if (enable_navdata_monitor_)
{
if (connect_navdata_monitor() != 0)
{
return 1;
}
}
return 0;
}
int GNSSFlowgraph::connect_osnma()
{
try
{
bool gal_e1_channels = false;
for (int i = 0; i < channels_count_; i++)
{
const std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str();
switch (mapStringValues_[gnss_signal])
{
case evGAL_1B:
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("OSNMA_from_TLM"), osnma_rx_, pmt::mp("OSNMA_from_TLM"));
gal_e1_channels = true;
break;
default:
break;
}
}
if (gal_e1_channels == true)
{
top_block_->msg_connect(osnma_rx_, pmt::mp("OSNMA_to_PVT"), pvt_->get_left_block(), pmt::mp("OSNMA_to_PVT"));
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect Galileo OSNMA msg ports: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "Galileo OSNMA message ports connected";
return 0;
}
int GNSSFlowgraph::connect_gal_e6_has()
{
try
{
bool gal_e6_channels = false;
for (int i = 0; i < channels_count_; i++)
{
const std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str();
switch (mapStringValues_[gnss_signal])
{
case evGAL_E6:
top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("E6_HAS_from_TLM"), gal_e6_has_rx_, pmt::mp("E6_HAS_from_TLM"));
gal_e6_channels = true;
break;
default:
break;
}
}
if (gal_e6_channels == true)
{
top_block_->msg_connect(gal_e6_has_rx_, pmt::mp("E6_HAS_to_PVT"), pvt_->get_left_block(), pmt::mp("E6_HAS_to_PVT"));
}
}
catch (const std::exception& e)
{
LOG(ERROR) << "Can't connect Galileo E6 HAS msg ports: " << e.what();
top_block_->disconnect_all();
return 1;
}
DLOG(INFO) << "Galileo E6 HAS message ports connected";
return 0;
}
void GNSSFlowgraph::check_signal_conditioners()
{
// check for unconnected signal conditioners and connect null_sinks
// in order to provide configuration flexibility to multiband files or signal sources
for (size_t n = 0; n < sig_conditioner_.size(); n++)
{
if (signal_conditioner_connected_.at(n) == false)
{
null_sinks_.push_back(gr::blocks::null_sink::make(sizeof(gr_complex)));
top_block_->connect(sig_conditioner_.at(n)->get_right_block(), 0,
null_sinks_.back(), 0);
LOG(INFO) << "Null sink connected to signal conditioner " << n << " due to lack of connection to any channel\n";
}
}
}
int GNSSFlowgraph::assign_channels()
{
// Put channels fixed to a given satellite at the beginning of the vector, then the rest
std::vector<unsigned int> vector_of_channels;
for (int i = 0; i < channels_count_; i++)
{
unsigned int sat = 0;
try
{
sat = configuration_->property("Channel" + std::to_string(i) + ".satellite", 0);
}
catch (const std::exception& e)
{
LOG(WARNING) << e.what();
}
if (sat == 0)
{
vector_of_channels.push_back(i);
}
else
{
auto it = vector_of_channels.begin();
it = vector_of_channels.insert(it, i);
}
}
#if CXX_LESS_THAN_17
for (const auto& entry : available_signals_map_)
{
const auto& signal_str = entry.first;
const auto& available_signals = entry.second;
const auto& mapping = signal_mapping.at(signal_str);
const auto& gnss_system_str = mapping.first;
const auto& signal_pretty_str = mapping.second;
#else // structured bindings available from C++17
for (const auto& [signal_str, available_signals] : available_signals_map_)
{
const auto& [gnss_system_str, signal_pretty_str] = signal_mapping.at(signal_str);
#endif
const auto channel_count_option = "Channels_" + signal_str + ".count";
const auto channel_count = configuration_->property(channel_count_option, uint64_t(0ULL));
auto max_sat_count = available_signals.size();
if (gnss_system_str == "Glonass")
{
max_sat_count += 8; // satellites sharing same frequency number
}
if (channel_count > max_sat_count - 1)
{
help_hint_ += " * The number of " + gnss_system_str;
help_hint_ += " " + signal_pretty_str;
help_hint_ += " channels is set to " + channel_count_option;
help_hint_ += "=" + std::to_string(channel_count);
help_hint_ += " but the maximum number of available " + gnss_system_str;
help_hint_ += " satellites is " + std::to_string(max_sat_count) + ".\n";
help_hint_ += " Please set " + channel_count_option + "=";
help_hint_ += std::to_string(max_sat_count - 1) + " or lower in your configuration file.\n";
top_block_->disconnect_all();
return 1;
}
}
// Assign satellites to channels in the initialization
for (unsigned int& i : vector_of_channels)
{
const std::string gnss_signal_str = channels_.at(i)->get_signal().get_signal_str(); // use channel's implicit signal
unsigned int sat = 0;
try
{
sat = configuration_->property("Channel" + std::to_string(i) + ".satellite", 0);
}
catch (const std::exception& e)
{
LOG(WARNING) << e.what();
}
if (sat == 0)
{
bool assistance_available;
float estimated_doppler;
double RX_time;
bool is_primary_freq;
bool signal_available = true;
const Gnss_Signal gnss_signal = search_next_signal(gnss_signal_str, is_primary_freq, assistance_available, estimated_doppler, RX_time, signal_available);
if (signal_available)
{
channels_.at(i)->set_signal(gnss_signal);
}
else
{
// No local code has been assigned. In particular, assisted
// secondary channels must wait for a primary lock rather
// than starting acquisition with their PRN 0 placeholder.
if (channels_state_[i] == 2 && acq_channels_count_ > 0)
{
--acq_channels_count_;
}
channels_state_[i] = 0;
LOG(WARNING) << "No satellite available to assign to channel " << i << " for signal " << gnss_signal_str;
}
}
else
{
const auto& mapping = signal_mapping.at(gnss_signal_str);
const auto& gnss_system_name = mapping.first;
const auto gnss_signal = Gnss_Signal(Gnss_Satellite(gnss_system_name, sat), gnss_signal_str);
available_signals_map_.at(gnss_signal_str).remove(gnss_signal);
channels_.at(i)->set_signal(gnss_signal);
}
}
return 0;
}
void GNSSFlowgraph::print_help()
{
if (!help_hint_.empty())
{
std::cerr << "It seems that your configuration file is not well defined.\n";
std::cerr << "A hint to fix your configuration file:\n";
std::cerr << help_hint_;
}
}
void GNSSFlowgraph::check_desktop_conf_in_fpga_env()
{
int number_of_fpga_acq_channels = 0;
for (int i = 0; i < channels_count_; i++)
{
if (channels_.at(i)->get_left_block_acq() == nullptr)
{
number_of_fpga_acq_channels++;
}
}
if (number_of_fpga_acq_channels != channels_count_)
{
help_hint_ += " * The Acquisition block implementation is not suitable for GNSS-SDR flowgraph with FPGA off-loading\n";
help_hint_ += " If you want to use this configuration in an environment without FPGA, please rebuild GNSS-SDR with CMake option '-DENABLE_FPGA=OFF'\n";
}
}
bool GNSSFlowgraph::send_telemetry_msg(const pmt::pmt_t& msg)
{
// Push ephemeris to PVT telemetry msg in port using a channel out port
// it uses the first channel as a message producer (it is already connected to PVT)
channels_.at(0)->get_right_block()->message_port_pub(pmt::mp("telemetry"), msg);
return true;
}
void GNSSFlowgraph::push_back_signal(const Gnss_Signal& gs)
{
auto& available_signals = available_signals_map_.at(gs.get_signal_str());
available_signals.remove(gs);
available_signals.push_back(gs);
// A new candidate entered this pool: re-check it even if previously exhausted.
signals_with_nothing_searchable_.erase(gs.get_signal_str());
}
void GNSSFlowgraph::remove_signal(const Gnss_Signal& gs)
{
auto& available_signals = available_signals_map_.at(gs.get_signal_str());
available_signals.remove(gs);
}
// project Doppler from primary frequency to secondary frequency
double GNSSFlowgraph::project_doppler(const std::string& searched_signal, const std::string& assist_signal, double assist_doppler_hz)
{
// GLONASS base frequencies and slot spacings both have an L2/L1 ratio of
// 7/9, so this scaling is exact for every FDMA slot.
const auto searched_freq = SIGNAL_FREQ_MAP.find(searched_signal);
const auto assist_freq = SIGNAL_FREQ_MAP.find(assist_signal);
if ((searched_freq == SIGNAL_FREQ_MAP.end()) || (assist_freq == SIGNAL_FREQ_MAP.end()))
{
return assist_doppler_hz;
}
return (assist_doppler_hz / assist_freq->second) * searched_freq->second;
}
uint32_t GNSSFlowgraph::doppler_bins_for_uncertainty(const std::string& acq_role, double uncertainty_hz) const
{
if (!std::isfinite(uncertainty_hz) || uncertainty_hz < 0.0)
{
return 0;
}
int32_t doppler_max = configuration_->property(acq_role + ".doppler_max", 5000);
int32_t doppler_step = configuration_->property(acq_role + ".doppler_step", 500);
#if USE_GLOG_AND_GFLAGS
const int32_t override_max = FLAGS_doppler_max;
const int32_t override_step = FLAGS_doppler_step;
#else
const int32_t override_max = absl::GetFlag(FLAGS_doppler_max);
const int32_t override_step = absl::GetFlag(FLAGS_doppler_step);
#endif
// Match PcpsAcquisitionAdapter's command-line overrides before sizing the grid.
if (override_max != 0)
{
doppler_max = override_max;
}
if (override_step != 0)
{
doppler_step = override_step;
}
if (doppler_max <= 0 || doppler_step <= 0)
{
return 0;
}
const double half_bins = std::ceil(uncertainty_hz / static_cast<double>(doppler_step));
if (half_bins > static_cast<double>((std::numeric_limits<uint32_t>::max() - 1U) / 2U))
{
return 0;
}
const uint32_t num_bins = 1U + 2U * static_cast<uint32_t>(half_bins);
// Same full-grid size as pcps_acquisition. The acquisition block caps larger
// requests at the full grid but keeps it centered on the prediction, which
// would search beyond +/-doppler_max. A window that is not narrower than the
// full grid gains nothing, so request the full search centered at 0 Hz.
// A single bin (live fix) is kept as before.
const auto full_grid_bins = static_cast<uint32_t>(std::ceil(2.0 * static_cast<double>(doppler_max) / static_cast<double>(doppler_step)));
if (num_bins > 1U && num_bins >= full_grid_bins)
{
DLOG(INFO) << "Doppler uncertainty " << uncertainty_hz << " Hz needs " << num_bins
<< " bins, not narrower than the full " << full_grid_bins << "-bin grid of "
<< acq_role << ": using the full search";
return 0;
}
return num_bins;
}
void GNSSFlowgraph::acquisition_manager(unsigned int who)
{
unsigned int current_channel;
for (int i = 0; i < channels_count_; i++)
{
current_channel = (i + who + 1) % channels_count_;
unsigned int sat_ = 0;
try
{
sat_ = configuration_->property("Channel" + std::to_string(current_channel) + ".satellite", 0);
}
catch (const std::exception& e)
{
LOG(WARNING) << e.what();
}
if ((acq_channels_count_ < max_acq_channels_) && (channels_state_[current_channel] <= 1))
{
bool is_primary_freq = true;
bool assistance_available = false;
bool start_acquisition = false;
Gnss_Signal gnss_signal;
float estimated_doppler;
double RX_time;
if ((sat_ == 0) && (channels_state_[current_channel] == 1) &&
(channels_[current_channel]->get_signal().get_satellite().get_PRN() == 0))
{
channels_state_[current_channel] = 0;
}
if ((sat_ == 0) && (channels_state_[current_channel] == 0))
{
bool signal_available = true;
gnss_signal = search_next_signal(channels_[current_channel]->get_signal().get_signal_str(),
is_primary_freq,
assistance_available,
estimated_doppler,
RX_time,
signal_available);
if (signal_available)
{
channels_[current_channel]->set_signal(gnss_signal);
start_acquisition = is_primary_freq || assistance_available || !configuration_->property("GNSS-SDR.assist_dual_frequency_acq", multiband_);
}
// else: nothing searchable for this signal now. Leave the channel
// idle instead of starting a doomed acquisition; acquisition_manager()
// re-checks every idle tick and resumes once a satellite is requeued.
}
else
{
channels_[current_channel]->set_signal(channels_[current_channel]->get_signal());
start_acquisition = true;
}
if (start_acquisition == true)
{
channels_state_[current_channel] = 2;
acq_channels_count_++;
DLOG(INFO) << "Channel " << current_channel
<< " Starting acquisition " << channels_[current_channel]->get_signal().get_satellite()
<< ", Signal " << channels_[current_channel]->get_signal().get_signal_str();
if (assistance_available == true && configuration_->property("GNSS-SDR.assist_dual_frequency_acq", multiband_))
{
// Use the projected primary-band Doppler; the role flag can retain a full search.
const std::string acq_role = block_factory::get_role_name(configuration_.get(), "Acquisition_", channels_[current_channel]->get_signal().get_signal_str(), static_cast<int>(current_channel));
if (configuration_->property(acq_role + ".dual_freq_assisted_doppler_narrowing", true))
{
channels_[current_channel]->assist_acquisition_doppler(estimated_doppler, 1);
}
else
{
channels_[current_channel]->assist_acquisition_doppler(0, 0);
}
}
else
{
// Fall back to geometric Doppler for visible satellites. Secondary signals
// require opt-in; pre-fix predictions also require explicit uncertainty bounds.
bool doppler_predicted = false;
double predicted_doppler_hz = 0.0;
double doppler_uncertainty_hz = 0.0;
uint32_t doppler_num_bins = 0;
const std::string acq_role_alm_ephe = block_factory::get_role_name(configuration_.get(), "Acquisition_", channels_[current_channel]->get_signal().get_signal_str(), static_cast<int>(current_channel));
const bool alm_ephe_allowed_for_this_signal = is_primary_freq || configuration_->property(acq_role_alm_ephe + ".alm_ephe_assisted_doppler_narrowing", false);
if (alm_ephe_allowed_for_this_signal && satellite_visibility_ && satellite_visibility_->enabled())
{
const auto pvt_ptr = get_pvt();
const Gnss_Satellite& sat = channels_[current_channel]->get_signal().get_satellite();
if (pvt_ptr && satellite_visibility_->IsSearchVisible(sat))
{
double receiver_time_s = 0.0;
const Monitor_Pvt fix_status = channels_status_->get_current_status_pvt(&receiver_time_s);
const std::string& signal_str = channels_[current_channel]->get_signal().get_signal_str();
doppler_predicted = satellite_visibility_->PredictedDopplerHz(pvt_ptr, fix_status, receiver_time_s, sat, signal_str, predicted_doppler_hz);
if (doppler_predicted)
{
doppler_uncertainty_hz = satellite_visibility_->PredictedDopplerUncertaintyHz(fix_status, signal_str);
}
}
}
if (doppler_predicted)
{
doppler_num_bins = doppler_bins_for_uncertainty(acq_role_alm_ephe, doppler_uncertainty_hz);
doppler_predicted = doppler_num_bins != 0;
}
if (doppler_predicted)
{
LOG(INFO) << "[alm_ephe_doppler] " << (is_primary_freq ? "primary" : "secondary")
<< " signal " << channels_[current_channel]->get_signal().get_signal_str()
<< " channel " << current_channel
<< " satellite " << channels_[current_channel]->get_signal().get_satellite()
<< ": alm/ephe-predicted Doppler " << predicted_doppler_hz
<< " Hz +/- " << doppler_uncertainty_hz << " Hz, " << doppler_num_bins << " bin(s)";
channels_[current_channel]->assist_acquisition_doppler(predicted_doppler_hz, doppler_num_bins);
}
else
{
// set Doppler center to 0 Hz and search the full Doppler range
channels_[current_channel]->assist_acquisition_doppler(0, 0);
}
}
#if ENABLE_FPGA
if (enable_fpga_offloading_)
{
// create a task for the FPGA such that it doesn't stop the flow
std::thread tmp_thread(&ChannelInterface::start_acquisition, channels_[current_channel]);
tmp_thread.detach();
}
else
{
channels_[current_channel]->start_acquisition();
}
#else
channels_[current_channel]->start_acquisition();
#endif
}
else if (gnss_signal.get_satellite().get_PRN() != 0)
{
// PRN 0 means search_next_signal() picked nothing (signal_available
// was false), so there is nothing to return to the pool; pushing the
// default-constructed signal would throw on its empty signal string.
push_back_signal(gnss_signal);
DLOG(INFO) << "Channel " << current_channel
<< " secondary frequency acquisition assistance not available in "
<< channels_[current_channel]->get_signal().get_satellite()
<< ", Signal " << channels_[current_channel]->get_signal().get_signal_str();
}
}
DLOG(INFO) << "Channel " << current_channel << " in state " << channels_state_[current_channel];
}
}
/*
* Applies an action to the flow graph
*
* \param[in] who Who generated the action:
* -> 0-199 are the channels IDs
* -> 200 is the control_thread dispatched by the control_thread apply_action
* -> 300 is the telecommand system (TC) for receiver control
* -> 400 - 599 is the TC channel control for channels 0-199
* \param[in] what What is the action:
* --- actions from channels ---
* -> 0 acquisition failed
* -> 1 acquisition successful
* -> 2 tracking lost
* --- actions from TC receiver control ---
* -> 10 TC request standby mode
* -> 11 TC request coldstart
* -> 12 TC request hotstart
* -> 13 TC request warmstart
* --- actions from TC channel control ---
* -> 20 stop channel
* -> 21 start channel
*/
void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
{
// todo: the acquisition events are initiated from the acquisition success or failure queued msg. If the acquisition is disabled for non-assisted secondary freq channels, the engine stops..
std::lock_guard<std::mutex> lock(signal_list_mutex_);
DLOG(INFO) << "Received " << what << " from " << who;
unsigned int sat = 0;
Gnss_Signal gs;
if (who < 200)
{
try
{
sat = configuration_->property("Channel" + std::to_string(who) + ".satellite", 0);
}
catch (const std::exception& e)
{
LOG(WARNING) << e.what();
}
}
switch (what)
{
case 0:
gs = channels_[who]->get_signal();
DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << gs.get_satellite() << ", Signal " << gs.get_signal_str();
channels_state_[who] = 0;
if (acq_channels_count_ > 0)
{
acq_channels_count_--;
}
// call the acquisition manager to assign new satellite and start next acquisition (if required)
acquisition_manager(who);
// push back the old signal AFTER assigning a new one to avoid selecting the same signal
if (sat == 0)
{
push_back_signal(gs);
}
break;
case 1:
gs = channels_[who]->get_signal();
// A successful acquisition is real RF evidence: proceed to tracking even if
// the satellite is now classified excluded. The search mask only saves CPU
// on unlikely searches; PVT.elevation_mask still gates use in the fix.
DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << gs.get_satellite();
// If the satellite is in the list of available ones, remove it.
remove_signal(gs);
channels_state_[who] = 3;
if (acq_channels_count_ > 0)
{
acq_channels_count_--;
}
// call the acquisition manager to assign new satellite and start next acquisition (if required)
acquisition_manager(who);
break;
case 2:
gs = channels_[who]->get_signal();
DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << gs.get_satellite();
// The retry below bypasses the search pool: do not retry a satellite that
// has since been classified excluded; search for a new target instead.
if (satellite_visibility_ && satellite_visibility_->enabled() && satellite_visibility_->IsSearchExcluded(gs.get_satellite()))
{
LOG(INFO) << "[visibility] channel " << who << ": not retrying " << gs.get_satellite()
<< " (" << gs.get_signal_str() << ") after tracking loss, now classified excluded";
channels_state_[who] = 0;
acquisition_manager(who);
if (sat == 0)
{
push_back_signal(gs);
}
break;
}
// Retry the same satellite unless it is in acquisition cooldown (a
// signal that locks and drops at once would loop here).
{
double receiver_time_s = 0.0;
channels_status_->get_current_status_pvt(&receiver_time_s);
if (acq_channels_count_ < max_acq_channels_ && !InAcquisitionCooldown(gs, receiver_time_s))
{
// try to acquire the same satellite
channels_state_[who] = 2;
acq_channels_count_++;
DLOG(INFO) << "Channel " << who << " Starting acquisition " << gs.get_satellite() << ", Signal " << gs.get_signal_str();
channels_[who]->set_signal(channels_[who]->get_signal());
// This retry bypasses acquisition_manager(). An assisted
// center retained from before tracking may now be stale.
channels_[who]->assist_acquisition_doppler(0, 0);
MarkAcquisitionAttempt(gs, receiver_time_s);
#if ENABLE_FPGA
if (enable_fpga_offloading_)
{
// create a task for the FPGA such that it doesn't stop the flow
std::thread tmp_thread(&ChannelInterface::start_acquisition, channels_[who]);
tmp_thread.detach();
}
else
{
channels_[who]->start_acquisition();
}
#else
channels_[who]->start_acquisition();
#endif
}
else
{
channels_state_[who] = 0;
LOG(INFO) << "Channel " << who << " Idle state";
if (sat == 0)
{
push_back_signal(channels_[who]->get_signal());
}
}
}
break;
case 10: // request standby mode
for (size_t n = 0; n < channels_.size(); n++)
{
if (channels_state_[n] == 1 || channels_state_[n] == 2 || channels_state_[n] == 3) // channel assigned (not yet started), in acquisition, or in tracking
{
Gnss_Signal gs_assigned = channels_[n]->get_signal();
if (gs_assigned.get_satellite().get_PRN() != 0)
{
push_back_signal(gs_assigned);
}
if (channels_state_[n] != 1)
{
channels_[n]->stop_channel(); // stop the acquisition or tracking operation
}
channels_state_[n] = 0;
}
}
acq_channels_count_ = 0; // all channels are in standby now and no new acquisition should be started
break;
default:
break;
}
}
void GNSSFlowgraph::priorize_satellites(const std::vector<std::pair<int, Gnss_Satellite>>& visible_satellites)
{
size_t old_size;
Gnss_Signal gs;
for (const auto& visible_satellite : visible_satellites)
{
std::vector<std::string> signal_str_vector;
if (visible_satellite.second.get_system() == "GPS")
{
signal_str_vector = {"1C", "2S", "L5"};
}
else if (visible_satellite.second.get_system() == "Galileo")
{
signal_str_vector = {"1B", "5X", "7X", "E6"};
}
else if (visible_satellite.second.get_system() == "QZSS")
{
signal_str_vector = {"J1", "J5"};
}
for (const auto& signal_str : signal_str_vector)
{
const auto sig_it = available_signals_map_.find(signal_str);
if (sig_it == available_signals_map_.end())
{
// No channels are configured for this signal
continue;
}
auto& available_signals = sig_it->second;
gs = Gnss_Signal(visible_satellite.second, signal_str);
old_size = available_signals.size();
available_signals.remove(gs);
if (old_size > available_signals.size())
{
available_signals.push_front(gs);
}
}
}
}
void GNSSFlowgraph::UpdateVisibilityReference(time_t utc_time, const std::array<float, 3>& LLH)
{
if (!visibility_aware_search_enabled())
{
return;
}
{
std::lock_guard<std::mutex> lock(signal_list_mutex_);
double receiver_time_s = 0.0;
const Monitor_Pvt fix_status = channels_status_->get_current_status_pvt(&receiver_time_s);
satellite_visibility_->SetCommandReference(utc_time, LLH, fix_status, receiver_time_s);
}
MaybeUpdateVisibility();
}
void GNSSFlowgraph::MaybeUpdateVisibility()
{
if (!satellite_visibility_ || !satellite_visibility_->enabled())
{
return;
}
const auto pvt_ptr = get_pvt();
if (!pvt_ptr)
{
return;
}
std::lock_guard<std::mutex> lock(signal_list_mutex_);
double receiver_time_s = 0.0;
const Monitor_Pvt fix_status = channels_status_->get_current_status_pvt(&receiver_time_s);
if (satellite_visibility_->Tick(pvt_ptr, fix_status, receiver_time_s))
{
DLOG(INFO) << "Satellite visibility updated (visibility-aware search enabled)";
// Classification changed: previously exhausted signals may be searchable again.
signals_with_nothing_searchable_.clear();
const auto current_channels_status = channels_status_->get_current_status_map();
auto is_currently_tracked = [&](const Gnss_Signal& gs) {
const uint32_t prn = gs.get_satellite().get_PRN();
const std::string system_short = gs.get_satellite().get_system_short();
const char system_char = system_short.empty() ? '\0' : system_short[0];
for (const auto& cs : current_channels_status)
{
if (cs.second->PRN == prn && cs.second->System == system_char)
{
return true;
}
}
return false;
};
for (size_t n = 0; n < channels_.size(); n++)
{
if (channels_state_[n] != 1)
{
continue;
}
const Gnss_Signal gs_assigned = channels_[n]->get_signal();
if (gs_assigned.get_satellite().get_PRN() == 0)
{
continue;
}
if (!satellite_visibility_->IsSearchExcluded(gs_assigned.get_satellite()) || is_currently_tracked(gs_assigned))
{
continue;
}
LOG(INFO) << "[visibility] channel " << n << ": requeuing " << gs_assigned.get_satellite()
<< " (" << gs_assigned.get_signal_str() << "), now classified excluded -- was assigned but not yet started";
push_back_signal(gs_assigned);
channels_state_[n] = 0;
}
}
}
bool GNSSFlowgraph::visibility_aware_search_enabled() const
{
return satellite_visibility_ && satellite_visibility_->enabled();
}
void GNSSFlowgraph::stop_duplicated_satellite_channels()
{
std::lock_guard<std::mutex> lock(signal_list_mutex_);
const auto current_channels_status = channels_status_->get_current_status_map();
// Group tracking channels by reported (system, PRN, signal), i.e. what PVT receives.
std::map<std::tuple<char, uint32_t, std::string>, std::vector<int>> tracked_channels;
for (const auto& cs : current_channels_status)
{
const int channel_id = cs.first;
if (!cs.second || channel_id < 0 || channel_id >= channels_count_ || channels_state_[channel_id] != 3)
{
continue;
}
tracked_channels[std::make_tuple(cs.second->System, cs.second->PRN, std::string(cs.second->Signal, 2))].push_back(channel_id);
}
for (const auto& entry : tracked_channels)
{
const std::vector<int>& channel_ids = entry.second;
if (channel_ids.size() < 2)
{
continue;
}
int strongest = channel_ids.front();
for (const int channel_id : channel_ids)
{
if (current_channels_status.at(channel_id)->CN0_dB_hz > current_channels_status.at(strongest)->CN0_dB_hz)
{
strongest = channel_id;
}
}
for (const int channel_id : channel_ids)
{
if (channel_id == strongest)
{
continue;
}
const Gnss_Synchro& synchro = *current_channels_status.at(channel_id);
const Gnss_Signal gs_assigned = channels_[channel_id]->get_signal();
std::ostringstream oss;
oss << "Channel " << channel_id << ", assigned to " << gs_assigned.get_satellite()
<< ", is tracking " << synchro.System << synchro.PRN << " (" << std::string(synchro.Signal, 2)
<< ") already tracked by channel " << strongest << " with higher C/N0 ("
<< std::fixed << std::setprecision(1) << synchro.CN0_dB_hz << " vs "
<< current_channels_status.at(strongest)->CN0_dB_hz << " dB-Hz). Stopping channel " << channel_id;
LOG(INFO) << oss.str();
if (gs_assigned.get_satellite().get_PRN() != 0)
{
push_back_signal(gs_assigned);
}
channels_[channel_id]->stop_channel();
// Channel status is reported once per second: drop the
// stale entry so the reassigned channel is not stopped
// again before it reports its own new observables.
channels_status_->clear_channel_status(channel_id);
channels_state_[channel_id] = 0;
acquisition_manager(channel_id);
}
}
}
void GNSSFlowgraph::set_configuration(const std::shared_ptr<ConfigurationInterface>& configuration)
{
if (running_)
{
LOG(WARNING) << "Unable to update configuration while flowgraph running";
return;
}
if (connected_)
{
LOG(WARNING) << "Unable to update configuration while flowgraph connected";
}
configuration_ = configuration;
}
#if ENABLE_FPGA
void GNSSFlowgraph::start_acquisition_helper()
{
std::lock_guard<std::mutex> lock(signal_list_mutex_);
for (int i = 0; i < channels_count_; i++)
{
if (channels_state_[i] == 2)
{
#if ENABLE_FPGA
if (enable_fpga_offloading_)
{
// create a task for the FPGA such that it doesn't stop the flow
std::thread tmp_thread(&ChannelInterface::start_acquisition, channels_[i]);
tmp_thread.detach();
}
else
{
channels_.at(i)->start_acquisition();
}
#else
channels_.at(i)->start_acquisition();
#endif
}
}
}
void GNSSFlowgraph::perform_hw_reset()
{
// a stop acquisition command causes the SW to reset the HW
std::shared_ptr<Channel> channel_ptr;
for (int i = 0; i < channels_count_; i++)
{
channel_ptr = std::dynamic_pointer_cast<Channel>(channels_.at(i));
channel_ptr->tracking()->stop_tracking();
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
channel_ptr = std::dynamic_pointer_cast<Channel>(channels_.at(0));
channel_ptr->acquisition()->stop_acquisition();
}
#endif
std::vector<std::string> GNSSFlowgraph::split_string(const std::string& s, char delim)
{
std::vector<std::string> v;
std::stringstream ss(s);
std::string item;
while (std::getline(ss, item, delim))
{
*(std::back_inserter(v)++) = item;
}
return v;
}
void GNSSFlowgraph::keep_one_glonass_slot_per_frequency(std::set<unsigned int>& available_prns)
{
std::set<int32_t> selected_frequency_channels;
std::set<unsigned int> unique_frequency_prns;
for (const auto prn : available_prns)
{
if (prn == 0)
{
LOG(WARNING) << "Ignoring invalid GLONASS slot " << prn << " in Glonass.prns";
continue;
}
const auto freq_channel = GLONASS_PRN.find(prn);
if (freq_channel == GLONASS_PRN.cend())
{
LOG(WARNING) << "Ignoring GLONASS slot " << prn << " without a configured GLONASS frequency channel";
continue;
}
if (selected_frequency_channels.insert(freq_channel->second).second)
{
unique_frequency_prns.insert(prn);
}
else
{
LOG(WARNING) << "Ignoring GLONASS slot " << prn
<< " because another configured orbital slot already uses frequency channel "
<< freq_channel->second;
}
}
available_prns = std::move(unique_frequency_prns);
}
void GNSSFlowgraph::set_signals_list()
{
// GLONASS uses FDMA, so only one orbital slot per frequency channel is queued for acquisition.
std::unordered_map<std::string, std::set<unsigned int>> available_prn_map = {
{"GPS", {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32}},
{"SBAS", {120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138}},
{"Galileo", {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36}},
{"Glonass", {1, 2, 3, 4, 9, 10, 11, 12, 18, 19, 20, 21, 26, 27}},
{"Beidou", {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37,
38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54,
55, 56, 57, 58, 59, 60, 61, 62, 63}},
// PRNs 198 and 202 are excluded: they are reserved but not assigned to any
// operational satellite (marked as non-standard codes in IS-QZSS-PNT).
// They can still be searched by setting the QZSS.prns configuration option.
{"QZSS", {193, 194, 195, 196, 197, 199, 200, 201, 203, 204, 205, 206}},
};
#if CXX_LESS_THAN_17
for (auto& entry : available_prn_map)
{
auto& gnss_system_str = entry.first;
auto& available_prns = entry.second;
#else
for (auto& [gnss_system_str, available_prns] : available_prn_map)
{
#endif
const auto sv_list = configuration_->property(gnss_system_str + ".prns", std::string(""));
if (!sv_list.empty())
{
// Reset the available prns:
std::set<unsigned int> tmp_set;
boost::tokenizer<> tok(sv_list);
std::transform(tok.begin(), tok.end(), std::inserter(tmp_set, tmp_set.begin()),
boost::lexical_cast<unsigned int, std::string>);
if (!tmp_set.empty())
{
available_prns = std::move(tmp_set);
}
}
const auto sv_banned = configuration_->property("GNSS-SDR." + gnss_system_str + "_banned_prns", std::string(""));
if (!sv_banned.empty())
{
std::stringstream ss(sv_banned);
while (ss.good())
{
std::string substr;
std::getline(ss, substr, ',');
try
{
const auto banned = static_cast<unsigned int>(std::stoi(substr));
available_prns.erase(banned);
}
catch (const std::invalid_argument& ia)
{
std::cerr << "Invalid argument at GNSS-SDR." << gnss_system_str << "_banned_prns configuration parameter: " << ia.what() << '\n';
}
catch (const std::out_of_range& oor)
{
std::cerr << "Out of range at GNSS-SDR." << gnss_system_str << "_banned_prns configuration parameter: " << oor.what() << '\n';
}
}
}
if (gnss_system_str == "Glonass")
{
keep_one_glonass_slot_per_frequency(available_prns);
}
}
#if CXX_LESS_THAN_17
for (const auto& entry : signal_mapping)
{
const auto& signal_str = entry.first;
const auto& signal_info = entry.second;
#else
for (const auto& [signal_str, signal_info] : signal_mapping)
{
#endif
const auto& gnss_system_str = signal_info.first;
const auto channel_count_option = "Channels_" + signal_str + ".count";
if (configuration_->property(channel_count_option, 0) > 0)
{
auto& available_signals = available_signals_map_[signal_str];
for (const auto& prn : available_prn_map.at(gnss_system_str))
{
if (signal_str == "1D")
{
if ((prn >= 1U && prn <= 5U) || (prn >= 59U && prn <= 63U))
{
continue; // GEO satellites do not broadcast B1C (ICD section 3.1)
}
}
if (signal_str == "5D")
{
if ((prn >= 1U && prn <= 5U) || (prn >= 59U && prn <= 63U))
{
continue; // GEO satellites do not broadcast B2a RNSS (ICD §2)
}
}
if (signal_str == "J5" && prn > QZSS_L5_MAX_PRN)
{
// QZSS L1 C/B PRNs (203-206) do not transmit an L5 signal
continue;
}
available_signals.emplace_back(Gnss_Satellite(gnss_system_str, prn), signal_str);
}
}
}
if (configuration_->property("Channels_SBAS.count", 0) > 0)
{
const std::string gnss_system_str = "SBAS";
const std::string signal_str = "1C";
auto& available_signals = available_signals_map_[signal_str];
for (const auto& prn : available_prn_map.at(gnss_system_str))
{
available_signals.emplace_back(Gnss_Satellite(gnss_system_str, prn), signal_str);
}
}
}
void GNSSFlowgraph::set_channels_state()
{
std::lock_guard<std::mutex> lock(signal_list_mutex_);
max_acq_channels_ = configuration_->property("Channels.in_acquisition", channels_count_);
if (max_acq_channels_ > channels_count_)
{
max_acq_channels_ = channels_count_;
LOG(WARNING) << "Channels_in_acquisition is bigger than number of channels. Variable acq_channels_count_ is set to " << channels_count_;
}
channels_state_.reserve(channels_count_);
for (int i = 0; i < channels_count_; i++)
{
if (i < max_acq_channels_)
{
channels_state_.push_back(2);
}
else
{
channels_state_.push_back(1);
}
DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i];
}
acq_channels_count_ = max_acq_channels_;
DLOG(INFO) << acq_channels_count_ << " channels in acquisition state";
}
bool GNSSFlowgraph::is_multiband() const
{
bool multiband = false;
if (configuration_->property("Channels_1C.count", 0) > 0)
{
if (configuration_->property("Channels_2S.count", 0) > 0)
{
multiband = true;
}
if (configuration_->property("Channels_L5.count", 0) > 0)
{
multiband = true;
}
}
if (configuration_->property("Channels_1B.count", 0) > 0)
{
if (configuration_->property("Channels_5X.count", 0) > 0)
{
multiband = true;
}
if (configuration_->property("Channels_7X.count", 0) > 0)
{
multiband = true;
}
if (configuration_->property("Channels_E6.count", 0) > 0)
{
multiband = true;
}
}
if (configuration_->property("Channels_1G.count", 0) > 0)
{
if (configuration_->property("Channels_2G.count", 0) > 0)
{
multiband = true;
}
}
const bool has_bds_b1 = configuration_->property("Channels_B1.count", 0) > 0 ||
configuration_->property("Channels_1D.count", 0) > 0;
const bool has_bds_b3 = configuration_->property("Channels_B3.count", 0) > 0;
const bool has_bds_b2a = configuration_->property("Channels_5D.count", 0) > 0;
if ((has_bds_b1 && (has_bds_b3 || has_bds_b2a)) || (has_bds_b3 && has_bds_b2a))
{
multiband = true;
}
if (configuration_->property("Channels_J1.count", 0) > 0)
{
if (configuration_->property("Channels_J5.count", 0) > 0)
{
multiband = true;
}
}
return multiband;
}
Gnss_Signal GNSSFlowgraph::search_next_signal(const std::string& searched_signal,
bool& is_primary_frequency,
bool& assistance_available,
float& estimated_doppler,
double& RX_time,
bool& signal_available)
{
is_primary_frequency = false;
assistance_available = false;
signal_available = true;
Gnss_Signal result{};
bool found_signal = false;
std::vector<std::string> assist_signal_candidates;
auto& available_signals = available_signals_map_.at(searched_signal);
// Time for the acquisition retry cooldown, fetched only if enabled
double cooldown_receiver_time_s = 0.0;
if (acquisition_retry_min_interval_s_ > 0.0)
{
channels_status_->get_current_status_pvt(&cooldown_receiver_time_s);
}
if (available_signals.empty())
{
signal_available = false;
const auto& entry = signal_mapping.at(searched_signal);
LOG(INFO) << "[visibility] No untracked/unassigned satellites currently available for signal "
<< entry.first << " " << entry.second << " -- channel stays idle";
return result;
}
switch (mapStringValues_[searched_signal])
{
case evGPS_2S:
case evGPS_L5:
assist_signal_candidates = {"1C"};
break;
case evGAL_5X:
case evGAL_7X:
case evGAL_E6:
assist_signal_candidates = {"1B"};
break;
case evGLO_2G:
assist_signal_candidates = {"1G"};
break;
case evBDS_B3:
// Prefer a satellite tracked on B1I; BeiDou-3 satellites also
// broadcast B1C, so fall back to a satellite tracked on B1C.
assist_signal_candidates = {"B1", "1D"};
break;
case evBDS_B2A:
assist_signal_candidates = {"B1", "1D", "B3"};
break;
case evGPS_1C:
case evGAL_1B:
case evGLO_1G:
case evBDS_B1:
case evBDS_B1C:
case evQZS_J1:
case evSBAS_1C:
is_primary_frequency = true;
break;
case evQZS_J5:
assist_signal_candidates = {"J1"};
break;
default:
break;
}
const bool any_assist_configured = std::any_of(assist_signal_candidates.begin(), assist_signal_candidates.end(),
[&](const std::string& assist_signal) { return configuration_->property("Channels_" + assist_signal + ".count", 0) > 0; });
// A BeiDou band without an enabled assisting band must acquire on its
// own, even when another constellation makes the receiver multiband.
// In B3I+B2a, B3I starts first and then assists B2a.
if (!any_assist_configured && (searched_signal == "5D" || searched_signal == "B3"))
{
assist_signal_candidates.clear();
is_primary_frequency = true;
}
if (any_assist_configured)
{
// 1. Get the current channel tracking map: locked channels, with or
// without a valid time reference. Tracked Doppler is usable before
// navigation-message decoding.
const auto current_channels_status = channels_status_->get_current_tracking_map();
// 2. search the currently tracked primary signal satellites and assist the acquisition if the satellite is not tracked on the assisted signal
for (const auto& assist_signal : assist_signal_candidates)
{
if (found_signal)
{
break;
}
if (configuration_->property("Channels_" + assist_signal + ".count", 0) <= 0)
{
continue;
}
for (const auto& current_status : current_channels_status)
{
if (std::string(current_status.second->Signal) == assist_signal)
{
const bool glonass_fdma = (searched_signal == "2G");
const uint32_t tracked_prn = current_status.second->PRN;
std::list<Gnss_Signal>::iterator it2;
it2 = std::find_if(std::begin(available_signals), std::end(available_signals),
[&](Gnss_Signal const& sig) {
if (sig.get_satellite().get_PRN() == tracked_prn)
{
return true;
}
if (!glonass_fdma)
{
return false;
}
const auto pool_channel = GLONASS_PRN.find(sig.get_satellite().get_PRN());
const auto tracked_channel = GLONASS_PRN.find(tracked_prn);
return (pool_channel != GLONASS_PRN.cend()) && (tracked_channel != GLONASS_PRN.cend()) &&
(pool_channel->second == tracked_channel->second);
});
if (it2 != available_signals.end())
{
// A tracked-status snapshot may be stale or below the search mask.
if (satellite_visibility_ && satellite_visibility_->enabled() &&
satellite_visibility_->IsSearchExcluded(it2->get_satellite()))
{
continue;
}
// GPS SV configuration: AS_status bits 0-2 (IS-GPS-200, 20.3.3.5.1.4).
// Require IIR-M for L2C, IIF for L5; accept unknown configurations.
if (enable_secondary_signal_status_gating_ && (mapStringValues_[searched_signal] == evGPS_L5 || mapStringValues_[searched_signal] == evGPS_2S))
{
const auto pvt_ptr = get_pvt();
if (pvt_ptr)
{
const auto gps_almanac_map = pvt_ptr->get_gps_almanac();
const auto alm_it = gps_almanac_map.find(static_cast<int>(it2->get_satellite().get_PRN()));
if (alm_it != gps_almanac_map.end() && alm_it->second.AS_status > 0)
{
const int32_t sv_config = alm_it->second.AS_status & GPS_SV_CONFIG_CODE_MASK;
const int32_t required_config = (mapStringValues_[searched_signal] == evGPS_L5) ? GPS_SV_CONFIG_BLOCK_IIF : GPS_SV_CONFIG_BLOCK_IIR_M;
if (sv_config != GPS_SV_CONFIG_UNKNOWN && sv_config < required_config)
{
continue;
}
}
}
}
// Gate E5b on I/NAV health. E5a health comes only from E5a F/NAV and
// may be stale before reacquisition, so it must not block the search.
if (enable_secondary_signal_status_gating_ && mapStringValues_[searched_signal] == evGAL_7X)
{
const auto pvt_ptr = get_pvt();
if (pvt_ptr)
{
const int prn = static_cast<int>(it2->get_satellite().get_PRN());
int32_t health = -1;
const auto gal_eph_map = pvt_ptr->get_galileo_ephemeris();
const auto eph_it = gal_eph_map.find(prn);
if (eph_it != gal_eph_map.end())
{
health = eph_it->second.E5b_HS;
}
else
{
const auto gal_alm_map = pvt_ptr->get_galileo_almanac();
const auto alm_it = gal_alm_map.find(prn);
if (alm_it != gal_alm_map.end())
{
health = alm_it->second.E5b_HS;
}
}
if (health > 0)
{
continue;
}
}
}
// BeiDou: skip B3I if SV_health is not zero (ephemeris first,
// then almanac).
if (enable_secondary_signal_status_gating_ && mapStringValues_[searched_signal] == evBDS_B3)
{
const auto pvt_ptr = get_pvt();
if (pvt_ptr)
{
const int prn = static_cast<int>(it2->get_satellite().get_PRN());
int32_t health = -1;
const auto bds_eph_map = pvt_ptr->get_beidou_dnav_ephemeris();
const auto eph_it = bds_eph_map.find(prn);
if (eph_it != bds_eph_map.end())
{
health = eph_it->second.SV_health;
}
else
{
const auto bds_alm_map = pvt_ptr->get_beidou_dnav_almanac();
const auto alm_it = bds_alm_map.find(prn);
if (alm_it != bds_alm_map.end())
{
health = alm_it->second.SV_health;
}
}
if (health > 0)
{
continue;
}
}
}
// GLONASS: skip L2 if the Bn health word of the ephemeris is
// not zero.
if (enable_secondary_signal_status_gating_ && mapStringValues_[searched_signal] == evGLO_2G)
{
const auto pvt_ptr = get_pvt();
if (pvt_ptr)
{
// Not *it2: the FDMA pool match can be another slot sharing
// the frequency channel.
const int prn = static_cast<int>(tracked_prn);
const auto glo_eph_map = pvt_ptr->get_glonass_ephemeris();
const auto eph_it = glo_eph_map.find(prn);
if (eph_it != glo_eph_map.end() && eph_it->second.d_B_n > 0)
{
continue;
}
}
}
// QZSS: skip L5 if SV_health is not zero (ephemeris first, then
// almanac). QZSS LNAV data is stored in the GPS maps.
if (enable_secondary_signal_status_gating_ && mapStringValues_[searched_signal] == evQZS_J5)
{
const auto pvt_ptr = get_pvt();
if (pvt_ptr)
{
const int prn = static_cast<int>(it2->get_satellite().get_PRN());
int32_t health = -1;
const auto gps_eph_map = pvt_ptr->get_gps_ephemeris();
const auto eph_it = gps_eph_map.find(prn);
if (eph_it != gps_eph_map.end())
{
health = eph_it->second.SV_health;
}
else
{
const auto gps_alm_map = pvt_ptr->get_gps_almanac();
const auto alm_it = gps_alm_map.find(prn);
if (alm_it != gps_alm_map.end())
{
health = alm_it->second.SV_health;
}
}
if (health > 0)
{
continue;
}
}
}
// Acquisition retry cooldown, checked on the signal that will be
// acquired: with FDMA, *it2 can be another slot on the same channel.
const bool other_glonass_slot = glonass_fdma && (it2->get_satellite().get_PRN() != tracked_prn);
const Gnss_Signal cooldown_signal = other_glonass_slot ? Gnss_Signal(Gnss_Satellite(std::string("Glonass"), tracked_prn), searched_signal) : *it2;
if (InAcquisitionCooldown(cooldown_signal, cooldown_receiver_time_s))
{
continue;
}
// Doppler observed on the assisting band, projected to the searched band
estimated_doppler = static_cast<float>(project_doppler(searched_signal, assist_signal, current_status.second->Carrier_Doppler_hz));
RX_time = current_status.second->RX_time;
result = *it2;
if (glonass_fdma && (result.get_satellite().get_PRN() != tracked_prn))
{
result = Gnss_Signal(Gnss_Satellite(std::string("Glonass"), tracked_prn), searched_signal);
}
// Log this assisted pick separately: it bypasses pop_by_visibility().
LOG(INFO) << "[visibility] signal " << searched_signal << ": picked "
<< result.get_satellite() << " via primary-frequency assist ("
<< assist_signal << " already tracked) -- bypasses visible/mayvisible bucket selection";
available_signals.erase(it2);
found_signal = true;
assistance_available = true;
MarkAcquisitionAttempt(result, cooldown_receiver_time_s);
break;
}
}
}
}
}
if (found_signal == false)
{
// With dual-frequency assistance required, wait for a tracked primary band.
if (!assist_signal_candidates.empty() && configuration_->property("GNSS-SDR.assist_dual_frequency_acq", multiband_))
{
signal_available = false;
constexpr double kNoAssistLogThrottleS = 5.0;
const auto now = std::chrono::steady_clock::now();
auto& last_logged = no_assist_log_throttle_[searched_signal];
if (std::chrono::duration<double>(now - last_logged).count() >= kNoAssistLogThrottleS)
{
last_logged = now;
std::string candidates_str;
for (size_t i = 0; i < assist_signal_candidates.size(); i++)
{
candidates_str += assist_signal_candidates[i];
if (i + 1 < assist_signal_candidates.size())
{
candidates_str += "/";
}
}
LOG(INFO) << "[visibility] signal " << searched_signal << ": no " << candidates_str
<< "-tracked satellite currently available to assist -- channel stays idle (no wasted pick attempted)";
}
return result;
}
if (satellite_visibility_ && satellite_visibility_->enabled())
{
bool picked = false;
result = pop_by_visibility(available_signals, searched_signal, picked, cooldown_receiver_time_s);
if (!picked)
{
signal_available = false;
return result;
}
}
else
{
// Take the first entry that is not in acquisition cooldown
auto it = std::find_if(available_signals.begin(), available_signals.end(),
[&](const Gnss_Signal& gs) { return !InAcquisitionCooldown(gs, cooldown_receiver_time_s); });
if (it == available_signals.end())
{
signal_available = false;
return result;
}
result = *it;
available_signals.erase(it);
MarkAcquisitionAttempt(result, cooldown_receiver_time_s);
}
}
return result;
}
namespace
{
std::string acquisition_cooldown_key(const Gnss_Signal& gs)
{
const auto sat = gs.get_satellite();
return sat.get_system_short() + std::to_string(sat.get_PRN()) + "_" + gs.get_signal_str();
}
} // namespace
bool GNSSFlowgraph::InAcquisitionCooldown(const Gnss_Signal& gs, double receiver_time_s) const
{
if (acquisition_retry_min_interval_s_ <= 0.0)
{
return false;
}
const auto it = last_acquisition_attempt_rx_time_s_.find(acquisition_cooldown_key(gs));
if (it == last_acquisition_attempt_rx_time_s_.cend())
{
return false;
}
const double elapsed_s = receiver_time_s - it->second;
// The clock advances in steps: the tolerance avoids holding an interval
// that is a multiple of the step for one more step. Fail open if the
// elapsed time is negative.
constexpr double tolerance_s = 1e-3;
return elapsed_s >= 0.0 && (elapsed_s + tolerance_s) < acquisition_retry_min_interval_s_;
}
void GNSSFlowgraph::MarkAcquisitionAttempt(const Gnss_Signal& gs, double receiver_time_s)
{
if (acquisition_retry_min_interval_s_ <= 0.0)
{
return;
}
last_acquisition_attempt_rx_time_s_[acquisition_cooldown_key(gs)] = receiver_time_s;
}
Gnss_Signal GNSSFlowgraph::pop_by_visibility(std::list<Gnss_Signal>& available_signals, const std::string& searched_signal, bool& picked, double cooldown_receiver_time_s)
{
picked = false;
// Cached exhaustion result: skips the scan and the channels_status_ lock until
// a push-back or a reclassification invalidates it.
if (signals_with_nothing_searchable_.count(searched_signal) > 0)
{
return Gnss_Signal{};
}
uint32_t& counter = visibility_pick_counter_[searched_signal];
const uint32_t ratio = satellite_visibility_->search_ratio();
const bool want_visible = counter < ratio;
// Tracking on another band overrides exclusion at the elevation mask.
// Fetch the shared status map only when an excluded entry needs this check.
const bool any_excluded = std::any_of(available_signals.begin(), available_signals.end(),
[&](const Gnss_Signal& gs) { return satellite_visibility_->IsSearchExcluded(gs.get_satellite()); });
std::map<int, std::shared_ptr<Gnss_Synchro>> current_channels_status;
if (any_excluded)
{
current_channels_status = channels_status_->get_current_status_map();
}
auto is_currently_tracked = [&](const Gnss_Signal& gs) {
const uint32_t prn = gs.get_satellite().get_PRN();
const std::string system_short = gs.get_satellite().get_system_short();
const char system_char = system_short.empty() ? '\0' : system_short[0];
for (const auto& cs : current_channels_status)
{
if (cs.second->PRN == prn && cs.second->System == system_char)
{
return true;
}
}
return false;
};
// Excluded entries are never picked (unless tracked elsewhere) but stay queued
// so they are picked up automatically once reclassified.
auto is_searchable = [&](const Gnss_Signal& gs) {
return !satellite_visibility_->IsSearchExcluded(gs.get_satellite()) || is_currently_tracked(gs);
};
auto find_bucket = [&](bool visible) {
return std::find_if(available_signals.begin(), available_signals.end(),
[&](const Gnss_Signal& gs) { return is_searchable(gs) && !InAcquisitionCooldown(gs, cooldown_receiver_time_s) && satellite_visibility_->IsSearchVisible(gs.get_satellite()) == visible; });
};
auto it = find_bucket(want_visible);
bool got_desired_bucket = (it != available_signals.end());
if (!got_desired_bucket)
{
// Selected bucket is empty for this signal: fall back to the other one.
it = find_bucket(!want_visible);
}
size_t remaining_visible = 0;
size_t remaining_mayvisible = 0;
size_t remaining_excluded = 0;
for (const auto& gs : available_signals)
{
if (!is_searchable(gs))
{
remaining_excluded++;
}
else if (satellite_visibility_->IsSearchVisible(gs.get_satellite()))
{
remaining_visible++;
}
else
{
remaining_mayvisible++;
}
}
if (it == available_signals.end())
{
// Nothing can be picked now. Cache that only if every entry is excluded:
// a cooldown expires with no event to invalidate the cache.
const bool any_searchable_at_all = std::any_of(available_signals.begin(), available_signals.end(), is_searchable);
if (!any_searchable_at_all)
{
// Only excluded entries remain: leave them queued and idle the channel. This
// is the CPU-saving case once a complete almanac/ephemeris places every
// untracked satellite below the mask.
LOG(INFO) << "[visibility] signal " << searched_signal << ": nothing searchable ("
<< remaining_excluded << " excluded entries queued, elevation known and not visible) -- channel stays idle"
<< " (further checks skipped until this changes)";
signals_with_nothing_searchable_.insert(searched_signal);
}
return Gnss_Signal{};
}
picked = true;
const Gnss_Signal result = *it;
const bool picked_visible = satellite_visibility_->IsSearchVisible(result.get_satellite());
available_signals.erase(it);
MarkAcquisitionAttempt(result, cooldown_receiver_time_s);
counter = (counter + 1) % (ratio + 1);
if (picked_visible)
{
remaining_visible = remaining_visible > 0 ? remaining_visible - 1 : 0;
}
else
{
remaining_mayvisible = remaining_mayvisible > 0 ? remaining_mayvisible - 1 : 0;
}
// Always-on diagnostic: which bucket each acquisition is drawn from and what
// remains queued.
LOG(INFO) << "[visibility] signal " << searched_signal << ": picked " << result.get_satellite()
<< " from " << (picked_visible ? "VISIBLE" : "MAYVISIBLE") << " bucket"
<< (got_desired_bucket ? "" : " (desired bucket was empty, fell back)")
<< " -- queue now has " << remaining_visible << " visible, " << remaining_mayvisible
<< " mayvisible, " << remaining_excluded << " excluded";
return result;
}
Reference in New Issue View Git Blame Copy Permalink