mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 04:30:33 +00:00
Code Issues Releases Wiki Activity

Merge pull request #107 from antonioramosdet/new_fsm

Browse Source 
Merging @antonioramosdet  pull request #107
This commit is contained in:
Javier Arribas 2018-01-10 16:39:01 +01:00 committed by GitHub
commit 8b93cd2ebd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
22 changed files with 211 additions and 277 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -349,7 +349,7 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data);
if(result == 0)
{
LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
this->set_time_offset_s(0.0); //reset rx time estimation
this->set_num_valid_observations(0);
}

1
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h
View File

@ -58,6 +58,7 @@
#include <gnuradio/gr_complex.h>
#include <gnuradio/fft/fft.h>
#include "gnss_synchro.h"
#include <glog/logging.h>
class pcps_acquisition_cc;

30
src/algorithms/channel/adapters/channel.cc
View File

@ -40,7 +40,7 @@ using google::LogMessage;
Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
std::shared_ptr<GNSSBlockInterface> pass_through, std::shared_ptr<AcquisitionInterface> acq,
std::shared_ptr<TrackingInterface> trk, std::shared_ptr<TelemetryDecoderInterface> nav,
std::string role, std::string implementation, boost::shared_ptr<gr::msg_queue> queue)
std::string role, std::string implementation, gr::msg_queue::sptr queue)
{
pass_through_ = pass_through;
acq_ = acq;
@ -50,6 +50,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
implementation_ = implementation;
channel_ = channel;
queue_ = queue;
channel_fsm_ = std::make_shared<ChannelFsm>();
flag_enable_fpga = configuration->property("Channel.enable_FPGA", false);
acq_->set_channel(channel_);
@ -89,24 +90,21 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
repeat_ = configuration->property("Acquisition_" + implementation_ + boost::lexical_cast<std::string>(channel_) + ".repeat_satellite", false);
DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_;
channel_fsm_.set_acquisition(acq_);
channel_fsm_.set_tracking(trk_);
channel_fsm_.set_channel(channel_);
channel_fsm_.set_queue(queue_);
channel_fsm_->set_acquisition(acq_);
channel_fsm_->set_tracking(trk_);
channel_fsm_->set_channel(channel_);
channel_fsm_->set_queue(queue_);
connected_ = false;
gnss_signal_ = Gnss_Signal(implementation_);
channel_msg_rx = channel_msg_receiver_make_cc(&channel_fsm_, repeat_);
channel_msg_rx = channel_msg_receiver_make_cc(channel_fsm_, repeat_);
}
// Destructor
Channel::~Channel()
{
channel_fsm_.terminate();
}
Channel::~Channel(){}
void Channel::connect(gr::top_block_sptr top_block)
@ -139,7 +137,6 @@ void Channel::connect(gr::top_block_sptr top_block)
top_block->msg_connect(nav_->get_left_block(), pmt::mp("preamble_timestamp_s"), trk_->get_right_block(), pmt::mp("preamble_timestamp_s"));
DLOG(INFO) << "MSG FEEDBACK CHANNEL telemetry_decoder -> tracking";
//std::cout<<"has port: "<<trk_->get_right_block()->has_msg_port(pmt::mp("events"))<<std::endl;
top_block->msg_connect(acq_->get_right_block(), pmt::mp("events"), channel_msg_rx, pmt::mp("events"));
top_block->msg_connect(trk_->get_right_block(), pmt::mp("events"), channel_msg_rx, pmt::mp("events"));
@ -187,6 +184,7 @@ gr::basic_block_sptr Channel::get_right_block()
void Channel::set_signal(const Gnss_Signal& gnss_signal)
{
std::lock_guard<std::mutex> lk(mx);
gnss_signal_ = gnss_signal;
std::string str_aux = gnss_signal_.get_signal_str();
const char * str = str_aux.c_str(); // get a C style null terminated string
@ -201,6 +199,14 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal)
void Channel::start_acquisition()
{
channel_fsm_.Event_start_acquisition();
std::lock_guard<std::mutex> lk(mx);
bool result = false;
result = channel_fsm_->Event_start_acquisition();
if(!result)
{
LOG(WARNING) << "Invalid channel event";
return;
}
DLOG(INFO) << "Channel start_acquisition()";
}

10
src/algorithms/channel/adapters/channel.h
View File

@ -37,6 +37,7 @@
#include <memory>
#include <string>
#include <mutex>
#include <gnuradio/msg_queue.h>
#include <gnuradio/block.h>
#include "channel_interface.h"
@ -63,8 +64,7 @@ public:
Channel(ConfigurationInterface *configuration, unsigned int channel,
std::shared_ptr<GNSSBlockInterface> pass_through, std::shared_ptr<AcquisitionInterface> acq,
std::shared_ptr<TrackingInterface> trk, std::shared_ptr<TelemetryDecoderInterface> nav,
std::string role, std::string implementation,
boost::shared_ptr<gr::msg_queue> queue);
std::string role, std::string implementation, gr::msg_queue::sptr queue);
//! Virtual destructor
virtual ~Channel();
@ -79,6 +79,7 @@ public:
inline std::string implementation() override { return implementation_; }
inline size_t item_size() override { return 0; }
inline Gnss_Signal get_signal() const override { return gnss_signal_; }
void start_acquisition() override; //!< Start the State Machine
@ -104,8 +105,9 @@ private:
Gnss_Signal gnss_signal_;
bool connected_;
bool repeat_;
ChannelFsm channel_fsm_;
boost::shared_ptr<gr::msg_queue> queue_;
std::shared_ptr<ChannelFsm> channel_fsm_;
gr::msg_queue::sptr queue_;
std::mutex mx;
};
#endif /*GNSS_SDR_CHANNEL_H_*/

207
src/algorithms/channel/libs/channel_fsm.cc
View File

@ -1,11 +1,12 @@
/*!
* \file channel_fsm.cc
* \brief Implementation of a State Machine for channel using boost::statechart
* \author Luis Esteve, 2011. luis(at)epsilon-formacion.com
* \brief Implementation of a State Machine for channel
* \authors Antonio Ramos, 2017. antonio.ramos(at)cttc.es
* Luis Esteve, 2011. luis(at)epsilon-formacion.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
@ -29,109 +30,16 @@
*/
#include "channel_fsm.h"
#include <memory>
#include <boost/statechart/simple_state.hpp>
#include <boost/statechart/state.hpp>
#include <boost/statechart/transition.hpp>
#include <boost/statechart/custom_reaction.hpp>
#include <boost/mpl/list.hpp>
#include <glog/logging.h>
#include "control_message_factory.h"
struct Ev_channel_start_acquisition: sc::event<Ev_channel_start_acquisition>
{};
struct Ev_channel_valid_acquisition: sc::event<Ev_channel_valid_acquisition>
{};
struct Ev_channel_failed_acquisition_repeat: sc::event<Ev_channel_failed_acquisition_repeat>
{};
struct Ev_channel_failed_acquisition_no_repeat: sc::event<Ev_channel_failed_acquisition_no_repeat>
{};
struct Ev_channel_failed_tracking_standby: sc::event<Ev_channel_failed_tracking_standby>
{};
//struct Ev_channel_failed_tracking_reacq: sc::event<Ev_channel_failed_tracking_reacq>
//{};
struct channel_idle_fsm_S0: public sc::state<channel_idle_fsm_S0, ChannelFsm>
{
public:
// sc::transition(event, next state)
typedef sc::transition<Ev_channel_start_acquisition, channel_acquiring_fsm_S1> reactions;
channel_idle_fsm_S0(my_context ctx) : my_base(ctx)
{
//std::cout << "Enter Channel_Idle_S0 " << std::endl;
}
};
struct channel_acquiring_fsm_S1: public sc::state<channel_acquiring_fsm_S1, ChannelFsm>
{
public:
typedef mpl::list<sc::transition<Ev_channel_failed_acquisition_no_repeat, channel_waiting_fsm_S3>,
sc::transition<Ev_channel_failed_acquisition_repeat, channel_acquiring_fsm_S1>,
sc::transition<Ev_channel_valid_acquisition, channel_tracking_fsm_S2> > reactions;
channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx)
{
//std::cout << "Enter Channel_Acq_S1 " << std::endl;
context<ChannelFsm> ().start_acquisition();
}
~channel_acquiring_fsm_S1()
{
//std::cout << "Exit Channel_Acq_S1 " << std::endl;
}
};
struct channel_tracking_fsm_S2: public sc::state<channel_tracking_fsm_S2, ChannelFsm>
{
public:
typedef mpl::list<sc::transition<Ev_channel_failed_tracking_standby, channel_idle_fsm_S0>,
sc::transition<Ev_channel_start_acquisition, channel_acquiring_fsm_S1>> reactions;
channel_tracking_fsm_S2(my_context ctx) : my_base(ctx)
{
//std::cout << "Enter Channel_tracking_S2 " << std::endl;
context<ChannelFsm> ().start_tracking();
}
~channel_tracking_fsm_S2()
{
//std::cout << "Exit Channel_tracking_S2 " << std::endl;
context<ChannelFsm> ().notify_stop_tracking();
}
};
struct channel_waiting_fsm_S3: public sc::state<channel_waiting_fsm_S3, ChannelFsm>
{
public:
typedef sc::transition<Ev_channel_start_acquisition,
channel_acquiring_fsm_S1> reactions;
channel_waiting_fsm_S3(my_context ctx) :
my_base(ctx)
{
//std::cout << "Enter Channel_waiting_S3 " << std::endl;
context<ChannelFsm> ().request_satellite();
}
// ~channel_waiting_fsm_S3(){}
};
ChannelFsm::ChannelFsm()
{
acq_ = nullptr;
trk_ = nullptr;
channel_ = 0;
initiate(); //start the FSM
d_state = 0;
}
@ -141,62 +49,115 @@ ChannelFsm::ChannelFsm(std::shared_ptr<AcquisitionInterface> acquisition) :
{
trk_ = nullptr;
channel_ = 0;
initiate(); //start the FSM
d_state = 0;
}
void ChannelFsm::Event_start_acquisition()
bool ChannelFsm::Event_start_acquisition()
{
this->process_event(Ev_channel_start_acquisition());
//std::cout<<"Ev_channel_start_acquisition launched"<<std::endl;
}
void ChannelFsm::Event_valid_acquisition()
std::lock_guard<std::mutex> lk(mx);
if((d_state == 1) || (d_state == 2))
{
this->process_event(Ev_channel_valid_acquisition());
return false;
}
void ChannelFsm::Event_failed_acquisition_repeat()
else
{
this->process_event(Ev_channel_failed_acquisition_repeat());
d_state = 1;
start_acquisition();
DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition";
return true;
}
}
void ChannelFsm::Event_failed_acquisition_no_repeat()
bool ChannelFsm::Event_valid_acquisition()
{
this->process_event(Ev_channel_failed_acquisition_no_repeat());
}
// Something is wrong here, we are using a memory after it ts freed
void ChannelFsm::Event_failed_tracking_standby()
std::lock_guard<std::mutex> lk(mx);
if(d_state != 1)
{
this->process_event(Ev_channel_failed_tracking_standby());
return false;
}
else
{
d_state = 2;
start_tracking();
DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition";
return true;
}
}
//void ChannelFsm::Event_failed_tracking_reacq() {
// this->process_event(Ev_channel_failed_tracking_reacq());
//}
bool ChannelFsm::Event_failed_acquisition_repeat()
{
std::lock_guard<std::mutex> lk(mx);
if(d_state != 1)
{
return false;
}
else
{
d_state = 1;
start_acquisition();
DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat";
return true;
}
}
bool ChannelFsm::Event_failed_acquisition_no_repeat()
{
std::lock_guard<std::mutex> lk(mx);
if(d_state != 1)
{
return false;
}
else
{
d_state = 3;
request_satellite();
DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat";
return true;
}
}
bool ChannelFsm::Event_failed_tracking_standby()
{
std::lock_guard<std::mutex> lk(mx);
if(d_state != 2)
{
return false;
}
else
{
d_state = 0;
notify_stop_tracking();
DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby";
return true;
}
}
void ChannelFsm::set_acquisition(std::shared_ptr<AcquisitionInterface> acquisition)
{
std::lock_guard<std::mutex> lk(mx);
acq_ = acquisition;
}
void ChannelFsm::set_tracking(std::shared_ptr<TrackingInterface> tracking)
{
std::lock_guard<std::mutex> lk(mx);
trk_ = tracking;
}
void ChannelFsm::set_queue(boost::shared_ptr<gr::msg_queue> queue)
void ChannelFsm::set_queue(gr::msg_queue::sptr queue)
{
std::lock_guard<std::mutex> lk(mx);
queue_ = queue;
}
void ChannelFsm::set_channel(unsigned int channel)
{
std::lock_guard<std::mutex> lk(mx);
channel_ = channel;
}

50
src/algorithms/channel/libs/channel_fsm.h
View File

@ -1,12 +1,12 @@
/*!
* \file channel_fsm.h
* \brief Interface of the State Machine for channel using boost::statechart
* \author Luis Esteve, 2011. luis(at)epsilon-formacion.com
*
* \brief Interface of the State Machine for channel
* \authors Antonio Ramos, 2017. antonio.ramos(at)cttc.es
* Luis Esteve, 2011. luis(at)epsilon-formacion.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
@ -32,26 +32,17 @@
#ifndef GNSS_SDR_CHANNEL_FSM_H
#define GNSS_SDR_CHANNEL_FSM_H
#include <boost/statechart/state_machine.hpp>
#include <mutex>
#include <memory>
#include <gnuradio/msg_queue.h>
#include "acquisition_interface.h"
#include "tracking_interface.h"
#include "telemetry_decoder_interface.h"
namespace sc = boost::statechart;
namespace mpl = boost::mpl;
struct channel_idle_fsm_S0;
struct channel_acquiring_fsm_S1;
struct channel_tracking_fsm_S2;
struct channel_waiting_fsm_S3;
/*!
* \brief This class implements a State Machine for channel using boost::statechart
* \brief This class implements a State Machine for channel
*/
class ChannelFsm: public sc::state_machine<ChannelFsm, channel_idle_fsm_S0>
class ChannelFsm
{
public:
ChannelFsm();
@ -59,26 +50,29 @@ public:
void set_acquisition(std::shared_ptr<AcquisitionInterface> acquisition);
void set_tracking(std::shared_ptr<TrackingInterface> tracking);
void set_queue(boost::shared_ptr<gr::msg_queue> queue);
void set_queue(gr::msg_queue::sptr queue);
void set_channel(unsigned int channel);
//FSM EVENTS
bool Event_start_acquisition();
bool Event_valid_acquisition();
bool Event_failed_acquisition_repeat();
bool Event_failed_acquisition_no_repeat();
bool Event_failed_tracking_standby();
private:
void start_acquisition();
void start_tracking();
void request_satellite();
void notify_stop_tracking();
//FSM EVENTS
void Event_start_acquisition();
void Event_valid_acquisition();
void Event_failed_acquisition_repeat();
void Event_failed_acquisition_no_repeat();
//void Event_gps_failed_tracking_reacq();
void Event_failed_tracking_standby();
private:
std::shared_ptr<AcquisitionInterface> acq_;
std::shared_ptr<TrackingInterface> trk_;
boost::shared_ptr<gr::msg_queue> queue_;
gr::msg_queue::sptr queue_;
unsigned int channel_;
unsigned int d_state;
std::mutex mx;
};
#endif /*GNSS_SDR_CHANNEL_FSM_H*/

21
src/algorithms/channel/libs/channel_msg_receiver_cc.cc
View File

@ -37,37 +37,34 @@
using google::LogMessage;
channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat)
channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat)
{
return channel_msg_receiver_cc_sptr(new channel_msg_receiver_cc(channel_fsm, repeat));
}
void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg)
{
bool result = false;
try
{
long int message = pmt::to_long(msg);
switch (message)
{
case 1: //positive acquisition
//DLOG(INFO) << "Channel " << channel_ << " ACQ SUCCESS satellite " <<
// gnss_synchro_.System << " " << gnss_synchro_.PRN;
d_channel_fsm->Event_valid_acquisition();
result = d_channel_fsm->Event_valid_acquisition();
break;
case 2: //negative acquisition
//DLOG(INFO) << "Channel " << channel_
// << " ACQ FAILED satellite " << gnss_synchro_.System << " " << gnss_synchro_.PRN;
if (d_repeat == true)
{
d_channel_fsm->Event_failed_acquisition_repeat();
result = d_channel_fsm->Event_failed_acquisition_repeat();
}
else
{
d_channel_fsm->Event_failed_acquisition_no_repeat();
result = d_channel_fsm->Event_failed_acquisition_no_repeat();
}
break;
case 3: // tracking loss of lock event
d_channel_fsm->Event_failed_tracking_standby();
result = d_channel_fsm->Event_failed_tracking_standby();
break;
default:
LOG(WARNING) << "Default case, invalid message.";
@ -78,10 +75,14 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
}
if(!result)
{
LOG(WARNING) << "msg_handler_telemetry invalid event";
}
}
channel_msg_receiver_cc::channel_msg_receiver_cc(ChannelFsm* channel_fsm, bool repeat) :
channel_msg_receiver_cc::channel_msg_receiver_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat) :
gr::block("channel_msg_receiver_cc", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
{
this->message_port_register_in(pmt::mp("events"));

8
src/algorithms/channel/libs/channel_msg_receiver_cc.h
View File

@ -38,7 +38,7 @@ class channel_msg_receiver_cc;
typedef boost::shared_ptr<channel_msg_receiver_cc> channel_msg_receiver_cc_sptr;
channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat);
channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat);
/*!
* \brief GNU Radio block that receives asynchronous channel messages from acquisition and tracking blocks
@ -46,11 +46,11 @@ channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fs
class channel_msg_receiver_cc : public gr::block
{
private:
ChannelFsm* d_channel_fsm;
std::shared_ptr<ChannelFsm> d_channel_fsm;
bool d_repeat; // todo: change FSM to include repeat value
friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat);
friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat);
void msg_handler_events(pmt::pmt_t msg);
channel_msg_receiver_cc(ChannelFsm* channel_fsm, bool repeat);
channel_msg_receiver_cc(std::shared_ptr<ChannelFsm> channel_fsm, bool repeat);
public:
~channel_msg_receiver_cc (); //!< Default destructor

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -404,7 +404,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_
void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite & satellite)
{
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
d_GPS_FSM.i_satellite_PRN = d_satellite.get_PRN();
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
}

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc
View File

@ -154,7 +154,7 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__(
//* The time of the last input symbol can be computed from the message ToW and
//* delay by the formulae:
//* \code
//* symbolTime_ms = msg->tow * 6000 + *pdelay * 20
//* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory
d_TOW_at_current_symbol = static_cast<double>(msg.tow) * 6.0 + static_cast<double>(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD;
d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
d_flag_valid_word = true;

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc
View File

@ -192,7 +192,7 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__((
//* The time of the last input symbol can be computed from the message ToW and
//* delay by the formulae:
//* \code
//* symbolTime_ms = msg->tow * 6000 + *pdelay * 10
//* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory
d_TOW_at_current_symbol = (static_cast<double>(msg.tow) * 6.0) + (static_cast<double>(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD;
d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
d_flag_valid_word = true;

2
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

@ -185,6 +185,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
{
gr::thread::scoped_lock lk(d_setlock);
/*
* correct the code phase according to the delay between acq and trk
*/
@ -521,6 +522,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc()
int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
gr::thread::scoped_lock lk(d_setlock);
// process vars
double carr_error_hz = 0.0;
double carr_error_filt_hz = 0.0;

1
src/core/interfaces/acquisition_interface.h
View File

@ -53,7 +53,6 @@ template<typename Data>class concurrent_queue;
class AcquisitionInterface: public GNSSBlockInterface
{
public:
//virtual void set_active(bool active) = 0;
virtual void set_gnss_synchro(Gnss_Synchro* gnss_synchro) = 0;
virtual void set_channel(unsigned int channel) = 0;
virtual void set_threshold(float threshold) = 0;

6
src/core/receiver/control_message_factory.cc
View File

@ -44,18 +44,18 @@ ControlMessageFactory::~ControlMessageFactory()
{}
boost::shared_ptr<gr::message> ControlMessageFactory::GetQueueMessage(unsigned int who, unsigned int what)
gr::message::sptr ControlMessageFactory::GetQueueMessage(unsigned int who, unsigned int what)
{
std::shared_ptr<ControlMessage> control_message = std::make_shared<ControlMessage>();
control_message->who = who;
control_message->what = what;
boost::shared_ptr<gr::message> queue_message = gr::message::make(0, 0, 0, sizeof(ControlMessage));
gr::message::sptr queue_message = gr::message::make(0, 0, 0, sizeof(ControlMessage));
memcpy(queue_message->msg(), control_message.get(), sizeof(ControlMessage));
return queue_message;
}
std::shared_ptr<std::vector<std::shared_ptr<ControlMessage>>> ControlMessageFactory::GetControlMessages(boost::shared_ptr<gr::message> queue_message)
std::shared_ptr<std::vector<std::shared_ptr<ControlMessage>>> ControlMessageFactory::GetControlMessages(gr::message::sptr queue_message)
{
std::shared_ptr<std::vector<std::shared_ptr<ControlMessage>>> control_messages = std::make_shared<std::vector<std::shared_ptr<ControlMessage>>>();
unsigned int control_messages_count = queue_message->length() / sizeof(ControlMessage);

2
src/core/receiver/control_message_factory.h
View File

@ -58,7 +58,7 @@ public:
//! Virtual destructor
virtual ~ControlMessageFactory();
boost::shared_ptr<gr::message> GetQueueMessage(unsigned int who, unsigned int what);
gr::message::sptr GetQueueMessage(unsigned int who, unsigned int what);
std::shared_ptr<std::vector<std::shared_ptr<ControlMessage>>> GetControlMessages(gr::message::sptr queue_message);
};

4
src/core/receiver/control_thread.cc
View File

@ -158,7 +158,7 @@ void ControlThread::run()
}
void ControlThread::set_control_queue(boost::shared_ptr<gr::msg_queue> control_queue)
void ControlThread::set_control_queue(gr::msg_queue::sptr control_queue)
{
if (flowgraph_->running())
{
@ -445,7 +445,7 @@ void ControlThread::init()
void ControlThread::read_control_messages()
{
DLOG(INFO) << "Reading control messages from queue";
boost::shared_ptr<gr::message> queue_message = control_queue_->delete_head();
gr::message::sptr queue_message = control_queue_->delete_head();
if (queue_message != 0)
{
control_messages_ = control_message_factory_->GetControlMessages(queue_message);

4
src/core/receiver/control_thread.h
View File

@ -89,7 +89,7 @@ public:
*
* \param[in] boost::shared_ptr<gr::msg_queue> control_queue
*/
void set_control_queue(boost::shared_ptr<gr::msg_queue> control_queue);
void set_control_queue(gr::msg_queue::sptr control_queue);
unsigned int processed_control_messages()
@ -146,7 +146,7 @@ private:
void apply_action(unsigned int what);
std::shared_ptr<GNSSFlowgraph> flowgraph_;
std::shared_ptr<ConfigurationInterface> configuration_;
boost::shared_ptr<gr::msg_queue> control_queue_;
gr::msg_queue::sptr control_queue_;
std::shared_ptr<ControlMessageFactory> control_message_factory_;
std::shared_ptr<std::vector<std::shared_ptr<ControlMessage>>> control_messages_;
bool stop_;

16
src/core/receiver/gnss_block_factory.cc
View File

@ -151,7 +151,7 @@ GNSSBlockFactory::~GNSSBlockFactory()
std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetSignalSource(
std::shared_ptr<ConfigurationInterface> configuration, boost::shared_ptr<gr::msg_queue> queue, int ID)
std::shared_ptr<ConfigurationInterface> configuration, gr::msg_queue::sptr queue, int ID)
{
std::string default_implementation = "File_Signal_Source";
std::string role = "SignalSource"; //backwards compatibility for old conf files
@ -264,7 +264,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetPVT(std::shared_ptr<Con
std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_1C(
std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue)
gr::msg_queue::sptr queue)
{
//"appendix" is added to the "role" with the aim of Acquisition, Tracking and Telemetry Decoder adapters
//can find their specific configurations when they read the config
@ -332,7 +332,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_1C(
std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_2S(
std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue)
gr::msg_queue::sptr queue)
{
LOG(INFO) << "Instantiating Channel " << channel << " with Acquisition Implementation: "
@ -397,7 +397,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_2S(
std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_1B(
std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue)
gr::msg_queue::sptr queue)
{
std::stringstream stream;
stream << channel;
@ -464,7 +464,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_1B(
std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_5X(
std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue)
gr::msg_queue::sptr queue)
{
std::stringstream stream;
stream << channel;
@ -531,7 +531,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_5X(
std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_L5(
std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue)
gr::msg_queue::sptr queue)
{
std::stringstream stream;
stream << channel;
@ -597,7 +597,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_L5(
std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFactory::GetChannels(
std::shared_ptr<ConfigurationInterface> configuration, boost::shared_ptr<gr::msg_queue> queue)
std::shared_ptr<ConfigurationInterface> configuration, gr::msg_queue::sptr queue)
{
std::string default_implementation = "Pass_Through";
std::string tracking_implementation;
@ -783,7 +783,7 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
std::shared_ptr<ConfigurationInterface> configuration,
std::string role,
std::string implementation, unsigned int in_streams,
unsigned int out_streams, boost::shared_ptr<gr::msg_queue> queue)
unsigned int out_streams, gr::msg_queue::sptr queue)
{
std::unique_ptr<GNSSBlockInterface> block;

16
src/core/receiver/gnss_block_factory.h
View File

@ -57,7 +57,7 @@ public:
GNSSBlockFactory();
virtual ~GNSSBlockFactory();
std::unique_ptr<GNSSBlockInterface> GetSignalSource(std::shared_ptr<ConfigurationInterface> configuration,
boost::shared_ptr<gr::msg_queue> queue, int ID = -1);
gr::msg_queue::sptr queue, int ID = -1);
std::unique_ptr<GNSSBlockInterface> GetSignalConditioner(std::shared_ptr<ConfigurationInterface> configuration, int ID = -1);
@ -66,7 +66,7 @@ public:
std::unique_ptr<GNSSBlockInterface> GetObservables(std::shared_ptr<ConfigurationInterface> configuration);
std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GetChannels(std::shared_ptr<ConfigurationInterface> configuration,
boost::shared_ptr<gr::msg_queue> queue);
gr::msg_queue::sptr queue);
/*
* \brief Returns the block with the required configuration and implementation
@ -74,29 +74,29 @@ public:
std::unique_ptr<GNSSBlockInterface> GetBlock(std::shared_ptr<ConfigurationInterface> configuration,
std::string role, std::string implementation,
unsigned int in_streams, unsigned int out_streams,
boost::shared_ptr<gr::msg_queue> queue = nullptr);
gr::msg_queue::sptr queue = nullptr);
private:
std::unique_ptr<GNSSBlockInterface> GetChannel_1C(std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue);
gr::msg_queue::sptr queue);
std::unique_ptr<GNSSBlockInterface> GetChannel_2S(std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue);
gr::msg_queue::sptr queue);
std::unique_ptr<GNSSBlockInterface> GetChannel_1B(std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue);
gr::msg_queue::sptr queue);
std::unique_ptr<GNSSBlockInterface> GetChannel_5X(std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue);
gr::msg_queue::sptr queue);
std::unique_ptr<GNSSBlockInterface> GetChannel_L5(std::shared_ptr<ConfigurationInterface> configuration,
std::string acq, std::string trk, std::string tlm, int channel,
boost::shared_ptr<gr::msg_queue> queue);
gr::msg_queue::sptr queue);
std::unique_ptr<AcquisitionInterface> GetAcqBlock(
std::shared_ptr<ConfigurationInterface> configuration,

94
src/core/receiver/gnss_flowgraph.cc
View File

@ -33,7 +33,6 @@
*/
#include "gnss_flowgraph.h"
#include <memory>
#include <algorithm>
#include <exception>
#include <iostream>
@ -51,8 +50,7 @@
using google::LogMessage;
GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr<ConfigurationInterface> configuration,
boost::shared_ptr<gr::msg_queue> queue)
GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr<ConfigurationInterface> configuration, gr::msg_queue::sptr queue)
{
connected_ = false;
running_ = false;
@ -91,12 +89,6 @@ void GNSSFlowgraph::start()
void GNSSFlowgraph::stop()
{
// for (unsigned int i = 0; i < channels_count_; i++)
// {
// channels_.at(i)->stop_channel();
// LOG(INFO) << "Channel " << i << " in state " << channels_state_[i];
// }
// LOG(INFO) << "Threads finished. Return to main program.";
top_block_->stop();
running_ = false;
}
@ -291,18 +283,13 @@ void GNSSFlowgraph::connect()
}
std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str(); // use channel's implicit signal!
while (gnss_signal.compare(available_GNSS_signals_.front().get_signal_str()) != 0 )
{
available_GNSS_signals_.push_back(available_GNSS_signals_.front());
available_GNSS_signals_.pop_front();
}
channels_.at(i)->set_signal(available_GNSS_signals_.front());
channels_.at(i)->set_signal(search_next_signal(gnss_signal, false));
if (channels_state_[i] == 1)
{
channels_.at(i)->start_acquisition();
available_GNSS_signals_.pop_front();
LOG(INFO) << "Channel " << i << " assigned to " << available_GNSS_signals_.front();
LOG(INFO) << "Channel " << i << " assigned to " << channels_.at(i)->get_signal();
LOG(INFO) << "Channel " << i << " connected to observables and ready for acquisition";
}
else
@ -373,77 +360,59 @@ bool GNSSFlowgraph::send_telemetry_msg(pmt::pmt_t msg)
*/
void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
{
DLOG(INFO) << "received " << what << " from " << who;
DLOG(INFO) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_;
switch (what)
{
case 0:
LOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str();
DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str();
available_GNSS_signals_.push_back(channels_.at(who)->get_signal());
//TODO: Optimize the channel and signal matching!
while ( channels_.at(who)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 )
{
available_GNSS_signals_.push_back(available_GNSS_signals_.front());
available_GNSS_signals_.pop_front();
}
channels_.at(who)->set_signal(available_GNSS_signals_.front());
available_GNSS_signals_.pop_front();
usleep(100);
LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str();
channels_.at(who)->set_signal(search_next_signal(channels_.at(who)->get_signal().get_signal_str(), true));
DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str();
channels_.at(who)->start_acquisition();
break;
case 1:
LOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite();
channels_state_[who] = 2;
acq_channels_count_--;
if (!available_GNSS_signals_.empty() && acq_channels_count_ < max_acq_channels_)
{
for (unsigned int i = 0; i < channels_count_; i++)
{
if (channels_state_[i] == 0)
if(!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_) && (channels_state_[i] == 0))
{
channels_state_[i] = 1;
while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 )
{
available_GNSS_signals_.push_back(available_GNSS_signals_.front());
available_GNSS_signals_.pop_front();
}
channels_.at(i)->set_signal(available_GNSS_signals_.front());
available_GNSS_signals_.pop_front();
channels_.at(i)->set_signal(search_next_signal(channels_.at(i)->get_signal().get_signal_str(), true));
acq_channels_count_++;
DLOG(INFO) << "Channel "<< i << " Starting acquisition " << channels_.at(i)->get_signal().get_satellite() << ", Signal " << channels_.at(i)->get_signal().get_signal_str();
channels_.at(i)->start_acquisition();
break;
}
DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i];
}
}
break;
case 2:
LOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite();
DLOG(INFO) << "Number of channels in acquisition = " << acq_channels_count_;
if (acq_channels_count_ < max_acq_channels_)
{
channels_state_[who] = 1;
acq_channels_count_++;
LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str();
channels_.at(who)->start_acquisition();
}
else
{
channels_state_[who] = 0;
LOG(INFO) << "Channel "<< who << " Idle state";
available_GNSS_signals_.push_back( channels_.at(who)->get_signal() );
}
// for (unsigned int i = 0; i < channels_count_; i++)
// {
// LOG(INFO) << "Channel " << i << " in state " << channels_state_[i] << std::endl;
// }
break;
default:
break;
}
DLOG(INFO) << "Number of available signals: " << available_GNSS_signals_.size();
applied_actions_++;
}
@ -555,7 +524,6 @@ void GNSSFlowgraph::init()
set_signals_list();
set_channels_state();
applied_actions_ = 0;
DLOG(INFO) << "Blocks instantiated. " << channels_count_ << " channels.";
}
@ -718,7 +686,7 @@ void GNSSFlowgraph::set_signals_list()
if (configuration_->property("Channels_5X.count", 0) > 0 )
{
/*
* Loop to create the list of Galileo E1 B signals
* Loop to create the list of Galileo E5a signals
*/
for (available_gnss_prn_iter = available_galileo_prn.cbegin();
available_gnss_prn_iter != available_galileo_prn.cend();
@ -754,14 +722,6 @@ void GNSSFlowgraph::set_signals_list()
gnss_it = available_GNSS_signals_.insert(gnss_it, signal_value);
}
}
// **** FOR DEBUGGING THE LIST OF GNSS SIGNALS ****
// std::list<Gnss_Signal>::const_iterator available_gnss_list_iter;
// for (available_gnss_list_iter = available_GNSS_signals_.cbegin(); available_gnss_list_iter
// != available_GNSS_signals_.cend(); available_gnss_list_iter++)
// {
// std::cout << *available_gnss_list_iter << std::endl;
// }
}
@ -776,14 +736,22 @@ void GNSSFlowgraph::set_channels_state()
channels_state_.reserve(channels_count_);
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);
if (i < max_acq_channels_) {channels_state_.push_back(1);}
else {channels_state_.push_back(0);}
DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i];
}
acq_channels_count_ = max_acq_channels_;
DLOG(INFO) << acq_channels_count_ << " channels in acquisition state";
}
Gnss_Signal GNSSFlowgraph::search_next_signal(std::string searched_signal, bool pop)
{
while(searched_signal.compare(available_GNSS_signals_.front().get_signal_str()) != 0)
{
available_GNSS_signals_.push_back(available_GNSS_signals_.front());
available_GNSS_signals_.pop_front();
}
Gnss_Signal result = available_GNSS_signals_.front();
if(pop){available_GNSS_signals_.pop_front();}
return result;
}

6
src/core/receiver/gnss_flowgraph.h
View File

@ -63,8 +63,7 @@ public:
/*!
* \brief Constructor that initializes the receiver flowgraph
*/
GNSSFlowgraph(std::shared_ptr<ConfigurationInterface> configuration,
boost::shared_ptr<gr::msg_queue> queue);
GNSSFlowgraph(std::shared_ptr<ConfigurationInterface> configuration, gr::msg_queue::sptr queue);
/*!
* \brief Virtual destructor
@ -119,6 +118,7 @@ private:
void set_signals_list();
void set_channels_state(); // Initializes the channels state (start acquisition or keep standby)
// using the configuration parameters (number of channels and max channels in acquisition)
Gnss_Signal search_next_signal(std::string searched_signal, bool pop);
bool connected_;
bool running_;
int sources_count_;
@ -139,7 +139,7 @@ private:
std::vector<std::shared_ptr<ChannelInterface>> channels_;
gnss_sdr_sample_counter_sptr ch_out_sample_counter;
gr::top_block_sptr top_block_;
boost::shared_ptr<gr::msg_queue> queue_;
gr::msg_queue::sptr queue_;
std::list<Gnss_Signal> available_GNSS_signals_;
std::vector<unsigned int> channels_state_;
};

2
src/core/system_parameters/gnss_signal.h
View File

@ -51,7 +51,7 @@ public:
Gnss_Signal(const std::string& signal_);
Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_);
~Gnss_Signal();
std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1B" for Galileo E1B, "5X" for Galileo E5a}
std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1B" for Galileo E1B, "5X" for Galileo E5a, "L5" for GPS L5}
Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal
friend bool operator== (const Gnss_Signal &, const Gnss_Signal &); //!< operator== for comparison
friend std::ostream& operator<<(std::ostream &, const Gnss_Signal &); //!< operator<< for pretty printing