mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-07-05 19:42:56 +00:00
minor code cleaning
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@195 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez
parent
1c0fed749d
commit
72df1f7ced
@ -8,7 +8,7 @@
|
|||||||
*
|
*
|
||||||
* -------------------------------------------------------------------------
|
* -------------------------------------------------------------------------
|
||||||
*
|
*
|
||||||
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
|
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
|
||||||
*
|
*
|
||||||
* GNSS-SDR is a software defined Global Navigation
|
* GNSS-SDR is a software defined Global Navigation
|
||||||
* Satellite Systems receiver
|
* Satellite Systems receiver
|
||||||
@ -47,439 +47,538 @@
|
|||||||
using google::LogMessage;
|
using google::LogMessage;
|
||||||
|
|
||||||
GNSSFlowgraph::GNSSFlowgraph(ConfigurationInterface *configuration,
|
GNSSFlowgraph::GNSSFlowgraph(ConfigurationInterface *configuration,
|
||||||
gr_msg_queue_sptr queue) {
|
gr_msg_queue_sptr queue)
|
||||||
connected_ = false;
|
{
|
||||||
running_ = false;
|
connected_ = false;
|
||||||
configuration_ = configuration;
|
running_ = false;
|
||||||
blocks_ = new std::vector<GNSSBlockInterface*>();
|
configuration_ = configuration;
|
||||||
block_factory_ = new GNSSBlockFactory();
|
blocks_ = new std::vector<GNSSBlockInterface*>();
|
||||||
queue_ = queue;
|
block_factory_ = new GNSSBlockFactory();
|
||||||
//available_GNSS_signals_ = new std::list<Gnss_Satellite>();
|
queue_ = queue;
|
||||||
|
//available_GNSS_signals_ = new std::list<Gnss_Satellite>();
|
||||||
init();
|
init();
|
||||||
}
|
}
|
||||||
|
|
||||||
GNSSFlowgraph::~GNSSFlowgraph() {
|
|
||||||
|
|
||||||
delete block_factory_;
|
GNSSFlowgraph::~GNSSFlowgraph()
|
||||||
|
{
|
||||||
for (unsigned int i = 0; i < blocks_->size(); i++) {
|
delete block_factory_;
|
||||||
delete blocks_->at(i);
|
for (unsigned int i = 0; i < blocks_->size(); i++)
|
||||||
}
|
{
|
||||||
blocks_->clear();
|
delete blocks_->at(i);
|
||||||
|
}
|
||||||
delete blocks_;
|
blocks_->clear();
|
||||||
|
delete blocks_;
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::start() {
|
|
||||||
if (running_) {
|
|
||||||
LOG_AT_LEVEL(WARNING) << "Already running";
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
try {
|
|
||||||
top_block_->start();
|
|
||||||
} catch (std::exception& e) {
|
|
||||||
LOG_AT_LEVEL(ERROR) << "Unable to start flowgraph";
|
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
running_ = true;
|
void GNSSFlowgraph::start()
|
||||||
|
{
|
||||||
|
if (running_)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(WARNING) << "Already running";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
top_block_->start();
|
||||||
|
}
|
||||||
|
catch (std::exception& e)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(ERROR) << "Unable to start flowgraph";
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
running_ = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::stop() {
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
|
||||||
// if(channels_state_[i]==2) channel(i)->
|
|
||||||
channel(i)->stop();
|
|
||||||
}
|
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
|
||||||
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
|
||||||
<< std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
DLOG(INFO) << "Threads finished. Return to main program.";
|
void GNSSFlowgraph::stop()
|
||||||
top_block_->stop();
|
{
|
||||||
running_ = false;
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
// if(channels_state_[i]==2) channel(i)->
|
||||||
|
channel(i)->stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
DLOG(INFO) << "Threads finished. Return to main program.";
|
||||||
|
top_block_->stop();
|
||||||
|
running_ = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::connect() {
|
|
||||||
/* Connects the blocks in the flowgraph
|
|
||||||
*
|
|
||||||
* Signal Source > Signal conditioner >> Channels >> Observables >> PVT > Output filter
|
|
||||||
*/
|
|
||||||
DLOG(INFO) << "Connecting flowgraph";
|
|
||||||
if (connected_) {
|
|
||||||
LOG_AT_LEVEL(WARNING) << "flowgraph already connected";
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
try {
|
|
||||||
signal_source()->connect(top_block_);
|
|
||||||
} catch (std::exception& e) {
|
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect signal source block internally";
|
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
|
||||||
top_block_->disconnect_all();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Signal Source > Signal conditioner >
|
void GNSSFlowgraph::connect()
|
||||||
try {
|
{
|
||||||
signal_conditioner()->connect(top_block_);
|
/* Connects the blocks in the flowgraph
|
||||||
} catch (std::exception& e) {
|
*
|
||||||
LOG_AT_LEVEL(ERROR)
|
* Signal Source > Signal conditioner >> Channels >> Observables >> PVT > Output filter
|
||||||
<< "Can't connect signal conditioner block internally";
|
*/
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
DLOG(INFO) << "Connecting flowgraph";
|
||||||
top_block_->disconnect_all();
|
if (connected_)
|
||||||
return;
|
{
|
||||||
}
|
LOG_AT_LEVEL(WARNING) << "flowgraph already connected";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
try
|
||||||
try {
|
{
|
||||||
channel(i)->connect(top_block_);
|
signal_source()->connect(top_block_);
|
||||||
} catch (std::exception& e) {
|
}
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect channel " << i
|
catch (std::exception& e)
|
||||||
<< " internally";
|
{
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
LOG_AT_LEVEL(ERROR) << "Can't connect signal source block internally";
|
||||||
top_block_->disconnect_all();
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
return;
|
top_block_->disconnect_all();
|
||||||
}
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
try {
|
// Signal Source > Signal conditioner >
|
||||||
observables()->connect(top_block_);
|
try
|
||||||
} catch (std::exception& e) {
|
{
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect observables block internally";
|
signal_conditioner()->connect(top_block_);
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
}
|
||||||
top_block_->disconnect_all();
|
catch (std::exception& e)
|
||||||
return;
|
{
|
||||||
}
|
LOG_AT_LEVEL(ERROR)
|
||||||
|
<< "Can't connect signal conditioner block internally";
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// Signal Source > Signal conditioner >> Channels >> Observables > PVT
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
try {
|
{
|
||||||
pvt()->connect(top_block_);
|
try
|
||||||
} catch (std::exception& e) {
|
{
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect PVT block internally";
|
channel(i)->connect(top_block_);
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
}
|
||||||
top_block_->disconnect_all();
|
catch (std::exception& e)
|
||||||
return;
|
{
|
||||||
}
|
LOG_AT_LEVEL(ERROR) << "Can't connect channel " << i
|
||||||
|
<< " internally";
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// Signal Source > Signal conditioner >> Channels >> Observables > PVT > Output Filter
|
try
|
||||||
try {
|
{
|
||||||
output_filter()->connect(top_block_);
|
observables()->connect(top_block_);
|
||||||
} catch (std::exception& e) {
|
}
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect output filter block internally";
|
catch (std::exception& e)
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
{
|
||||||
top_block_->disconnect_all();
|
LOG_AT_LEVEL(ERROR) << "Can't connect observables block internally";
|
||||||
return;
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
}
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
DLOG(INFO) << "blocks connected internally";
|
// Signal Source > Signal conditioner >> Channels >> Observables > PVT
|
||||||
|
try
|
||||||
|
{
|
||||||
|
pvt()->connect(top_block_);
|
||||||
|
}
|
||||||
|
catch (std::exception& e)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(ERROR) << "Can't connect PVT block internally";
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// Signal Source > Signal conditioner >
|
// Signal Source > Signal conditioner >> Channels >> Observables > PVT > Output Filter
|
||||||
try {
|
try
|
||||||
top_block_->connect(signal_source()->get_right_block(), 0,
|
{
|
||||||
signal_conditioner()->get_left_block(), 0);
|
output_filter()->connect(top_block_);
|
||||||
} catch (std::exception& e) {
|
}
|
||||||
LOG_AT_LEVEL(ERROR)
|
catch (std::exception& e)
|
||||||
<< "Can't connect signal source to signal conditioner";
|
{
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
LOG_AT_LEVEL(ERROR) << "Can't connect output filter block internally";
|
||||||
top_block_->disconnect_all();
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
return;
|
top_block_->disconnect_all();
|
||||||
}
|
return;
|
||||||
DLOG(INFO) << "Signal source connected to signal conditioner";
|
}
|
||||||
|
|
||||||
// Signal Source > Signal conditioner >> channels_count_ number of Channels in parallel
|
DLOG(INFO) << "blocks connected internally";
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
|
||||||
try {
|
|
||||||
top_block_->connect(signal_conditioner()->get_right_block(), 0,
|
|
||||||
channel(i)->get_left_block(), 0);
|
|
||||||
} catch (std::exception& e) {
|
|
||||||
LOG_AT_LEVEL(ERROR)
|
|
||||||
<< "Can't connect signal conditioner to channel " << i;
|
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
|
||||||
top_block_->disconnect_all();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
DLOG(INFO) << "signal conditioner connected to channel " << i;
|
// Signal Source > Signal conditioner >
|
||||||
|
try
|
||||||
|
{
|
||||||
|
top_block_->connect(signal_source()->get_right_block(), 0,
|
||||||
|
signal_conditioner()->get_left_block(), 0);
|
||||||
|
}
|
||||||
|
catch (std::exception& e)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(ERROR)
|
||||||
|
<< "Can't connect signal source to signal conditioner";
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
DLOG(INFO) << "Signal source connected to signal conditioner";
|
||||||
|
|
||||||
// Signal Source > Signal conditioner >> Channels >> Observables
|
// Signal Source > Signal conditioner >> channels_count_ number of Channels in parallel
|
||||||
try {
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
top_block_->connect(channel(i)->get_right_block(), 0,
|
{
|
||||||
observables()->get_left_block(), i);
|
try
|
||||||
} catch (std::exception& e) {
|
{
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect channel " << i
|
top_block_->connect(signal_conditioner()->get_right_block(), 0,
|
||||||
<< " to observables";
|
channel(i)->get_left_block(), 0);
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
}
|
||||||
top_block_->disconnect_all();
|
catch (std::exception& e)
|
||||||
return;
|
{
|
||||||
}
|
LOG_AT_LEVEL(ERROR)
|
||||||
|
<< "Can't connect signal conditioner to channel " << i;
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
channel(i)->set_signal(available_GNSS_signals_.front());
|
DLOG(INFO) << "signal conditioner connected to channel " << i;
|
||||||
std::cout << "Channel " << i << " assigned to "
|
|
||||||
<< available_GNSS_signals_.front() << std::endl;
|
|
||||||
available_GNSS_signals_.pop_front();
|
|
||||||
channel(i)->start();
|
|
||||||
if (channels_state_[i] == 1) {
|
|
||||||
channel(i)->start_acquisition();
|
|
||||||
DLOG(INFO) << "Channel " << i
|
|
||||||
<< " connected to observables and ready for acquisition";
|
|
||||||
} else {
|
|
||||||
DLOG(INFO) << "Channel " << i
|
|
||||||
<< " connected to observables in standby mode";
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
// Signal Source > Signal conditioner >> Channels >> Observables
|
||||||
/*
|
try
|
||||||
* Connect the observables output of each channel to the PVT block
|
{
|
||||||
*/
|
top_block_->connect(channel(i)->get_right_block(), 0,
|
||||||
try {
|
observables()->get_left_block(), i);
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
}
|
||||||
top_block_->connect(observables()->get_right_block(), i,
|
catch (std::exception& e)
|
||||||
pvt()->get_left_block(), i);
|
{
|
||||||
}
|
LOG_AT_LEVEL(ERROR) << "Can't connect channel " << i
|
||||||
} catch (std::exception& e) {
|
<< " to observables";
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect observables to PVT";
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
top_block_->disconnect_all();
|
||||||
top_block_->disconnect_all();
|
return;
|
||||||
return;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
try {
|
channel(i)->set_signal(available_GNSS_signals_.front());
|
||||||
top_block_->connect(pvt()->get_right_block(), 0,
|
std::cout << "Channel " << i << " assigned to "
|
||||||
output_filter()->get_left_block(), 0);
|
<< available_GNSS_signals_.front() << std::endl;
|
||||||
} catch (std::exception& e) {
|
available_GNSS_signals_.pop_front();
|
||||||
LOG_AT_LEVEL(ERROR) << "Can't connect PVT to output filter";
|
channel(i)->start();
|
||||||
LOG_AT_LEVEL(ERROR) << e.what();
|
if (channels_state_[i] == 1)
|
||||||
top_block_->disconnect_all();
|
{
|
||||||
return;
|
channel(i)->start_acquisition();
|
||||||
}
|
DLOG(INFO) << "Channel " << i
|
||||||
|
<< " connected to observables and ready for acquisition";
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
DLOG(INFO) << "Channel " << i
|
||||||
|
<< " connected to observables in standby mode";
|
||||||
|
}
|
||||||
|
|
||||||
DLOG(INFO) << "PVT connected to output filter";
|
}
|
||||||
|
/*
|
||||||
|
* Connect the observables output of each channel to the PVT block
|
||||||
|
*/
|
||||||
|
try
|
||||||
|
{
|
||||||
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
top_block_->connect(observables()->get_right_block(), i,
|
||||||
|
pvt()->get_left_block(), i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (std::exception& e)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(ERROR) << "Can't connect observables to PVT";
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
connected_ = true;
|
try
|
||||||
DLOG(INFO) << "Flowgraph connected";
|
{
|
||||||
top_block_->dump();
|
top_block_->connect(pvt()->get_right_block(), 0,
|
||||||
|
output_filter()->get_left_block(), 0);
|
||||||
|
}
|
||||||
|
catch (std::exception& e)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(ERROR) << "Can't connect PVT to output filter";
|
||||||
|
LOG_AT_LEVEL(ERROR) << e.what();
|
||||||
|
top_block_->disconnect_all();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
DLOG(INFO) << "PVT connected to output filter";
|
||||||
|
connected_ = true;
|
||||||
|
DLOG(INFO) << "Flowgraph connected";
|
||||||
|
top_block_->dump();
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::wait() {
|
|
||||||
if (!running_) {
|
|
||||||
LOG_AT_LEVEL(WARNING) << "Can't apply wait. Flowgraph is not running";
|
|
||||||
return;
|
void GNSSFlowgraph::wait()
|
||||||
}
|
{
|
||||||
top_block_->wait();
|
if (!running_)
|
||||||
DLOG(INFO) << "Flowgraph finished calculations";
|
{
|
||||||
running_ = false;
|
LOG_AT_LEVEL(WARNING) << "Can't apply wait. Flowgraph is not running";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
top_block_->wait();
|
||||||
|
DLOG(INFO) << "Flowgraph finished calculations";
|
||||||
|
running_ = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Applies an action to the flowgraph
|
* Applies an action to the flowgraph
|
||||||
*
|
*
|
||||||
* \param[in] who Who generated the action
|
* \param[in] who Who generated the action
|
||||||
* \param[in] what What is the action 0: acquisition failed
|
* \param[in] what What is the action 0: acquisition failed
|
||||||
*/
|
*/
|
||||||
void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) {
|
void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||||
DLOG(INFO) << "received " << what << " from " << who;
|
{
|
||||||
|
DLOG(INFO) << "received " << what << " from " << who;
|
||||||
|
|
||||||
switch (what) {
|
switch (what)
|
||||||
case 0:
|
{
|
||||||
|
case 0:
|
||||||
|
|
||||||
DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite "
|
DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite "
|
||||||
<< channel(who)->get_signal().get_satellite();
|
<< channel(who)->get_signal().get_satellite();
|
||||||
available_GNSS_signals_.push_back(channel(who)->get_signal());
|
available_GNSS_signals_.push_back(channel(who)->get_signal());
|
||||||
channel(who)->set_signal(available_GNSS_signals_.front());
|
channel(who)->set_signal(available_GNSS_signals_.front());
|
||||||
available_GNSS_signals_.pop_front();
|
available_GNSS_signals_.pop_front();
|
||||||
channel(who)->start_acquisition();
|
channel(who)->start_acquisition();
|
||||||
break;
|
break;
|
||||||
// TODO: Tracking messages
|
// TODO: Tracking messages
|
||||||
|
|
||||||
case 1:
|
case 1:
|
||||||
|
|
||||||
DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite "
|
DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite "
|
||||||
<< channel(who)->get_signal().get_satellite();
|
<< channel(who)->get_signal().get_satellite();
|
||||||
|
channels_state_[who] = 2;
|
||||||
|
acq_channels_count_--;
|
||||||
|
if (acq_channels_count_ < max_acq_channels_)
|
||||||
|
{
|
||||||
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
if (channels_state_[i] == 0)
|
||||||
|
{
|
||||||
|
channels_state_[i] = 1;
|
||||||
|
acq_channels_count_++;
|
||||||
|
channel(i)->start_acquisition();
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
channels_state_[who] = 2;
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
acq_channels_count_--;
|
{
|
||||||
if (acq_channels_count_ < max_acq_channels_) {
|
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
<< std::endl;
|
||||||
if (channels_state_[i] == 0) {
|
}
|
||||||
channels_state_[i] = 1;
|
break;
|
||||||
acq_channels_count_++;
|
|
||||||
channel(i)->start_acquisition();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
case 2:
|
||||||
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
|
||||||
<< std::endl;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 2:
|
DLOG(INFO) << "Channel " << who << " TRK FAILED satellite "
|
||||||
|
<< channel(who)->get_signal().get_satellite();
|
||||||
|
|
||||||
DLOG(INFO) << "Channel " << who << " TRK FAILED satellite "
|
if (acq_channels_count_ < max_acq_channels_)
|
||||||
<< channel(who)->get_signal().get_satellite();
|
{
|
||||||
|
channels_state_[who] = 1;
|
||||||
|
acq_channels_count_++;
|
||||||
|
channel(who)->start_acquisition();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
channels_state_[who] = 0;
|
||||||
|
channel(who)->standby();
|
||||||
|
}
|
||||||
|
|
||||||
if (acq_channels_count_ < max_acq_channels_) {
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
channels_state_[who] = 1;
|
{
|
||||||
acq_channels_count_++;
|
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
||||||
channel(who)->start_acquisition();
|
<< std::endl;
|
||||||
}else {
|
}
|
||||||
channels_state_[who] = 0;
|
break;
|
||||||
channel(who)->standby();
|
|
||||||
}
|
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
default:
|
||||||
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
break;
|
||||||
<< std::endl;
|
}
|
||||||
}
|
DLOG(INFO) << "Number of available satellites: "
|
||||||
break;
|
<< available_GNSS_signals_.size();
|
||||||
|
|
||||||
default:
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
DLOG(INFO) << "Number of available satellites: "
|
|
||||||
<< available_GNSS_signals_.size();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::set_configuration(ConfigurationInterface* configuration) {
|
|
||||||
if (running_) {
|
|
||||||
LOG_AT_LEVEL(WARNING)
|
|
||||||
<< "Unable to update configuration while flowgraph running";
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (connected_) {
|
|
||||||
LOG_AT_LEVEL(WARNING)
|
|
||||||
<< "Unable to update configuration while flowgraph connected";
|
|
||||||
}
|
|
||||||
|
|
||||||
configuration_ = configuration;
|
void GNSSFlowgraph::set_configuration(ConfigurationInterface* configuration)
|
||||||
|
{
|
||||||
|
if (running_)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(WARNING)
|
||||||
|
<< "Unable to update configuration while flowgraph running";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (connected_)
|
||||||
|
{
|
||||||
|
LOG_AT_LEVEL(WARNING)
|
||||||
|
<< "Unable to update configuration while flowgraph connected";
|
||||||
|
}
|
||||||
|
configuration_ = configuration;
|
||||||
}
|
}
|
||||||
|
|
||||||
GNSSBlockInterface* GNSSFlowgraph::signal_source() {
|
|
||||||
return blocks_->at(0);
|
|
||||||
|
GNSSBlockInterface* GNSSFlowgraph::signal_source()
|
||||||
|
{
|
||||||
|
return blocks_->at(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
GNSSBlockInterface* GNSSFlowgraph::signal_conditioner() {
|
|
||||||
return blocks_->at(1);
|
|
||||||
|
GNSSBlockInterface* GNSSFlowgraph::signal_conditioner()
|
||||||
|
{
|
||||||
|
return blocks_->at(1);
|
||||||
}
|
}
|
||||||
|
|
||||||
ChannelInterface* GNSSFlowgraph::channel(unsigned int index) {
|
|
||||||
return (ChannelInterface*) blocks_->at(index + 5);
|
|
||||||
|
ChannelInterface* GNSSFlowgraph::channel(unsigned int index)
|
||||||
|
{
|
||||||
|
return (ChannelInterface*) blocks_->at(index + 5);
|
||||||
}
|
}
|
||||||
|
|
||||||
GNSSBlockInterface* GNSSFlowgraph::observables() {
|
|
||||||
return blocks_->at(2);
|
|
||||||
|
GNSSBlockInterface* GNSSFlowgraph::observables()
|
||||||
|
{
|
||||||
|
return blocks_->at(2);
|
||||||
}
|
}
|
||||||
|
|
||||||
GNSSBlockInterface* GNSSFlowgraph::pvt() {
|
|
||||||
return blocks_->at(3);
|
|
||||||
|
GNSSBlockInterface* GNSSFlowgraph::pvt()
|
||||||
|
{
|
||||||
|
return blocks_->at(3);
|
||||||
}
|
}
|
||||||
|
|
||||||
GNSSBlockInterface* GNSSFlowgraph::output_filter() {
|
|
||||||
return blocks_->at(4);
|
|
||||||
|
GNSSBlockInterface* GNSSFlowgraph::output_filter()
|
||||||
|
{
|
||||||
|
return blocks_->at(4);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::init() {
|
|
||||||
/*
|
|
||||||
* Instantiates the receiver blocks
|
|
||||||
*/
|
|
||||||
blocks_->push_back(block_factory_->GetSignalSource(configuration_, queue_));
|
|
||||||
blocks_->push_back(block_factory_->GetSignalConditioner(configuration_,
|
|
||||||
queue_));
|
|
||||||
blocks_->push_back(block_factory_->GetObservables(configuration_, queue_));
|
|
||||||
blocks_->push_back(block_factory_->GetPVT(configuration_, queue_));
|
|
||||||
blocks_->push_back(block_factory_->GetOutputFilter(configuration_, queue_));
|
|
||||||
|
|
||||||
std::vector<GNSSBlockInterface*>* channels = block_factory_->GetChannels(
|
|
||||||
configuration_, queue_);
|
|
||||||
|
|
||||||
channels_count_ = channels->size();
|
void GNSSFlowgraph::init()
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
* Instantiates the receiver blocks
|
||||||
|
*/
|
||||||
|
blocks_->push_back(block_factory_->GetSignalSource(configuration_, queue_));
|
||||||
|
blocks_->push_back(block_factory_->GetSignalConditioner(configuration_,
|
||||||
|
queue_));
|
||||||
|
blocks_->push_back(block_factory_->GetObservables(configuration_, queue_));
|
||||||
|
blocks_->push_back(block_factory_->GetPVT(configuration_, queue_));
|
||||||
|
blocks_->push_back(block_factory_->GetOutputFilter(configuration_, queue_));
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
std::vector<GNSSBlockInterface*>* channels = block_factory_->GetChannels(
|
||||||
blocks_->push_back(channels->at(i));
|
configuration_, queue_);
|
||||||
}
|
|
||||||
|
|
||||||
top_block_ = gr_make_top_block("GNSSFlowgraph");
|
channels_count_ = channels->size();
|
||||||
|
|
||||||
delete channels;
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
blocks_->push_back(channels->at(i));
|
||||||
|
}
|
||||||
|
|
||||||
// fill the available_GNSS_signals_ queue with the satellites ID's to be searched by the acquisition
|
top_block_ = gr_make_top_block("GNSSFlowgraph");
|
||||||
|
|
||||||
set_signals_list();
|
delete channels;
|
||||||
|
|
||||||
set_channels_state();
|
// fill the available_GNSS_signals_ queue with the satellites ID's to be searched by the acquisition
|
||||||
|
|
||||||
applied_actions_ = 0;
|
set_signals_list();
|
||||||
|
set_channels_state();
|
||||||
|
|
||||||
DLOG(INFO) << "Blocks instantiated. " << channels_count_ << " channels.";
|
applied_actions_ = 0;
|
||||||
|
|
||||||
|
DLOG(INFO) << "Blocks instantiated. " << channels_count_ << " channels.";
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::set_signals_list() {
|
void GNSSFlowgraph::set_signals_list()
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
* Sets a sequential list of satellites (1...32)
|
||||||
|
*/
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Sets a sequential list of satellites (1...32)
|
* \TODO Describe GNSS satellites more nicely, with RINEX notation
|
||||||
*/
|
* See http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf (page 5)
|
||||||
|
*/
|
||||||
|
std::set<unsigned int> available_gps_prn = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
|
||||||
|
11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 25, 26, 27, 28,
|
||||||
|
29, 30, 31, 32 };
|
||||||
|
|
||||||
/*
|
std::set<unsigned int>::iterator available_gps_prn_iter;
|
||||||
* \TODO Describe GNSS satellites more nicely, with RINEX notation
|
|
||||||
* See http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf (page 5)
|
|
||||||
*/
|
|
||||||
std::set<unsigned int> available_gps_prn = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
|
|
||||||
11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 25, 26, 27, 28,
|
|
||||||
29, 30, 31, 32 };
|
|
||||||
|
|
||||||
std::set<unsigned int>::iterator available_gps_prn_iter;
|
Gnss_Signal signal_value;
|
||||||
|
|
||||||
Gnss_Signal signal_value;
|
/*
|
||||||
|
* Loop to create the list of GNSS Signals
|
||||||
|
* To add signals from other systems, add another loop 'for'
|
||||||
|
*/
|
||||||
|
|
||||||
/*
|
for (available_gps_prn_iter = available_gps_prn.begin(); available_gps_prn_iter
|
||||||
* Loop to create the list of GNSS Signals
|
!= available_gps_prn.end(); available_gps_prn_iter++)
|
||||||
* To add signals from other systems, add another loop 'for'
|
{
|
||||||
*/
|
signal_value = Gnss_Signal(Gnss_Satellite(std::string("GPS"),
|
||||||
|
*available_gps_prn_iter), std::string("1C"));
|
||||||
|
available_GNSS_signals_.push_back(signal_value);
|
||||||
|
}
|
||||||
|
|
||||||
for (available_gps_prn_iter = available_gps_prn.begin(); available_gps_prn_iter
|
std::list<Gnss_Signal>::iterator gnss_it = available_GNSS_signals_.begin();
|
||||||
!= available_gps_prn.end(); available_gps_prn_iter++) {
|
|
||||||
signal_value = Gnss_Signal(Gnss_Satellite(std::string("GPS"),
|
|
||||||
*available_gps_prn_iter), std::string("1C"));
|
|
||||||
available_GNSS_signals_.push_back(signal_value);
|
|
||||||
}
|
|
||||||
|
|
||||||
std::list<Gnss_Signal>::iterator gnss_it = available_GNSS_signals_.begin();
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
std::string default_system = "GPS";
|
||||||
|
std::string default_signal = "1C";
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
std::string gnss_system = (configuration_->property("Channel"
|
||||||
std::string default_system = "GPS";
|
+ boost::lexical_cast<std::string>(i) + ".system",
|
||||||
std::string default_signal = "1C";
|
default_system));
|
||||||
|
std::string gnss_signal = (configuration_->property("Channel"
|
||||||
|
+ boost::lexical_cast<std::string>(i) + ".signal",
|
||||||
|
default_signal));
|
||||||
|
unsigned int sat = configuration_->property("Channel"
|
||||||
|
+ boost::lexical_cast<std::string>(i) + ".satellite", 0);
|
||||||
|
|
||||||
std::string gnss_system = (configuration_->property("Channel"
|
if ((sat == 0) || (sat == gnss_it->get_satellite().get_PRN())) // 0 = not PRN in configuration file
|
||||||
+ boost::lexical_cast<std::string>(i) + ".system",
|
{
|
||||||
default_system));
|
gnss_it++;
|
||||||
std::string gnss_signal = (configuration_->property("Channel"
|
}
|
||||||
+ boost::lexical_cast<std::string>(i) + ".signal",
|
else
|
||||||
default_signal));
|
{
|
||||||
unsigned int sat = configuration_->property("Channel"
|
signal_value = Gnss_Signal(Gnss_Satellite(gnss_system, sat),
|
||||||
+ boost::lexical_cast<std::string>(i) + ".satellite", 0);
|
gnss_signal);
|
||||||
|
available_GNSS_signals_.remove(signal_value);
|
||||||
|
available_GNSS_signals_.insert(gnss_it, signal_value);
|
||||||
|
}
|
||||||
|
|
||||||
if ((sat == 0) || (sat == gnss_it->get_satellite().get_PRN())) // 0 = not PRN in configuration file
|
}
|
||||||
{
|
/* std::cout << "Signal queue: " << std::endl;
|
||||||
gnss_it++;
|
|
||||||
} else {
|
|
||||||
signal_value = Gnss_Signal(Gnss_Satellite(gnss_system, sat),
|
|
||||||
gnss_signal);
|
|
||||||
available_GNSS_signals_.remove(signal_value);
|
|
||||||
available_GNSS_signals_.insert(gnss_it, signal_value);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
/* std::cout << "Signal queue: " << std::endl;
|
|
||||||
for (std::list<Gnss_Signal>::iterator it =
|
for (std::list<Gnss_Signal>::iterator it =
|
||||||
available_GNSS_signals_.begin(); it
|
available_GNSS_signals_.begin(); it
|
||||||
!= available_GNSS_signals_.end(); it++) {
|
!= available_GNSS_signals_.end(); it++) {
|
||||||
@ -488,38 +587,47 @@ void GNSSFlowgraph::set_signals_list() {
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::set_channels_state() {
|
|
||||||
|
|
||||||
max_acq_channels_ = (configuration_->property("Channels.in_acquisition",
|
|
||||||
channels_count_));
|
|
||||||
|
|
||||||
if (max_acq_channels_ > channels_count_) {
|
void GNSSFlowgraph::set_channels_state()
|
||||||
max_acq_channels_ = channels_count_;
|
{
|
||||||
std::cout
|
max_acq_channels_ = (configuration_->property("Channels.in_acquisition",
|
||||||
<< "Channels_in_acquisition is bigger than number of channels. Variable acq_channels_count_ is set to "
|
channels_count_));
|
||||||
<< channels_count_ << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
channels_state_.reserve(channels_count_);
|
if (max_acq_channels_ > channels_count_)
|
||||||
|
{
|
||||||
|
max_acq_channels_ = channels_count_;
|
||||||
|
std::cout
|
||||||
|
<< "Channels_in_acquisition is bigger than number of channels. Variable acq_channels_count_ is set to "
|
||||||
|
<< channels_count_ << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
channels_state_.reserve(channels_count_);
|
||||||
if (i < max_acq_channels_) {
|
|
||||||
channels_state_.push_back(1);
|
|
||||||
} else
|
|
||||||
channels_state_.push_back(0);
|
|
||||||
}
|
|
||||||
acq_channels_count_ = max_acq_channels_;
|
|
||||||
|
|
||||||
DLOG(INFO) << acq_channels_count_ << " channels in acquisition state";
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
|
{
|
||||||
|
if (i < max_acq_channels_)
|
||||||
|
{
|
||||||
|
channels_state_.push_back(1);
|
||||||
|
} else
|
||||||
|
channels_state_.push_back(0);
|
||||||
|
}
|
||||||
|
acq_channels_count_ = max_acq_channels_;
|
||||||
|
|
||||||
for (unsigned int i = 0; i < channels_count_; i++) {
|
DLOG(INFO) << acq_channels_count_ << " channels in acquisition state";
|
||||||
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
|
||||||
<< std::endl;
|
for (unsigned int i = 0; i < channels_count_; i++)
|
||||||
}
|
{
|
||||||
|
std::cout << "Channel " << i << " in state " << channels_state_[i]
|
||||||
|
<< std::endl;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void GNSSFlowgraph::apply_action(unsigned int what) {
|
|
||||||
DLOG(INFO) << "Applied action " << what << " to flowgraph";
|
|
||||||
applied_actions_++;
|
void GNSSFlowgraph::apply_action(unsigned int what)
|
||||||
|
{
|
||||||
|
DLOG(INFO) << "Applied action " << what << " to flowgraph";
|
||||||
|
applied_actions_++;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user