mirror of https://github.com/gnss-sdr/gnss-sdr
1253 lines
58 KiB
C++
1253 lines
58 KiB
C++
/*!
|
|
* \file control_thread.cc
|
|
* \brief This class implements the receiver control plane
|
|
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
|
|
*
|
|
* GNSS Receiver Control Plane: connects the flowgraph, starts running it,
|
|
* and while it does not stop, reads the control messages generated by the blocks,
|
|
* process them, and apply the corresponding actions.
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*
|
|
* 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
|
|
*
|
|
* -----------------------------------------------------------------------------
|
|
*/
|
|
|
|
#if ARMA_NO_BOUND_CHECKING
|
|
#define ARMA_NO_DEBUG 1
|
|
#endif
|
|
|
|
#include "control_thread.h"
|
|
#include "concurrent_map.h"
|
|
#include "configuration_interface.h"
|
|
#include "file_configuration.h"
|
|
#include "galileo_almanac.h"
|
|
#include "galileo_ephemeris.h"
|
|
#include "galileo_iono.h"
|
|
#include "galileo_utc_model.h"
|
|
#include "geofunctions.h"
|
|
#include "glonass_gnav_ephemeris.h"
|
|
#include "glonass_gnav_utc_model.h"
|
|
#include "gnss_flowgraph.h"
|
|
#include "gnss_satellite.h"
|
|
#include "gnss_sdr_flags.h"
|
|
#include "gps_acq_assist.h" // for Gps_Acq_Assist
|
|
#include "gps_almanac.h" // for Gps_Almanac
|
|
#include "gps_cnav_ephemeris.h" // for Gps_CNAV_Ephemeris
|
|
#include "gps_cnav_utc_model.h" // for Gps_CNAV_Utc_Model
|
|
#include "gps_ephemeris.h" // for Gps_Ephemeris
|
|
#include "gps_iono.h" // for Gps_Iono
|
|
#include "gps_utc_model.h" // for Gps_Utc_Model
|
|
#include "pvt_interface.h" // for PvtInterface
|
|
#include "rtklib.h" // for gtime_t, alm_t
|
|
#include "rtklib_conversions.h" // for alm_to_rtklib
|
|
#include "rtklib_ephemeris.h" // for alm2pos, eph2pos
|
|
#include "rtklib_rtkcmn.h" // for utc2gpst
|
|
#include <armadillo> // for interaction with geofunctions
|
|
#include <boost/lexical_cast.hpp> // for bad_lexical_cast
|
|
#include <glog/logging.h> // for LOG
|
|
#include <pmt/pmt.h> // for make_any
|
|
#include <algorithm> // for find, min
|
|
#include <chrono> // for milliseconds
|
|
#include <cmath> // for floor, fmod, log
|
|
#include <ctime> // for time_t, gmtime, strftime
|
|
#include <exception> // for exception
|
|
#include <iostream> // for operator<<
|
|
#include <limits> // for numeric_limits
|
|
#include <map> // for map
|
|
#include <pthread.h> // for pthread_cancel
|
|
#include <stdexcept> // for invalid_argument
|
|
#include <sys/ipc.h> // for IPC_CREAT
|
|
#include <sys/msg.h> // for msgctl, msgget
|
|
|
|
#ifdef ENABLE_FPGA
|
|
#include <boost/chrono.hpp> // for steady_clock
|
|
#endif
|
|
|
|
#if PMT_USES_BOOST_ANY
|
|
namespace wht = boost;
|
|
#else
|
|
namespace wht = std;
|
|
#endif
|
|
|
|
extern Concurrent_Map<Gps_Acq_Assist> global_gps_acq_assist_map;
|
|
extern Concurrent_Queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
|
|
|
|
|
|
ControlThread::ControlThread()
|
|
{
|
|
if (FLAGS_c == "-")
|
|
{
|
|
configuration_ = std::make_shared<FileConfiguration>(FLAGS_config_file);
|
|
}
|
|
else
|
|
{
|
|
configuration_ = std::make_shared<FileConfiguration>(FLAGS_c);
|
|
}
|
|
// Basic configuration checks
|
|
auto aux = std::dynamic_pointer_cast<FileConfiguration>(configuration_);
|
|
conf_file_has_section_ = aux->has_section();
|
|
|
|
conf_file_has_mandatory_globals_ = (configuration_->property("GNSS-SDR.internal_fs_sps", 0) == 0 ? false : true);
|
|
|
|
const std::string empty_implementation;
|
|
std::string src_impl = configuration_->property("SignalSource.implementation", empty_implementation);
|
|
int src_count_deprecated = configuration_->property("Receiver.sources_count", 1);
|
|
int src_count = configuration_->property("GNSS-SDR.num_sources", src_count_deprecated);
|
|
if (src_impl.empty())
|
|
{
|
|
src_impl = std::string("");
|
|
int num_src = 0;
|
|
for (auto i = 0; i < src_count; i++)
|
|
{
|
|
auto src_impl_multiple = configuration_->property("SignalSource" + std::to_string(i) + ".implementation", empty_implementation);
|
|
num_src += !src_impl_multiple.empty();
|
|
}
|
|
conf_has_signal_sources_ = (num_src == src_count);
|
|
}
|
|
else
|
|
{
|
|
conf_has_signal_sources_ = true;
|
|
}
|
|
|
|
std::string pvt_impl = configuration_->property("PVT.implementation", empty_implementation);
|
|
conf_has_pvt_ = !pvt_impl.empty();
|
|
|
|
std::string obs_impl = configuration_->property("Observables.implementation", empty_implementation);
|
|
conf_has_observables_ = !obs_impl.empty();
|
|
|
|
well_formatted_configuration_ = conf_file_has_section_ && conf_file_has_mandatory_globals_ && conf_has_signal_sources_ && conf_has_observables_ && conf_has_pvt_;
|
|
|
|
restart_ = false;
|
|
init();
|
|
}
|
|
|
|
|
|
ControlThread::ControlThread(std::shared_ptr<ConfigurationInterface> configuration)
|
|
: configuration_(std::move(configuration)),
|
|
well_formatted_configuration_(true),
|
|
conf_file_has_section_(true),
|
|
conf_file_has_mandatory_globals_(true),
|
|
conf_has_signal_sources_(true),
|
|
conf_has_observables_(true),
|
|
conf_has_pvt_(true),
|
|
restart_(false)
|
|
{
|
|
init();
|
|
}
|
|
|
|
|
|
void ControlThread::init()
|
|
{
|
|
telecommand_enabled_ = configuration_->property("GNSS-SDR.telecommand_enabled", false);
|
|
// OPTIONAL: specify a custom year to override the system time in order to postprocess old gnss records and avoid wrong week rollover
|
|
pre_2009_file_ = configuration_->property("GNSS-SDR.pre_2009_file", false);
|
|
// Instantiates a control queue, a GNSS flowgraph, and a control message factory
|
|
control_queue_ = std::make_shared<Concurrent_Queue<pmt::pmt_t>>();
|
|
cmd_interface_.set_msg_queue(control_queue_); // set also the queue pointer for the telecommand thread
|
|
if (well_formatted_configuration_)
|
|
{
|
|
try
|
|
{
|
|
flowgraph_ = std::make_shared<GNSSFlowgraph>(configuration_, control_queue_);
|
|
}
|
|
catch (const boost::bad_lexical_cast &e)
|
|
{
|
|
std::cout << "Caught bad lexical cast with error " << e.what() << '\n';
|
|
}
|
|
}
|
|
else
|
|
{
|
|
flowgraph_ = nullptr;
|
|
}
|
|
|
|
stop_ = false;
|
|
processed_control_messages_ = 0;
|
|
applied_actions_ = 0;
|
|
supl_mcc_ = 0;
|
|
supl_mns_ = 0;
|
|
supl_lac_ = 0;
|
|
supl_ci_ = 0;
|
|
msqid_ = -1;
|
|
agnss_ref_location_ = Agnss_Ref_Location();
|
|
agnss_ref_time_ = Agnss_Ref_Time();
|
|
|
|
const std::string empty_string;
|
|
const std::string ref_location_str = configuration_->property("GNSS-SDR.AGNSS_ref_location", empty_string);
|
|
const std::string ref_time_str = configuration_->property("GNSS-SDR.AGNSS_ref_utc_time", empty_string);
|
|
if (ref_location_str != empty_string)
|
|
{
|
|
std::vector<double> vect;
|
|
std::stringstream ss(ref_location_str);
|
|
double d;
|
|
while (ss >> d)
|
|
{
|
|
vect.push_back(d);
|
|
if ((ss.peek() == ',') or (ss.peek() == ' '))
|
|
{
|
|
ss.ignore();
|
|
}
|
|
}
|
|
// fill agnss_ref_location_
|
|
if (vect.size() >= 2)
|
|
{
|
|
if ((vect[0] < 90.0) and (vect[0] > -90) and (vect[1] < 180.0) and (vect[1] > -180.0))
|
|
{
|
|
agnss_ref_location_.lat = vect[0];
|
|
agnss_ref_location_.lon = vect[1];
|
|
agnss_ref_location_.valid = true;
|
|
}
|
|
else
|
|
{
|
|
std::cerr << "GNSS-SDR.AGNSS_ref_location=" << ref_location_str << " is not a valid position.\n";
|
|
agnss_ref_location_.valid = false;
|
|
}
|
|
}
|
|
}
|
|
if (ref_time_str == empty_string)
|
|
{
|
|
// Make an educated guess
|
|
time_t rawtime;
|
|
time(&rawtime);
|
|
agnss_ref_time_.seconds = rawtime;
|
|
agnss_ref_time_.valid = true;
|
|
}
|
|
else
|
|
{
|
|
// fill agnss_ref_time_
|
|
struct tm tm
|
|
{
|
|
};
|
|
if (strptime(ref_time_str.c_str(), "%d/%m/%Y %H:%M:%S", &tm) != nullptr)
|
|
{
|
|
agnss_ref_time_.seconds = timegm(&tm);
|
|
if (agnss_ref_time_.seconds > 0)
|
|
{
|
|
agnss_ref_time_.valid = true;
|
|
}
|
|
else
|
|
{
|
|
std::cerr << "GNSS-SDR.AGNSS_ref_utc_time=" << ref_time_str << " is not well-formed. Please use four digits for the year: DD/MM/YYYY HH:MM:SS\n";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::cerr << "GNSS-SDR.AGNSS_ref_utc_time=" << ref_time_str << " is not well-formed. Should be DD/MM/YYYY HH:MM:SS in UTC\n";
|
|
agnss_ref_time_.valid = false;
|
|
}
|
|
}
|
|
|
|
receiver_on_standby_ = false;
|
|
}
|
|
|
|
|
|
ControlThread::~ControlThread() // NOLINT(modernize-use-equals-default)
|
|
{
|
|
DLOG(INFO) << "Control Thread destructor called";
|
|
if (msqid_ != -1)
|
|
{
|
|
msgctl(msqid_, IPC_RMID, nullptr);
|
|
}
|
|
|
|
if (sysv_queue_thread_.joinable())
|
|
{
|
|
sysv_queue_thread_.join();
|
|
}
|
|
|
|
if (cmd_interface_thread_.joinable())
|
|
{
|
|
cmd_interface_thread_.join();
|
|
}
|
|
}
|
|
|
|
|
|
void ControlThread::telecommand_listener()
|
|
{
|
|
if (telecommand_enabled_)
|
|
{
|
|
const int tcp_cmd_port = configuration_->property("GNSS-SDR.telecommand_tcp_port", 3333);
|
|
cmd_interface_.run_cmd_server(tcp_cmd_port);
|
|
}
|
|
}
|
|
|
|
|
|
void ControlThread::event_dispatcher(bool &valid_event, pmt::pmt_t &msg)
|
|
{
|
|
if (valid_event)
|
|
{
|
|
processed_control_messages_++;
|
|
const size_t msg_type_hash_code = pmt::any_ref(msg).type().hash_code();
|
|
if (msg_type_hash_code == channel_event_type_hash_code_)
|
|
{
|
|
if (receiver_on_standby_ == false)
|
|
{
|
|
const auto new_event = wht::any_cast<channel_event_sptr>(pmt::any_ref(msg));
|
|
DLOG(INFO) << "New channel event rx from ch id: " << new_event->channel_id
|
|
<< " what: " << new_event->event_type;
|
|
flowgraph_->apply_action(new_event->channel_id, new_event->event_type);
|
|
}
|
|
}
|
|
else if (msg_type_hash_code == command_event_type_hash_code_)
|
|
{
|
|
const auto new_event = wht::any_cast<command_event_sptr>(pmt::any_ref(msg));
|
|
DLOG(INFO) << "New command event rx from ch id: " << new_event->command_id
|
|
<< " what: " << new_event->event_type;
|
|
|
|
if (new_event->command_id == 200)
|
|
{
|
|
apply_action(new_event->event_type);
|
|
}
|
|
else
|
|
{
|
|
if (new_event->command_id == 300) // some TC commands require also actions from control_thread
|
|
{
|
|
apply_action(new_event->event_type);
|
|
}
|
|
flowgraph_->apply_action(new_event->command_id, new_event->event_type);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
DLOG(INFO) << "Control Queue: unknown object type!\n";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (receiver_on_standby_ == false)
|
|
{
|
|
// perform non-priority tasks
|
|
flowgraph_->acquisition_manager(0); // start acquisition of untracked satellites
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
* Runs the control thread that manages the receiver control plane
|
|
*
|
|
* This is the main loop that reads and process the control messages
|
|
* 1- Connect the GNSS receiver flowgraph
|
|
* 2- Start the GNSS receiver flowgraph
|
|
* while (flowgraph_->running() && !stop)_{
|
|
* 3- Read control messages and process them }
|
|
*/
|
|
int ControlThread::run()
|
|
{
|
|
// Connect the flowgraph
|
|
if (!flowgraph_)
|
|
{
|
|
print_help_at_exit();
|
|
return 0;
|
|
}
|
|
try
|
|
{
|
|
flowgraph_->connect();
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
LOG(ERROR) << e.what();
|
|
return 0;
|
|
}
|
|
if (flowgraph_->connected())
|
|
{
|
|
LOG(INFO) << "Flowgraph connected";
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
// Start the flowgraph
|
|
flowgraph_->start();
|
|
if (flowgraph_->running())
|
|
{
|
|
LOG(INFO) << "Flowgraph started";
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
// launch GNSS assistance process AFTER the flowgraph is running because the GNU Radio asynchronous queues must be already running to transport msgs
|
|
assist_GNSS();
|
|
// start the keyboard_listener thread
|
|
if (FLAGS_keyboard)
|
|
{
|
|
keyboard_thread_ = std::thread(&ControlThread::keyboard_listener, this);
|
|
}
|
|
sysv_queue_thread_ = std::thread(&ControlThread::sysv_queue_listener, this);
|
|
|
|
// start the telecommand listener thread
|
|
cmd_interface_.set_pvt(flowgraph_->get_pvt());
|
|
cmd_interface_thread_ = std::thread(&ControlThread::telecommand_listener, this);
|
|
|
|
#ifdef ENABLE_FPGA
|
|
// Create a task for the acquisition such that id doesn't block the flow of the control thread
|
|
fpga_helper_thread_ = boost::thread(&GNSSFlowgraph::start_acquisition_helper,
|
|
flowgraph_);
|
|
#endif
|
|
// Main loop to read and process the control messages
|
|
pmt::pmt_t msg;
|
|
while (flowgraph_->running() && !stop_)
|
|
{
|
|
// read event messages, triggered by event signaling with a 100 ms timeout to perform low priority receiver management tasks
|
|
bool valid_event = control_queue_->timed_wait_and_pop(msg, 100);
|
|
// call the new sat dispatcher and receiver controller
|
|
event_dispatcher(valid_event, msg);
|
|
}
|
|
std::cout << "Stopping GNSS-SDR, please wait!\n";
|
|
flowgraph_->stop();
|
|
stop_ = true;
|
|
flowgraph_->disconnect();
|
|
|
|
#ifdef ENABLE_FPGA
|
|
// trigger a HW reset
|
|
// The HW reset causes any HW accelerator module that is waiting for more samples to complete its calculations
|
|
// to trigger an interrupt and finish its signal processing tasks immediately. In this way all SW threads that
|
|
// are waiting for interrupts in the HW can exit in a normal way.
|
|
flowgraph_->perform_hw_reset();
|
|
fpga_helper_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
|
|
#endif
|
|
|
|
// Terminate keyboard thread
|
|
if (FLAGS_keyboard && keyboard_thread_.joinable())
|
|
{
|
|
pthread_t id = keyboard_thread_.native_handle();
|
|
keyboard_thread_.detach();
|
|
#ifndef ANDROID
|
|
pthread_cancel(id);
|
|
#else
|
|
// todo: find alternative
|
|
#endif
|
|
}
|
|
|
|
// Terminate telecommand thread
|
|
if (telecommand_enabled_)
|
|
{
|
|
pthread_t id2 = cmd_interface_thread_.native_handle();
|
|
cmd_interface_thread_.detach();
|
|
#ifndef ANDROID
|
|
pthread_cancel(id2);
|
|
#else
|
|
// todo: find alternative
|
|
#endif
|
|
}
|
|
|
|
LOG(INFO) << "Flowgraph stopped";
|
|
|
|
if (restart_)
|
|
{
|
|
return 42; // signal the gnss-sdr-harness.sh to restart the receiver program
|
|
}
|
|
|
|
return 0; // normal shutdown
|
|
}
|
|
|
|
|
|
void ControlThread::set_control_queue(std::shared_ptr<Concurrent_Queue<pmt::pmt_t>> control_queue)
|
|
{
|
|
if (flowgraph_->running())
|
|
{
|
|
LOG(WARNING) << "Unable to set control queue while flowgraph is running";
|
|
return;
|
|
}
|
|
control_queue_ = std::move(control_queue);
|
|
cmd_interface_.set_msg_queue(control_queue_);
|
|
}
|
|
|
|
|
|
/*
|
|
* Returns true if reading was successful
|
|
*/
|
|
bool ControlThread::read_assistance_from_XML()
|
|
{
|
|
// return variable (true == succeeded)
|
|
bool ret = false;
|
|
// getting names from the config file, if available
|
|
std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename_);
|
|
std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model_xml", utc_default_xml_filename_);
|
|
std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename_);
|
|
std::string gal_iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_iono_xml", gal_iono_default_xml_filename_);
|
|
std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename_);
|
|
std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename_);
|
|
std::string eph_gal_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_ephemeris_xml", eph_gal_default_xml_filename_);
|
|
std::string eph_cnav_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_cnav_ephemeris_xml", eph_cnav_default_xml_filename_);
|
|
std::string gal_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_utc_model_xml", gal_utc_default_xml_filename_);
|
|
std::string cnav_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_cnav_utc_model_xml", cnav_utc_default_xml_filename_);
|
|
std::string eph_glo_xml_filename = configuration_->property("GNSS-SDR.SUPL_glo_ephemeris_xml", eph_glo_gnav_default_xml_filename_);
|
|
std::string glo_utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_glo_utc_model_xml", glo_utc_default_xml_filename_);
|
|
std::string gal_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gal_almanac_xml", gal_almanac_default_xml_filename_);
|
|
std::string gps_almanac_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_almanac_xml", gps_almanac_default_xml_filename_);
|
|
|
|
if (configuration_->property("GNSS-SDR.AGNSS_XML_enabled", false) == true)
|
|
{
|
|
eph_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_ephemeris_xml", eph_default_xml_filename_);
|
|
utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_utc_model_xml", utc_default_xml_filename_);
|
|
iono_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_iono_xml", iono_default_xml_filename_);
|
|
gal_iono_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_iono_xml", gal_iono_default_xml_filename_);
|
|
ref_time_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_ref_time_xml", ref_time_default_xml_filename_);
|
|
ref_location_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_ref_location_xml", ref_location_default_xml_filename_);
|
|
eph_gal_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_ephemeris_xml", eph_gal_default_xml_filename_);
|
|
eph_cnav_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_cnav_ephemeris_xml", eph_cnav_default_xml_filename_);
|
|
gal_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_utc_model_xml", gal_utc_default_xml_filename_);
|
|
cnav_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_cnav_utc_model_xml", cnav_utc_default_xml_filename_);
|
|
eph_glo_xml_filename = configuration_->property("GNSS-SDR.AGNSS_glo_ephemeris_xml", eph_glo_gnav_default_xml_filename_);
|
|
glo_utc_xml_filename = configuration_->property("GNSS-SDR.AGNSS_glo_utc_model_xml", glo_utc_default_xml_filename_);
|
|
gal_almanac_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gal_almanac_xml", gal_almanac_default_xml_filename_);
|
|
gps_almanac_xml_filename = configuration_->property("GNSS-SDR.AGNSS_gps_almanac_xml", gps_almanac_default_xml_filename_);
|
|
}
|
|
|
|
std::cout << "Trying to read GNSS ephemeris from XML file(s)...\n";
|
|
|
|
if (configuration_->property("Channels_1C.count", 0) > 0)
|
|
{
|
|
if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true)
|
|
{
|
|
std::map<int, Gps_Ephemeris>::const_iterator gps_eph_iter;
|
|
for (gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.cbegin();
|
|
gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.cend();
|
|
gps_eph_iter++)
|
|
{
|
|
std::cout << "From XML file: Read NAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_eph_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(gps_eph_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_acquisition_.load_utc_xml(utc_xml_filename) == true)
|
|
{
|
|
const std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(supl_client_acquisition_.gps_utc);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
std::cout << "From XML file: Read GPS UTC model parameters.\n";
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_acquisition_.load_iono_xml(iono_xml_filename) == true)
|
|
{
|
|
const std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(supl_client_acquisition_.gps_iono);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
std::cout << "From XML file: Read GPS ionosphere model parameters.\n";
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_ephemeris_.load_gps_almanac_xml(gps_almanac_xml_filename) == true)
|
|
{
|
|
std::map<int, Gps_Almanac>::const_iterator gps_alm_iter;
|
|
for (gps_alm_iter = supl_client_ephemeris_.gps_almanac_map.cbegin();
|
|
gps_alm_iter != supl_client_ephemeris_.gps_almanac_map.cend();
|
|
gps_alm_iter++)
|
|
{
|
|
std::cout << "From XML file: Read GPS almanac for satellite " << Gnss_Satellite("GPS", gps_alm_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Gps_Almanac> tmp_obj = std::make_shared<Gps_Almanac>(gps_alm_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
ret = true;
|
|
}
|
|
}
|
|
|
|
if ((configuration_->property("Channels_1B.count", 0) > 0) or (configuration_->property("Channels_5X.count", 0) > 0))
|
|
{
|
|
if (supl_client_ephemeris_.load_gal_ephemeris_xml(eph_gal_xml_filename) == true)
|
|
{
|
|
std::map<int, Galileo_Ephemeris>::const_iterator gal_eph_iter;
|
|
for (gal_eph_iter = supl_client_ephemeris_.gal_ephemeris_map.cbegin();
|
|
gal_eph_iter != supl_client_ephemeris_.gal_ephemeris_map.cend();
|
|
gal_eph_iter++)
|
|
{
|
|
std::cout << "From XML file: Read ephemeris for satellite " << Gnss_Satellite("Galileo", gal_eph_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(gal_eph_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_acquisition_.load_gal_iono_xml(gal_iono_xml_filename) == true)
|
|
{
|
|
const std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(supl_client_acquisition_.gal_iono);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
std::cout << "From XML file: Read Galileo ionosphere model parameters.\n";
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_acquisition_.load_gal_utc_xml(gal_utc_xml_filename) == true)
|
|
{
|
|
const std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(supl_client_acquisition_.gal_utc);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
std::cout << "From XML file: Read Galileo UTC model parameters.\n";
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_ephemeris_.load_gal_almanac_xml(gal_almanac_xml_filename) == true)
|
|
{
|
|
std::map<int, Galileo_Almanac>::const_iterator gal_alm_iter;
|
|
for (gal_alm_iter = supl_client_ephemeris_.gal_almanac_map.cbegin();
|
|
gal_alm_iter != supl_client_ephemeris_.gal_almanac_map.cend();
|
|
gal_alm_iter++)
|
|
{
|
|
std::cout << "From XML file: Read Galileo almanac for satellite " << Gnss_Satellite("Galileo", gal_alm_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Galileo_Almanac> tmp_obj = std::make_shared<Galileo_Almanac>(gal_alm_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
ret = true;
|
|
}
|
|
}
|
|
|
|
if ((configuration_->property("Channels_2S.count", 0) > 0) or (configuration_->property("Channels_L5.count", 0) > 0))
|
|
{
|
|
if (supl_client_ephemeris_.load_cnav_ephemeris_xml(eph_cnav_xml_filename) == true)
|
|
{
|
|
std::map<int, Gps_CNAV_Ephemeris>::const_iterator gps_cnav_eph_iter;
|
|
for (gps_cnav_eph_iter = supl_client_ephemeris_.gps_cnav_ephemeris_map.cbegin();
|
|
gps_cnav_eph_iter != supl_client_ephemeris_.gps_cnav_ephemeris_map.cend();
|
|
gps_cnav_eph_iter++)
|
|
{
|
|
std::cout << "From XML file: Read CNAV ephemeris for satellite " << Gnss_Satellite("GPS", gps_cnav_eph_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(gps_cnav_eph_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_acquisition_.load_cnav_utc_xml(cnav_utc_xml_filename) == true)
|
|
{
|
|
const std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(supl_client_acquisition_.gps_cnav_utc);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
std::cout << "From XML file: Read GPS CNAV UTC model parameters.\n";
|
|
ret = true;
|
|
}
|
|
}
|
|
|
|
if ((configuration_->property("Channels_1G.count", 0) > 0) or (configuration_->property("Channels_2G.count", 0) > 0))
|
|
{
|
|
if (supl_client_ephemeris_.load_gnav_ephemeris_xml(eph_glo_xml_filename) == true)
|
|
{
|
|
std::map<int, Glonass_Gnav_Ephemeris>::const_iterator glo_gnav_eph_iter;
|
|
for (glo_gnav_eph_iter = supl_client_ephemeris_.glonass_gnav_ephemeris_map.cbegin();
|
|
glo_gnav_eph_iter != supl_client_ephemeris_.glonass_gnav_ephemeris_map.cend();
|
|
glo_gnav_eph_iter++)
|
|
{
|
|
std::cout << "From XML file: Read GLONASS GNAV ephemeris for satellite " << Gnss_Satellite("GLONASS", glo_gnav_eph_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(glo_gnav_eph_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
ret = true;
|
|
}
|
|
|
|
if (supl_client_acquisition_.load_glo_utc_xml(glo_utc_xml_filename) == true)
|
|
{
|
|
const std::shared_ptr<Glonass_Gnav_Utc_Model> tmp_obj = std::make_shared<Glonass_Gnav_Utc_Model>(supl_client_acquisition_.glo_gnav_utc);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
std::cout << "From XML file: Read GLONASS UTC model parameters.\n";
|
|
ret = true;
|
|
}
|
|
}
|
|
|
|
if (ret == false)
|
|
{
|
|
std::cout << "Error reading XML files\n";
|
|
std::cout << "Disabling GNSS assistance...\n";
|
|
}
|
|
|
|
// Only look for {ref time, ref location} if SUPL is enabled
|
|
const bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
|
|
if (enable_gps_supl_assistance == true)
|
|
{
|
|
// Try to read Ref Time from XML
|
|
if (supl_client_acquisition_.load_ref_time_xml(ref_time_xml_filename) == true)
|
|
{
|
|
LOG(INFO) << "SUPL: Read XML Ref Time";
|
|
const std::shared_ptr<Agnss_Ref_Time> tmp_obj = std::make_shared<Agnss_Ref_Time>(supl_client_acquisition_.gps_time);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
else
|
|
{
|
|
LOG(INFO) << "SUPL: could not read Ref Time XML";
|
|
}
|
|
|
|
// Try to read Ref Location from XML
|
|
if (supl_client_acquisition_.load_ref_location_xml(ref_location_xml_filename) == true)
|
|
{
|
|
LOG(INFO) << "SUPL: Read XML Ref Location";
|
|
const std::shared_ptr<Agnss_Ref_Location> tmp_obj = std::make_shared<Agnss_Ref_Location>(supl_client_acquisition_.gps_ref_loc);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
else
|
|
{
|
|
LOG(INFO) << "SUPL: could not read Ref Location XML";
|
|
}
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
|
|
void ControlThread::assist_GNSS()
|
|
{
|
|
// ######### GNSS Assistance #################################
|
|
// GNSS Assistance configuration
|
|
const bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
|
|
const bool enable_agnss_xml = configuration_->property("GNSS-SDR.AGNSS_XML_enabled", false);
|
|
if ((enable_gps_supl_assistance == true) and (enable_agnss_xml == false))
|
|
{
|
|
std::cout << "SUPL RRLP GPS assistance enabled!\n";
|
|
const std::string default_acq_server("supl.google.com");
|
|
const std::string default_eph_server("supl.google.com");
|
|
supl_client_ephemeris_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_server", default_acq_server);
|
|
supl_client_acquisition_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_server", default_eph_server);
|
|
supl_client_ephemeris_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_port", 7275);
|
|
supl_client_acquisition_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_port", 7275);
|
|
supl_mcc_ = configuration_->property("GNSS-SDR.SUPL_MCC", 244);
|
|
supl_mns_ = configuration_->property("GNSS-SDR.SUPL_MNC ", 5);
|
|
|
|
const std::string default_lac("0x59e2");
|
|
const std::string default_ci("0x31b0");
|
|
const std::string supl_lac_s = configuration_->property("GNSS-SDR.SUPL_LAC", default_lac);
|
|
const std::string supl_ci_s = configuration_->property("GNSS-SDR.SUPL_CI", default_ci);
|
|
try
|
|
{
|
|
supl_lac_ = std::stoi(supl_lac_s, nullptr, 0);
|
|
}
|
|
catch (const std::invalid_argument &ia)
|
|
{
|
|
std::cerr << "Invalid argument for SUPL LAC: " << ia.what() << '\n';
|
|
supl_lac_ = -1;
|
|
}
|
|
try
|
|
{
|
|
supl_ci_ = std::stoi(supl_ci_s, nullptr, 0);
|
|
}
|
|
catch (const std::invalid_argument &ia)
|
|
{
|
|
std::cerr << "Invalid argument for SUPL CI: " << ia.what() << '\n';
|
|
supl_ci_ = -1;
|
|
}
|
|
|
|
if (supl_lac_ < 0 or supl_lac_ > 65535)
|
|
{
|
|
supl_lac_ = 0x59e2;
|
|
}
|
|
|
|
if (supl_ci_ < 0 or supl_ci_ > 268435455) // 2^16 for GSM and CDMA, 2^28 for UMTS and LTE networks
|
|
{
|
|
supl_ci_ = 0x31b0;
|
|
}
|
|
|
|
const bool SUPL_read_gps_assistance_xml = configuration_->property("GNSS-SDR.SUPL_read_gps_assistance_xml", false);
|
|
if (SUPL_read_gps_assistance_xml == true)
|
|
{
|
|
// Read assistance from file
|
|
if (read_assistance_from_XML())
|
|
{
|
|
std::cout << "GNSS assistance data loaded from local XML file(s).\n";
|
|
std::cout << "No SUPL request has been performed.\n";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Request ephemeris from SUPL server
|
|
supl_client_ephemeris_.request = 1;
|
|
std::cout << "SUPL: Try to read GPS ephemeris data from SUPL server...\n";
|
|
int error = supl_client_ephemeris_.get_assistance(supl_mcc_, supl_mns_, supl_lac_, supl_ci_);
|
|
if (error == 0)
|
|
{
|
|
std::map<int, Gps_Ephemeris>::const_iterator gps_eph_iter;
|
|
for (gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.cbegin();
|
|
gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.cend();
|
|
gps_eph_iter++)
|
|
{
|
|
std::cout << "SUPL: Received ephemeris data for satellite " << Gnss_Satellite("GPS", gps_eph_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(gps_eph_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
// Save ephemeris to XML file
|
|
const std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename_);
|
|
if (supl_client_ephemeris_.save_ephemeris_map_xml(eph_xml_filename, supl_client_ephemeris_.gps_ephemeris_map) == true)
|
|
{
|
|
std::cout << "SUPL: XML ephemeris data file created\n";
|
|
}
|
|
else
|
|
{
|
|
std::cout << "SUPL: Failed to create XML ephemeris data file\n";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::cout << "ERROR: SUPL client request for ephemeris data returned " << error << '\n';
|
|
std::cout << "Please check your network connectivity and SUPL server configuration\n";
|
|
std::cout << "Trying to read AGNSS data from local XML file(s)...\n";
|
|
if (read_assistance_from_XML() == false)
|
|
{
|
|
std::cout << "ERROR: Could not read XML files: Disabling SUPL assistance.\n";
|
|
}
|
|
}
|
|
|
|
// Request almanac, IONO and UTC Model data
|
|
supl_client_ephemeris_.request = 0;
|
|
std::cout << "SUPL: Try to read Almanac, Iono, Utc Model, Ref Time and Ref Location data from SUPL server...\n";
|
|
error = supl_client_ephemeris_.get_assistance(supl_mcc_, supl_mns_, supl_lac_, supl_ci_);
|
|
if (error == 0)
|
|
{
|
|
std::map<int, Gps_Almanac>::const_iterator gps_alm_iter;
|
|
for (gps_alm_iter = supl_client_ephemeris_.gps_almanac_map.cbegin();
|
|
gps_alm_iter != supl_client_ephemeris_.gps_almanac_map.cend();
|
|
gps_alm_iter++)
|
|
{
|
|
std::cout << "SUPL: Received almanac data for satellite " << Gnss_Satellite("GPS", gps_alm_iter->second.PRN) << '\n';
|
|
const std::shared_ptr<Gps_Almanac> tmp_obj = std::make_shared<Gps_Almanac>(gps_alm_iter->second);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
supl_client_ephemeris_.save_gps_almanac_xml("gps_almanac_map.xml", supl_client_ephemeris_.gps_almanac_map);
|
|
if (supl_client_ephemeris_.gps_iono.valid == true)
|
|
{
|
|
std::cout << "SUPL: Received GPS Ionosphere model parameters\n";
|
|
const std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(supl_client_ephemeris_.gps_iono);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
if (supl_client_ephemeris_.gps_utc.valid == true)
|
|
{
|
|
std::cout << "SUPL: Received GPS UTC model parameters\n";
|
|
const std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(supl_client_ephemeris_.gps_utc);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
}
|
|
// Save iono and UTC model data to xml file
|
|
const std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename_);
|
|
if (supl_client_ephemeris_.save_iono_xml(iono_xml_filename, supl_client_ephemeris_.gps_iono) == true)
|
|
{
|
|
std::cout << "SUPL: Iono data file created\n";
|
|
}
|
|
else
|
|
{
|
|
std::cout << "SUPL: Failed to create Iono data file\n";
|
|
}
|
|
const std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model_xml", utc_default_xml_filename_);
|
|
if (supl_client_ephemeris_.save_utc_xml(utc_xml_filename, supl_client_ephemeris_.gps_utc) == true)
|
|
{
|
|
std::cout << "SUPL: UTC model data file created\n";
|
|
}
|
|
else
|
|
{
|
|
std::cout << "SUPL: Failed to create UTC model data file\n";
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::cout << "ERROR: SUPL client for almanac data returned " << error << '\n';
|
|
std::cout << "Please check your network connectivity and SUPL server configuration\n";
|
|
}
|
|
|
|
// Request acquisition assistance
|
|
supl_client_acquisition_.request = 2;
|
|
std::cout << "SUPL: Try to read acquisition assistance data from SUPL server...\n";
|
|
error = supl_client_acquisition_.get_assistance(supl_mcc_, supl_mns_, supl_lac_, supl_ci_);
|
|
if (error == 0)
|
|
{
|
|
std::map<int, Gps_Acq_Assist>::const_iterator gps_acq_iter;
|
|
for (gps_acq_iter = supl_client_acquisition_.gps_acq_map.cbegin();
|
|
gps_acq_iter != supl_client_acquisition_.gps_acq_map.cend();
|
|
gps_acq_iter++)
|
|
{
|
|
std::cout << "SUPL: Received acquisition assistance data for satellite " << Gnss_Satellite("GPS", gps_acq_iter->second.PRN) << '\n';
|
|
global_gps_acq_assist_map.write(gps_acq_iter->second.PRN, gps_acq_iter->second);
|
|
}
|
|
if (supl_client_acquisition_.gps_ref_loc.valid == true)
|
|
{
|
|
std::cout << "SUPL: Received Ref Location data (Acquisition Assistance)\n";
|
|
agnss_ref_location_ = supl_client_acquisition_.gps_ref_loc;
|
|
const std::shared_ptr<Agnss_Ref_Location> tmp_obj = std::make_shared<Agnss_Ref_Location>(agnss_ref_location_);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
supl_client_acquisition_.save_ref_location_xml("agnss_ref_location.xml", agnss_ref_location_);
|
|
}
|
|
if (supl_client_acquisition_.gps_time.valid == true)
|
|
{
|
|
std::cout << "SUPL: Received Ref Time data (Acquisition Assistance)\n";
|
|
agnss_ref_time_ = supl_client_acquisition_.gps_time;
|
|
const std::shared_ptr<Agnss_Ref_Time> tmp_obj = std::make_shared<Agnss_Ref_Time>(agnss_ref_time_);
|
|
flowgraph_->send_telemetry_msg(pmt::make_any(tmp_obj));
|
|
supl_client_acquisition_.save_ref_time_xml("agnss_ref_time.xml", agnss_ref_time_);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::cout << "ERROR: SUPL client for acquisition assistance returned " << error << '\n';
|
|
std::cout << "Please check your network connectivity and SUPL server configuration\n";
|
|
std::cout << "Disabling SUPL acquisition assistance.\n";
|
|
}
|
|
}
|
|
}
|
|
|
|
if ((enable_gps_supl_assistance == false) and (enable_agnss_xml == true))
|
|
{
|
|
// read assistance from file
|
|
if (read_assistance_from_XML())
|
|
{
|
|
std::cout << "GNSS assistance data loaded from local XML file(s).\n";
|
|
}
|
|
}
|
|
|
|
// If AGNSS is enabled, make use of it
|
|
if ((agnss_ref_location_.valid == true) and ((enable_gps_supl_assistance == true) or (enable_agnss_xml == true)))
|
|
{
|
|
// Get the list of visible satellites
|
|
std::array<float, 3> ref_LLH{};
|
|
ref_LLH[0] = agnss_ref_location_.lat;
|
|
ref_LLH[1] = agnss_ref_location_.lon;
|
|
time_t ref_rx_utc_time = 0;
|
|
if (agnss_ref_time_.valid == true)
|
|
{
|
|
ref_rx_utc_time = static_cast<time_t>(agnss_ref_time_.seconds);
|
|
}
|
|
|
|
const std::vector<std::pair<int, Gnss_Satellite>> visible_sats = get_visible_sats(ref_rx_utc_time, ref_LLH);
|
|
// Set the receiver in Standby mode
|
|
flowgraph_->apply_action(0, 10);
|
|
// Give priority to visible satellites in the search list
|
|
flowgraph_->priorize_satellites(visible_sats);
|
|
// Hot Start
|
|
flowgraph_->apply_action(0, 12);
|
|
}
|
|
}
|
|
|
|
|
|
void ControlThread::apply_action(unsigned int what)
|
|
{
|
|
std::shared_ptr<PvtInterface> pvt_ptr;
|
|
std::vector<std::pair<int, Gnss_Satellite>> visible_satellites;
|
|
applied_actions_++;
|
|
switch (what)
|
|
{
|
|
case 0:
|
|
LOG(INFO) << "Received action STOP";
|
|
stop_ = true;
|
|
break;
|
|
case 1:
|
|
LOG(INFO) << "Received action RESTART";
|
|
stop_ = true;
|
|
restart_ = true;
|
|
break;
|
|
case 10: // request standby mode
|
|
LOG(INFO) << "TC request standby mode";
|
|
receiver_on_standby_ = true;
|
|
break;
|
|
case 11:
|
|
LOG(INFO) << "Receiver action COLDSTART";
|
|
// delete all ephemeris and almanac information from maps (also the PVT map queue)
|
|
pvt_ptr = flowgraph_->get_pvt();
|
|
pvt_ptr->clear_ephemeris();
|
|
// todo: reorder the satellite queues to the receiver default startup order.
|
|
// This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
|
|
// start again the satellite acquisitions
|
|
receiver_on_standby_ = false;
|
|
break;
|
|
case 12:
|
|
LOG(INFO) << "Receiver action HOTSTART";
|
|
visible_satellites = get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
|
|
// reorder the satellite queue to acquire first those visible satellites
|
|
flowgraph_->priorize_satellites(visible_satellites);
|
|
// start again the satellite acquisitions
|
|
receiver_on_standby_ = false;
|
|
break;
|
|
case 13:
|
|
LOG(INFO) << "Receiver action WARMSTART";
|
|
// delete all ephemeris and almanac information from maps (also the PVT map queue)
|
|
pvt_ptr = flowgraph_->get_pvt();
|
|
pvt_ptr->clear_ephemeris();
|
|
// load the ephemeris and the almanac from XML files (receiver assistance)
|
|
read_assistance_from_XML();
|
|
// call here the function that computes the set of visible satellites and its elevation
|
|
// for the date and time specified by the warm start command and the assisted position
|
|
get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
|
|
// reorder the satellite queue to acquire first those visible satellites
|
|
flowgraph_->priorize_satellites(visible_satellites);
|
|
// start again the satellite acquisitions
|
|
receiver_on_standby_ = false;
|
|
break;
|
|
default:
|
|
LOG(INFO) << "Unrecognized action.";
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, const std::array<float, 3> &LLH)
|
|
{
|
|
// 1. Compute rx ECEF position from LLH WGS84
|
|
const arma::vec LLH_rad = arma::vec{degtorad(LLH[0]), degtorad(LLH[1]), LLH[2]};
|
|
arma::mat C_tmp = arma::zeros(3, 3);
|
|
arma::vec r_eb_e = arma::zeros(3, 1);
|
|
arma::vec v_eb_e = arma::zeros(3, 1);
|
|
Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp);
|
|
|
|
// 2. Compute rx GPS time from UTC time
|
|
gtime_t utc_gtime;
|
|
utc_gtime.time = rx_utc_time;
|
|
utc_gtime.sec = 0.0;
|
|
const gtime_t gps_gtime = utc2gpst(utc_gtime);
|
|
|
|
// 3. loop through all the available ephemeris or almanac and compute satellite positions and elevations
|
|
// store visible satellites in a vector of pairs <int,Gnss_Satellite> to associate an elevation to the each satellite
|
|
std::vector<std::pair<int, Gnss_Satellite>> available_satellites;
|
|
std::vector<unsigned int> visible_gps;
|
|
std::vector<unsigned int> visible_gal;
|
|
const std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
|
|
struct tm tstruct
|
|
{
|
|
};
|
|
char buf[80];
|
|
tstruct = *gmtime(&rx_utc_time);
|
|
strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct);
|
|
const std::string str_time = std::string(buf);
|
|
std::cout << "Get visible satellites at " << str_time
|
|
<< "UTC, assuming RX position " << LLH[0] << " [deg], " << LLH[1] << " [deg], " << LLH[2] << " [m]\n";
|
|
|
|
const std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris();
|
|
for (const auto &it : gps_eph_map)
|
|
{
|
|
const eph_t rtklib_eph = eph_to_rtklib(it.second, pre_2009_file_);
|
|
std::array<double, 3> r_sat{};
|
|
double clock_bias_s;
|
|
double sat_pos_variance_m2;
|
|
eph2pos(gps_gtime, &rtklib_eph, r_sat.data(), &clock_bias_s,
|
|
&sat_pos_variance_m2);
|
|
double Az;
|
|
double El;
|
|
double dist_m;
|
|
const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
|
|
const arma::vec dx = r_sat_eb_e - r_eb_e;
|
|
topocent(&Az, &El, &dist_m, r_eb_e, dx);
|
|
// push sat
|
|
if (El > 0)
|
|
{
|
|
std::cout << "Using GPS Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n';
|
|
available_satellites.emplace_back(floor(El),
|
|
(Gnss_Satellite(std::string("GPS"), it.second.PRN)));
|
|
visible_gps.push_back(it.second.PRN);
|
|
}
|
|
}
|
|
|
|
const std::map<int, Galileo_Ephemeris> gal_eph_map = pvt_ptr->get_galileo_ephemeris();
|
|
for (const auto &it : gal_eph_map)
|
|
{
|
|
const eph_t rtklib_eph = eph_to_rtklib(it.second);
|
|
std::array<double, 3> r_sat{};
|
|
double clock_bias_s;
|
|
double sat_pos_variance_m2;
|
|
eph2pos(gps_gtime, &rtklib_eph, r_sat.data(), &clock_bias_s,
|
|
&sat_pos_variance_m2);
|
|
double Az;
|
|
double El;
|
|
double dist_m;
|
|
const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
|
|
const arma::vec dx = r_sat_eb_e - r_eb_e;
|
|
topocent(&Az, &El, &dist_m, r_eb_e, dx);
|
|
// push sat
|
|
if (El > 0)
|
|
{
|
|
std::cout << "Using Galileo Ephemeris: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n';
|
|
available_satellites.emplace_back(floor(El),
|
|
(Gnss_Satellite(std::string("Galileo"), it.second.PRN)));
|
|
visible_gal.push_back(it.second.PRN);
|
|
}
|
|
}
|
|
|
|
const std::map<int, Gps_Almanac> gps_alm_map = pvt_ptr->get_gps_almanac();
|
|
for (const auto &it : gps_alm_map)
|
|
{
|
|
const alm_t rtklib_alm = alm_to_rtklib(it.second);
|
|
std::array<double, 3> r_sat{};
|
|
double clock_bias_s;
|
|
gtime_t aux_gtime;
|
|
aux_gtime.time = fmod(utc2gpst(gps_gtime).time + 345600, 604800);
|
|
aux_gtime.sec = 0.0;
|
|
alm2pos(aux_gtime, &rtklib_alm, r_sat.data(), &clock_bias_s);
|
|
double Az;
|
|
double El;
|
|
double dist_m;
|
|
const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
|
|
const arma::vec dx = r_sat_eb_e - r_eb_e;
|
|
topocent(&Az, &El, &dist_m, r_eb_e, dx);
|
|
// push sat
|
|
std::vector<unsigned int>::iterator it2;
|
|
if (El > 0)
|
|
{
|
|
it2 = std::find(visible_gps.begin(), visible_gps.end(), it.second.PRN);
|
|
if (it2 == visible_gps.end())
|
|
{
|
|
std::cout << "Using GPS Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n';
|
|
available_satellites.emplace_back(floor(El),
|
|
(Gnss_Satellite(std::string("GPS"), it.second.PRN)));
|
|
}
|
|
}
|
|
}
|
|
|
|
const std::map<int, Galileo_Almanac> gal_alm_map = pvt_ptr->get_galileo_almanac();
|
|
for (const auto &it : gal_alm_map)
|
|
{
|
|
const alm_t rtklib_alm = alm_to_rtklib(it.second);
|
|
std::array<double, 3> r_sat{};
|
|
double clock_bias_s;
|
|
gtime_t gal_gtime;
|
|
gal_gtime.time = fmod(utc2gpst(gps_gtime).time + 345600, 604800);
|
|
gal_gtime.sec = 0.0;
|
|
alm2pos(gal_gtime, &rtklib_alm, r_sat.data(), &clock_bias_s);
|
|
double Az;
|
|
double El;
|
|
double dist_m;
|
|
const arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
|
|
const arma::vec dx = r_sat_eb_e - r_eb_e;
|
|
topocent(&Az, &El, &dist_m, r_eb_e, dx);
|
|
// push sat
|
|
std::vector<unsigned int>::iterator it2;
|
|
if (El > 0)
|
|
{
|
|
it2 = std::find(visible_gal.begin(), visible_gal.end(), it.second.PRN);
|
|
if (it2 == visible_gal.end())
|
|
{
|
|
std::cout << "Using Galileo Almanac: Sat " << it.second.PRN << " Az: " << Az << " El: " << El << '\n';
|
|
available_satellites.emplace_back(floor(El),
|
|
(Gnss_Satellite(std::string("Galileo"), it.second.PRN)));
|
|
}
|
|
}
|
|
}
|
|
|
|
// sort the visible satellites in ascending order of elevation
|
|
std::sort(available_satellites.begin(), available_satellites.end(), [](const std::pair<int, Gnss_Satellite> &a, const std::pair<int, Gnss_Satellite> &b) { // use lambda. Cleaner and easier to read
|
|
return a.first < b.first;
|
|
});
|
|
// provide list starting from satellites with higher elevation
|
|
std::reverse(available_satellites.begin(), available_satellites.end());
|
|
return available_satellites;
|
|
}
|
|
|
|
|
|
void ControlThread::gps_acq_assist_data_collector() const
|
|
{
|
|
// ############ 1.bis READ EPHEMERIS/UTC_MODE/IONO QUEUE ####################
|
|
Gps_Acq_Assist gps_acq;
|
|
Gps_Acq_Assist gps_acq_old;
|
|
while (stop_ == false)
|
|
{
|
|
global_gps_acq_assist_queue.wait_and_pop(gps_acq);
|
|
if (gps_acq.PRN == 0)
|
|
{
|
|
break;
|
|
}
|
|
|
|
// DEBUG MESSAGE
|
|
std::cout << "Acquisition assistance record has arrived from SAT ID "
|
|
<< gps_acq.PRN
|
|
<< " with Doppler "
|
|
<< gps_acq.Doppler0
|
|
<< " [Hz]\n";
|
|
// insert new acq record to the global ephemeris map
|
|
if (global_gps_acq_assist_map.read(gps_acq.PRN, gps_acq_old))
|
|
{
|
|
std::cout << "Acquisition assistance record updated\n";
|
|
global_gps_acq_assist_map.write(gps_acq.PRN, gps_acq);
|
|
}
|
|
else
|
|
{
|
|
// insert new acq record
|
|
LOG(INFO) << "New acq assist record inserted";
|
|
global_gps_acq_assist_map.write(gps_acq.PRN, gps_acq);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void ControlThread::sysv_queue_listener()
|
|
{
|
|
typedef struct
|
|
{
|
|
long mtype; // NOLINT(google-runtime-int) required by SysV queue messaging
|
|
double stop_message;
|
|
} stop_msgbuf;
|
|
|
|
bool read_queue = true;
|
|
stop_msgbuf msg;
|
|
double received_message = 0.0;
|
|
const int msgrcv_size = sizeof(msg.stop_message);
|
|
|
|
const key_t key = 1102;
|
|
|
|
if ((msqid_ = msgget(key, 0644 | IPC_CREAT)) == -1)
|
|
{
|
|
std::cerr << "GNSS-SDR cannot create SysV message queues\n";
|
|
read_queue = false;
|
|
}
|
|
|
|
while (read_queue && !stop_)
|
|
{
|
|
if (msgrcv(msqid_, &msg, msgrcv_size, 1, 0) != -1)
|
|
{
|
|
received_message = msg.stop_message;
|
|
if ((std::abs(received_message - (-200.0)) < 10 * std::numeric_limits<double>::epsilon()))
|
|
{
|
|
std::cout << "Quit order received, stopping GNSS-SDR !!\n";
|
|
control_queue_->push(pmt::make_any(command_event_make(200, 0)));
|
|
read_queue = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void ControlThread::keyboard_listener()
|
|
{
|
|
bool read_keys = true;
|
|
char c = '0';
|
|
while (read_keys && !stop_)
|
|
{
|
|
std::cin.get(c);
|
|
if (c == 'q')
|
|
{
|
|
std::cout << "Quit keystroke order received, stopping GNSS-SDR !!\n";
|
|
control_queue_->push(pmt::make_any(command_event_make(200, 0)));
|
|
stop_ = true;
|
|
read_keys = false;
|
|
}
|
|
else
|
|
{
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void ControlThread::print_help_at_exit() const
|
|
{
|
|
std::cerr << "Error: the configuration file is not well formatted\n";
|
|
if (!conf_file_has_section_)
|
|
{
|
|
std::cerr << " * The section label has not been found if the configuration file\n"
|
|
<< " Please add the [GNSS-SDR] label at the top of your configuration file\n"
|
|
<< " A configuration example is available at https://gnss-sdr.org/my-first-fix/\n";
|
|
return;
|
|
}
|
|
if (!conf_file_has_mandatory_globals_)
|
|
{
|
|
std::cerr << " * Have you forgotten to set the mandatory global parameter GNSS-SDR.internal_fs_sps in your conf file?\n"
|
|
<< " Documentation about this parameter at https://gnss-sdr.org/docs/sp-blocks/global-parameters/\n"
|
|
<< " A configuration example is available at https://gnss-sdr.org/my-first-fix/\n";
|
|
}
|
|
if (!conf_has_signal_sources_)
|
|
{
|
|
std::cerr << " * The configuration file must define at least one SignalSource.implementation\n"
|
|
<< " Documentation of SignalSource block implementations at https://gnss-sdr.org/docs/sp-blocks/signal-source/\n";
|
|
}
|
|
if (!conf_has_observables_)
|
|
{
|
|
std::cerr << " * The configuration file must define an Observables.implementation\n"
|
|
<< " Documentation of the Observables block at https://gnss-sdr.org/docs/sp-blocks/observables/\n";
|
|
}
|
|
if (!conf_has_pvt_)
|
|
{
|
|
std::cerr << " * The configuration file must define a PVT.implementation\n"
|
|
<< " Documentation of the PVT block at https://gnss-sdr.org/docs/sp-blocks/pvt/\n";
|
|
}
|
|
}
|