mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-06-30 08:53:15 +00:00
Some code cleaning
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@120 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
parent
dab517aff0
commit
bc62d8d5be
|
@ -98,8 +98,8 @@ bool pseudoranges_pairCompare_min( std::pair<int,gnss_pseudorange> a, std::pair<
|
||||||
|
|
||||||
|
|
||||||
int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
|
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||||
|
{
|
||||||
d_sample_counter++;
|
d_sample_counter++;
|
||||||
|
|
||||||
std::map<int,gnss_pseudorange> gnss_pseudoranges_map;
|
std::map<int,gnss_pseudorange> gnss_pseudoranges_map;
|
||||||
|
|
|
@ -59,7 +59,7 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool
|
||||||
}
|
}
|
||||||
catch (std::ifstream::failure e)
|
catch (std::ifstream::failure e)
|
||||||
{
|
{
|
||||||
std::cout << "Exception opening PVT lib dump file "<<e.what()<<"\r\n";
|
std::cout << "Exception opening PVT lib dump file "<< e.what() << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -257,7 +257,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
|
||||||
satpos(1,i) = d_ephemeris[i].d_satpos_Y;
|
satpos(1,i) = d_ephemeris[i].d_satpos_Y;
|
||||||
satpos(2,i) = d_ephemeris[i].d_satpos_Z;
|
satpos(2,i) = d_ephemeris[i].d_satpos_Z;
|
||||||
LOG_AT_LEVEL(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X
|
LOG_AT_LEVEL(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X
|
||||||
<<" [m] Y="<<d_ephemeris[i].d_satpos_Y<<" [m] Z="<<d_ephemeris[i].d_satpos_Z<<" [m]\r\n";
|
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
|
||||||
obs(i) = gnss_pseudoranges_iter->second.pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
|
obs(i) = gnss_pseudoranges_iter->second.pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
|
||||||
valid_obs++;
|
valid_obs++;
|
||||||
}
|
}
|
||||||
|
|
|
@ -46,30 +46,30 @@ bool kml_printer::set_headers(std::string filename)
|
||||||
if (kml_file.is_open())
|
if (kml_file.is_open())
|
||||||
{
|
{
|
||||||
DLOG(INFO) << "KML printer writing on " << filename.c_str();
|
DLOG(INFO) << "KML printer writing on " << filename.c_str();
|
||||||
kml_file<<"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\r\n"
|
kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
|
||||||
<<"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\r\n"
|
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
|
||||||
<<" <Document>\r\n"
|
<< " <Document>" << std::endl
|
||||||
<<" <name>GNSS Track</name>\r\n"
|
<< " <name>GNSS Track</name>" << std::endl
|
||||||
<< " <description>GNSS-SDR Receiver position log file created at " << asctime (timeinfo)
|
<< " <description>GNSS-SDR Receiver position log file created at " << asctime (timeinfo)
|
||||||
<<" </description>\r\n"
|
<< " </description>" << std::endl
|
||||||
<<"<Style id=\"yellowLineGreenPoly\">\r\n"
|
<< "<Style id=\"yellowLineGreenPoly\">" << std::endl
|
||||||
<<" <LineStyle>\r\n"
|
<< " <LineStyle>" << std::endl
|
||||||
<<" <color>7f00ffff</color>\r\n"
|
<< " <color>7f00ffff</color>" << std::endl
|
||||||
<<" <width>1</width>\r\n"
|
<< " <width>1</width>" << std::endl
|
||||||
<<" </LineStyle>\r\n"
|
<< " </LineStyle>" << std::endl
|
||||||
<<"<PolyStyle>\r\n"
|
<< "<PolyStyle>" << std::endl
|
||||||
<<" <color>7f00ff00</color>\r\n"
|
<< " <color>7f00ff00</color>" << std::endl
|
||||||
<<"</PolyStyle>\r\n"
|
<< "</PolyStyle>" << std::endl
|
||||||
<<"</Style>\r\n"
|
<< "</Style>" << std::endl
|
||||||
<<"<Placemark>\r\n"
|
<< "<Placemark>" << std::endl
|
||||||
<<"<name>GNSS-SDR PVT</name>\r\n"
|
<< "<name>GNSS-SDR PVT</name>" << std::endl
|
||||||
<<"<description>GNSS-SDR position log</description>\r\n"
|
<< "<description>GNSS-SDR position log</description>" << std::endl
|
||||||
<<"<styleUrl>#yellowLineGreenPoly</styleUrl>\r\n"
|
<< "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
|
||||||
<<"<LineString>\r\n"
|
<< "<LineString>" << std::endl
|
||||||
<<"<extrude>0</extrude>\r\n"
|
<< "<extrude>0</extrude>" << std::endl
|
||||||
<<"<tessellate>1</tessellate>\r\n"
|
<< "<tessellate>1</tessellate>" << std::endl
|
||||||
<<"<altitudeMode>absolute</altitudeMode>\r\n"
|
<< "<altitudeMode>absolute</altitudeMode>" << std::endl
|
||||||
<<"<coordinates>\r\n";
|
<< "<coordinates>" << std::endl;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
|
@ -96,9 +96,10 @@ bool kml_printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v
|
||||||
longitude=position->d_avg_longitude_d;
|
longitude=position->d_avg_longitude_d;
|
||||||
height=position->d_avg_height_m;
|
height=position->d_avg_height_m;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (kml_file.is_open())
|
if (kml_file.is_open())
|
||||||
{
|
{
|
||||||
kml_file<<longitude<<","<<latitude<<","<<height<<"\r\n";
|
kml_file << longitude << "," << latitude << "," << height << std::endl;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
|
@ -111,10 +112,10 @@ bool kml_printer::close_file()
|
||||||
{
|
{
|
||||||
if (kml_file.is_open())
|
if (kml_file.is_open())
|
||||||
{
|
{
|
||||||
kml_file<<"</coordinates>\r\n"
|
kml_file<<"</coordinates>" << std::endl
|
||||||
<<"</LineString>\r\n"
|
<<"</LineString>" << std::endl
|
||||||
<<"</Placemark>\r\n"
|
<<"</Placemark>" << std::endl
|
||||||
<<"</Document>\r\n"
|
<<"</Document>" << std::endl
|
||||||
<<"</kml>";
|
<<"</kml>";
|
||||||
kml_file.close();
|
kml_file.close();
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -72,6 +72,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
|
||||||
|
|
||||||
repeat_ = configuration->property("Acquisition" + boost::lexical_cast<
|
repeat_ = configuration->property("Acquisition" + boost::lexical_cast<
|
||||||
std::string>(channel_) + ".repeat_satellite", false);
|
std::string>(channel_) + ".repeat_satellite", false);
|
||||||
|
|
||||||
DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_
|
DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
|
|
||||||
|
@ -160,7 +161,6 @@ gr_basic_block_sptr Channel::get_left_block()
|
||||||
|
|
||||||
gr_basic_block_sptr Channel::get_right_block()
|
gr_basic_block_sptr Channel::get_right_block()
|
||||||
{
|
{
|
||||||
|
|
||||||
return nav_->get_right_block();
|
return nav_->get_right_block();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -34,33 +34,26 @@
|
||||||
#include <glog/log_severity.h>
|
#include <glog/log_severity.h>
|
||||||
#include <glog/logging.h>
|
#include <glog/logging.h>
|
||||||
|
|
||||||
struct Ev_gps_channel_start_acquisition: sc::event<
|
struct Ev_gps_channel_start_acquisition: sc::event<Ev_gps_channel_start_acquisition>
|
||||||
Ev_gps_channel_start_acquisition>
|
|
||||||
{};
|
{};
|
||||||
|
|
||||||
struct Ev_gps_channel_valid_acquisition: sc::event<
|
struct Ev_gps_channel_valid_acquisition: sc::event<Ev_gps_channel_valid_acquisition>
|
||||||
Ev_gps_channel_valid_acquisition>
|
|
||||||
{};
|
{};
|
||||||
|
|
||||||
struct Ev_gps_channel_failed_acquisition_repeat: sc::event<
|
struct Ev_gps_channel_failed_acquisition_repeat: sc::event<Ev_gps_channel_failed_acquisition_repeat>
|
||||||
Ev_gps_channel_failed_acquisition_repeat>
|
|
||||||
{};
|
{};
|
||||||
|
|
||||||
struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event<
|
struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event<Ev_gps_channel_failed_acquisition_no_repeat>
|
||||||
Ev_gps_channel_failed_acquisition_no_repeat>
|
|
||||||
{};
|
{};
|
||||||
|
|
||||||
struct Ev_gps_channel_failed_tracking: sc::event<
|
struct Ev_gps_channel_failed_tracking: sc::event<Ev_gps_channel_failed_tracking>
|
||||||
Ev_gps_channel_failed_tracking>
|
|
||||||
{};
|
{};
|
||||||
|
|
||||||
struct gps_channel_idle_fsm_S0: public sc::state<gps_channel_idle_fsm_S0,
|
struct gps_channel_idle_fsm_S0: public sc::state<gps_channel_idle_fsm_S0,GpsL1CaChannelFsm>
|
||||||
GpsL1CaChannelFsm>
|
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
// sc::transition(event, next state)
|
// sc::transition(event, next state)
|
||||||
typedef sc::transition<Ev_gps_channel_start_acquisition,
|
typedef sc::transition<Ev_gps_channel_start_acquisition, gps_channel_acquiring_fsm_S1> reactions;
|
||||||
gps_channel_acquiring_fsm_S1> reactions;
|
|
||||||
gps_channel_idle_fsm_S0(my_context ctx) :
|
gps_channel_idle_fsm_S0(my_context ctx) :
|
||||||
my_base(ctx)
|
my_base(ctx)
|
||||||
{
|
{
|
||||||
|
@ -68,72 +61,74 @@ public:
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
struct gps_channel_acquiring_fsm_S1: public sc::state<
|
|
||||||
gps_channel_acquiring_fsm_S1, GpsL1CaChannelFsm>
|
struct gps_channel_acquiring_fsm_S1: public sc::state<gps_channel_acquiring_fsm_S1, GpsL1CaChannelFsm>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
typedef mpl::list<sc::transition<
|
typedef mpl::list<sc::transition<Ev_gps_channel_failed_acquisition_no_repeat, gps_channel_waiting_fsm_S3>,
|
||||||
Ev_gps_channel_failed_acquisition_no_repeat,
|
sc::transition<Ev_gps_channel_failed_acquisition_repeat, gps_channel_acquiring_fsm_S1>,
|
||||||
gps_channel_waiting_fsm_S3>, sc::transition<
|
sc::transition<Ev_gps_channel_valid_acquisition, gps_channel_tracking_fsm_S2> >reactions;
|
||||||
Ev_gps_channel_failed_acquisition_repeat,
|
|
||||||
gps_channel_acquiring_fsm_S1>, sc::transition<
|
|
||||||
Ev_gps_channel_valid_acquisition, gps_channel_tracking_fsm_S2> >
|
|
||||||
reactions;
|
|
||||||
|
|
||||||
gps_channel_acquiring_fsm_S1(my_context ctx) :
|
gps_channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx)
|
||||||
my_base(ctx)
|
|
||||||
{
|
{
|
||||||
//std::cout << "Enter Channel_Acq_S1 " << std::endl;
|
//std::cout << "Enter Channel_Acq_S1 " << std::endl;
|
||||||
context<GpsL1CaChannelFsm> ().start_acquisition();
|
context<GpsL1CaChannelFsm> ().start_acquisition();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
struct gps_channel_tracking_fsm_S2: public sc::state<
|
|
||||||
gps_channel_tracking_fsm_S2, GpsL1CaChannelFsm>
|
|
||||||
|
struct gps_channel_tracking_fsm_S2: public sc::state<gps_channel_tracking_fsm_S2, GpsL1CaChannelFsm>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
typedef sc::transition<Ev_gps_channel_failed_tracking,
|
typedef sc::transition<Ev_gps_channel_failed_tracking, gps_channel_acquiring_fsm_S1> reactions;
|
||||||
gps_channel_acquiring_fsm_S1> reactions;
|
|
||||||
|
|
||||||
gps_channel_tracking_fsm_S2(my_context ctx) :
|
gps_channel_tracking_fsm_S2(my_context ctx) : my_base(ctx)
|
||||||
my_base(ctx)
|
|
||||||
{
|
{
|
||||||
//std::cout << "Enter Channel_tracking_S2 " << std::endl;
|
//std::cout << "Enter Channel_tracking_S2 " << std::endl;
|
||||||
context<GpsL1CaChannelFsm> ().start_tracking();
|
context<GpsL1CaChannelFsm> ().start_tracking();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
struct gps_channel_waiting_fsm_S3: public sc::state<
|
|
||||||
gps_channel_waiting_fsm_S3, GpsL1CaChannelFsm>
|
|
||||||
|
struct gps_channel_waiting_fsm_S3: public sc::state<gps_channel_waiting_fsm_S3, GpsL1CaChannelFsm>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
typedef sc::transition<Ev_gps_channel_start_acquisition,
|
typedef sc::transition<Ev_gps_channel_start_acquisition, gps_channel_acquiring_fsm_S1> reactions;
|
||||||
gps_channel_acquiring_fsm_S1> reactions;
|
|
||||||
|
|
||||||
gps_channel_waiting_fsm_S3(my_context ctx) :
|
gps_channel_waiting_fsm_S3(my_context ctx) : my_base(ctx)
|
||||||
my_base(ctx)
|
|
||||||
{
|
{
|
||||||
//std::cout << "Enter Channel_waiting_S3 " << std::endl;
|
//std::cout << "Enter Channel_waiting_S3 " << std::endl;
|
||||||
context<GpsL1CaChannelFsm> ().request_satellite();
|
context<GpsL1CaChannelFsm> ().request_satellite();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
GpsL1CaChannelFsm::GpsL1CaChannelFsm()
|
GpsL1CaChannelFsm::GpsL1CaChannelFsm()
|
||||||
{
|
{
|
||||||
initiate(); //start the FSM
|
initiate(); //start the FSM
|
||||||
}
|
}
|
||||||
|
|
||||||
GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) :
|
|
||||||
acq_(acquisition)
|
|
||||||
|
|
||||||
|
GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) : acq_(acquisition)
|
||||||
{
|
{
|
||||||
initiate(); //start the FSM
|
initiate(); //start the FSM
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::Event_gps_start_acquisition()
|
void GpsL1CaChannelFsm::Event_gps_start_acquisition()
|
||||||
{
|
{
|
||||||
this->process_event(Ev_gps_channel_start_acquisition());
|
this->process_event(Ev_gps_channel_start_acquisition());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::Event_gps_valid_acquisition()
|
void GpsL1CaChannelFsm::Event_gps_valid_acquisition()
|
||||||
{
|
{
|
||||||
this->process_event(Ev_gps_channel_valid_acquisition());
|
this->process_event(Ev_gps_channel_valid_acquisition());
|
||||||
|
@ -144,40 +139,56 @@ void GpsL1CaChannelFsm::Event_gps_failed_acquisition_repeat()
|
||||||
this->process_event(Ev_gps_channel_failed_acquisition_repeat());
|
this->process_event(Ev_gps_channel_failed_acquisition_repeat());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::Event_gps_failed_acquisition_no_repeat()
|
void GpsL1CaChannelFsm::Event_gps_failed_acquisition_no_repeat()
|
||||||
{
|
{
|
||||||
this->process_event(Ev_gps_channel_failed_acquisition_no_repeat());
|
this->process_event(Ev_gps_channel_failed_acquisition_no_repeat());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::Event_gps_failed_tracking()
|
void GpsL1CaChannelFsm::Event_gps_failed_tracking()
|
||||||
{
|
{
|
||||||
this->process_event(Ev_gps_channel_failed_tracking());
|
this->process_event(Ev_gps_channel_failed_tracking());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::set_acquisition(AcquisitionInterface *acquisition)
|
void GpsL1CaChannelFsm::set_acquisition(AcquisitionInterface *acquisition)
|
||||||
{
|
{
|
||||||
acq_ = acquisition;
|
acq_ = acquisition;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::set_tracking(TrackingInterface *tracking)
|
void GpsL1CaChannelFsm::set_tracking(TrackingInterface *tracking)
|
||||||
{
|
{
|
||||||
trk_ = tracking;
|
trk_ = tracking;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::set_queue(gr_msg_queue_sptr queue)
|
void GpsL1CaChannelFsm::set_queue(gr_msg_queue_sptr queue)
|
||||||
{
|
{
|
||||||
queue_ = queue;
|
queue_ = queue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::set_channel(unsigned int channel)
|
void GpsL1CaChannelFsm::set_channel(unsigned int channel)
|
||||||
{
|
{
|
||||||
channel_ = channel;
|
channel_ = channel;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::start_acquisition()
|
void GpsL1CaChannelFsm::start_acquisition()
|
||||||
{
|
{
|
||||||
acq_->reset();
|
acq_->reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::start_tracking()
|
void GpsL1CaChannelFsm::start_tracking()
|
||||||
{
|
{
|
||||||
LOG_AT_LEVEL(INFO) << "Channel " << channel_
|
LOG_AT_LEVEL(INFO) << "Channel " << channel_
|
||||||
|
@ -193,6 +204,8 @@ void GpsL1CaChannelFsm::start_tracking()
|
||||||
trk_->start_tracking();
|
trk_->start_tracking();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void GpsL1CaChannelFsm::request_satellite()
|
void GpsL1CaChannelFsm::request_satellite()
|
||||||
{
|
{
|
||||||
ControlMessageFactory* cmf = new ControlMessageFactory();
|
ControlMessageFactory* cmf = new ControlMessageFactory();
|
||||||
|
@ -201,6 +214,5 @@ void GpsL1CaChannelFsm::request_satellite()
|
||||||
queue_->handle(cmf->GetQueueMessage(channel_, 0));
|
queue_->handle(cmf->GetQueueMessage(channel_, 0));
|
||||||
}
|
}
|
||||||
delete cmf;
|
delete cmf;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -77,7 +77,7 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
|
||||||
std::cout << "Observables dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
|
std::cout << "Observables dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
|
||||||
}
|
}
|
||||||
catch (std::ifstream::failure e) {
|
catch (std::ifstream::failure e) {
|
||||||
std::cout << "Exception opening observables dump file "<<e.what()<<"\r\n";
|
std::cout << "Exception opening observables dump file " << e.what() << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -221,7 +221,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
|
||||||
//{
|
//{
|
||||||
//std::cout<<"PREAMBLE NAVIGATION NOW!\r\n";
|
//std::cout<<"PREAMBLE NAVIGATION NOW!\r\n";
|
||||||
//d_sample_counter=0;
|
//d_sample_counter=0;
|
||||||
|
|
||||||
//}
|
//}
|
||||||
current_prn_timestamps_ms_iter = min_element(current_prn_timestamps_ms.begin(), current_prn_timestamps_ms.end(), pairCompare_double);
|
current_prn_timestamps_ms_iter = min_element(current_prn_timestamps_ms.begin(), current_prn_timestamps_ms.end(), pairCompare_double);
|
||||||
|
|
||||||
|
@ -270,8 +269,9 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
|
||||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
catch (std::ifstream::failure e) {
|
catch (std::ifstream::failure e)
|
||||||
std::cout << "Exception writing observables dump file "<<e.what()<<"\r\n";
|
{
|
||||||
|
std::cout << "Exception writing observables dump file "<< e.what() << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
consume_each(1); //one by one
|
consume_each(1); //one by one
|
||||||
|
|
|
@ -58,8 +58,9 @@ gps_l1_ca_make_telemetry_decoder_cc(unsigned int satellite, long if_freq, long f
|
||||||
fs_in, vector_length, queue, dump));
|
fs_in, vector_length, queue, dump));
|
||||||
}
|
}
|
||||||
|
|
||||||
void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items,
|
|
||||||
gr_vector_int &ninput_items_required)
|
|
||||||
|
void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
|
||||||
{
|
{
|
||||||
for (unsigned i = 0; i < 3; i++)
|
for (unsigned i = 0; i < 3; i++)
|
||||||
{
|
{
|
||||||
|
@ -68,6 +69,7 @@ void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items,
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
|
gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
|
||||||
int vector_length, gr_msg_queue_sptr queue, bool dump) :
|
int vector_length, gr_msg_queue_sptr queue, bool dump) :
|
||||||
gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(double)),
|
gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(double)),
|
||||||
|
@ -118,14 +120,13 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int sate
|
||||||
d_flag_parity = false;
|
d_flag_parity = false;
|
||||||
|
|
||||||
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
|
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc()
|
gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc()
|
||||||
{
|
{
|
||||||
delete d_preambles_symbols;
|
delete d_preambles_symbols;
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -150,18 +151,18 @@ bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
|
||||||
t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
|
t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
|
||||||
|
|
||||||
// Now XOR the 5 6-bit fields together to produce the 6-bit final result.
|
// Now XOR the 5 6-bit fields together to produce the 6-bit final result.
|
||||||
|
|
||||||
parity = t ^ _lrotl(t,6) ^ _lrotl(t,12) ^ _lrotl(t,18) ^ _lrotl(t,24);
|
parity = t ^ _lrotl(t,6) ^ _lrotl(t,12) ^ _lrotl(t,18) ^ _lrotl(t,24);
|
||||||
parity = parity & 0x3F;
|
parity = parity & 0x3F;
|
||||||
if (parity == (gpsword&0x3F))
|
if (parity == (gpsword&0x3F)) return(true);
|
||||||
return(true);
|
else return(false);
|
||||||
else
|
|
||||||
return(false);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
|
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||||
|
{
|
||||||
int corr_value = 0;
|
int corr_value = 0;
|
||||||
int preamble_diff;
|
int preamble_diff;
|
||||||
|
|
||||||
|
|
|
@ -64,21 +64,27 @@ using google::LogMessage;
|
||||||
|
|
||||||
gps_l1_ca_dll_pll_tracking_cc_sptr
|
gps_l1_ca_dll_pll_tracking_cc_sptr
|
||||||
gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
|
gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
|
||||||
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
|
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips)
|
||||||
|
{
|
||||||
return gps_l1_ca_dll_pll_tracking_cc_sptr(new gps_l1_ca_dll_pll_tracking_cc(satellite, if_freq,
|
return gps_l1_ca_dll_pll_tracking_cc_sptr(new gps_l1_ca_dll_pll_tracking_cc(satellite, if_freq,
|
||||||
fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips));
|
fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
|
void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
|
||||||
gr_vector_int &ninput_items_required){
|
gr_vector_int &ninput_items_required)
|
||||||
|
{
|
||||||
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call
|
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
|
gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
|
||||||
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
|
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
|
||||||
gr_block ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
|
gr_block ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
|
||||||
gr_make_io_signature(5, 5, sizeof(double))) {
|
gr_make_io_signature(5, 5, sizeof(double)))
|
||||||
|
{
|
||||||
|
|
||||||
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
|
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
|
||||||
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
|
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
|
||||||
|
@ -139,14 +145,15 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
|
||||||
d_carrier_lock_threshold = 5;
|
d_carrier_lock_threshold = 5;
|
||||||
}
|
}
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
|
void gps_l1_ca_dll_pll_tracking_cc::start_tracking()
|
||||||
|
{
|
||||||
/*
|
/*
|
||||||
* correct the code phase according to the delay between acq and trk
|
* correct the code phase according to the delay between acq and trk
|
||||||
*/
|
*/
|
||||||
unsigned long int acq_trk_diff_samples;
|
unsigned long int acq_trk_diff_samples;
|
||||||
float acq_trk_diff_seconds;
|
float acq_trk_diff_seconds;
|
||||||
acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
|
acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
|
||||||
std::cout<<"acq_trk_diff_samples="<<acq_trk_diff_samples<<"\r\n";
|
std::cout << "acq_trk_diff_samples=" << acq_trk_diff_samples << std::endl;
|
||||||
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in;
|
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in;
|
||||||
//doppler effect
|
//doppler effect
|
||||||
// Fd=(C/(C+Vr))*F
|
// Fd=(C/(C+Vr))*F
|
||||||
|
@ -161,14 +168,12 @@ void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
|
||||||
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
|
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
|
||||||
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
|
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
|
||||||
|
|
||||||
|
|
||||||
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
||||||
d_next_prn_length_samples = round(T_prn_mod_samples);
|
d_next_prn_length_samples = round(T_prn_mod_samples);
|
||||||
#else
|
#else
|
||||||
d_next_prn_length_samples = std::round(T_prn_mod_samples);
|
d_next_prn_length_samples = std::round(T_prn_mod_samples);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
|
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
|
||||||
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
|
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
|
||||||
float T_prn_diff_seconds;
|
float T_prn_diff_seconds;
|
||||||
|
@ -206,16 +211,19 @@ void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
|
||||||
|
|
||||||
// DEBUG OUTPUT
|
// DEBUG OUTPUT
|
||||||
std::cout << "Tracking start on channel " << d_channel << " for satellite ID* " << this->d_satellite << std::endl;
|
std::cout << "Tracking start on channel " << d_channel << " for satellite ID* " << this->d_satellite << std::endl;
|
||||||
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
|
DLOG(INFO) << "Start tracking for satellite "<< this->d_satellite << " received" << std::endl;
|
||||||
|
|
||||||
// enable tracking
|
// enable tracking
|
||||||
d_pull_in = true;
|
d_pull_in = true;
|
||||||
d_enable_tracking = true;
|
d_enable_tracking = true;
|
||||||
|
|
||||||
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" Code Phase correction [samples]="<<delay_correction_samples<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
|
std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz << " Code Phase correction [samples]=" << delay_correction_samples << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
|
void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
|
||||||
{
|
{
|
||||||
float tcode_chips;
|
float tcode_chips;
|
||||||
|
@ -247,13 +255,17 @@ void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
|
void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
|
||||||
{
|
{
|
||||||
float phase_rad, phase_step_rad;
|
float phase_rad, phase_step_rad;
|
||||||
|
|
||||||
phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz / (float)d_fs_in;
|
phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz / (float)d_fs_in;
|
||||||
phase_rad = d_rem_carr_phase_rad;
|
phase_rad = d_rem_carr_phase_rad;
|
||||||
for(int i = 0; i < d_current_prn_length_samples; i++) {
|
for(int i = 0; i < d_current_prn_length_samples; i++)
|
||||||
|
{
|
||||||
d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad));
|
d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad));
|
||||||
phase_rad += phase_step_rad;
|
phase_rad += phase_step_rad;
|
||||||
}
|
}
|
||||||
|
@ -261,7 +273,11 @@ void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
|
||||||
d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + d_rem_carr_phase_rad;
|
d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + d_rem_carr_phase_rad;
|
||||||
}
|
}
|
||||||
|
|
||||||
gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
|
|
||||||
|
|
||||||
|
|
||||||
|
gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc()
|
||||||
|
{
|
||||||
d_dump_file.close();
|
d_dump_file.close();
|
||||||
delete[] d_ca_code;
|
delete[] d_ca_code;
|
||||||
delete[] d_early_code;
|
delete[] d_early_code;
|
||||||
|
@ -271,12 +287,18 @@ gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
|
||||||
delete[] d_Prompt_buffer;
|
delete[] d_Prompt_buffer;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/* Tracking signal processing
|
/* Tracking signal processing
|
||||||
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
|
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
|
||||||
*/
|
*/
|
||||||
|
|
||||||
int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
|
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||||
|
{
|
||||||
|
|
||||||
// if ((unsigned int)ninput_items[0]<(d_vector_length*2))
|
// if ((unsigned int)ninput_items[0]<(d_vector_length*2))
|
||||||
// {
|
// {
|
||||||
|
@ -325,13 +347,14 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
|
|
||||||
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
|
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
|
||||||
double **out = (double **) &output_items[0];
|
double **out = (double **) &output_items[0];
|
||||||
|
|
||||||
// check for samples consistency
|
// check for samples consistency
|
||||||
for(int i=0; i<d_current_prn_length_samples; i++) {
|
for(int i=0; i<d_current_prn_length_samples; i++) {
|
||||||
if (std::isnan(in[i].real()) == true or std::isnan(in[i].imag()) == true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
|
if (std::isnan(in[i].real()) == true or std::isnan(in[i].imag()) == true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
|
||||||
{
|
{
|
||||||
const int samples_available = ninput_items[0];
|
const int samples_available = ninput_items[0];
|
||||||
d_sample_counter = d_sample_counter + samples_available;
|
d_sample_counter = d_sample_counter + samples_available;
|
||||||
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<<d_sample_counter;
|
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<< d_sample_counter << std::endl;
|
||||||
consume_each(samples_available);
|
consume_each(samples_available);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -358,6 +381,7 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
d_Prompt += bb_signal_sample * d_prompt_code[i];
|
d_Prompt += bb_signal_sample * d_prompt_code[i];
|
||||||
d_Late += bb_signal_sample * d_late_code[i];
|
d_Late += bb_signal_sample * d_late_code[i];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute PLL error and update carrier NCO -
|
// Compute PLL error and update carrier NCO -
|
||||||
carr_error = pll_cloop_two_quadrant_atan(d_Prompt) / (float)TWO_PI;
|
carr_error = pll_cloop_two_quadrant_atan(d_Prompt) / (float)TWO_PI;
|
||||||
// Implement carrier loop filter and generate NCO command
|
// Implement carrier loop filter and generate NCO command
|
||||||
|
@ -412,7 +436,9 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
// fill buffer with prompt correlator output values
|
// fill buffer with prompt correlator output values
|
||||||
d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
|
d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
|
||||||
d_cn0_estimation_counter++;
|
d_cn0_estimation_counter++;
|
||||||
}else{
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
d_cn0_estimation_counter = 0;
|
d_cn0_estimation_counter = 0;
|
||||||
d_CN0_SNV_dB_Hz = gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in);
|
d_CN0_SNV_dB_Hz = gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in);
|
||||||
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
|
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
|
||||||
|
@ -421,12 +447,14 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
if (d_carrier_lock_test < d_carrier_lock_threshold or d_carrier_lock_test > MINIMUM_VALID_CN0)
|
if (d_carrier_lock_test < d_carrier_lock_threshold or d_carrier_lock_test > MINIMUM_VALID_CN0)
|
||||||
{
|
{
|
||||||
d_carrier_lock_fail_counter++;
|
d_carrier_lock_fail_counter++;
|
||||||
}else{
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
|
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
|
||||||
}
|
}
|
||||||
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
|
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
|
||||||
{
|
{
|
||||||
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
|
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ;
|
||||||
tracking_message = 3; //loss of lock
|
tracking_message = 3; //loss of lock
|
||||||
d_channel_internal_queue->push(tracking_message);
|
d_channel_internal_queue->push(tracking_message);
|
||||||
d_carrier_lock_fail_counter = 0;
|
d_carrier_lock_fail_counter = 0;
|
||||||
|
@ -466,7 +494,8 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
|
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
|
||||||
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
|
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
|
||||||
}
|
}
|
||||||
}else
|
}
|
||||||
|
else
|
||||||
{
|
{
|
||||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||||
{
|
{
|
||||||
|
@ -475,7 +504,9 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
|
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}else{
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
double **out = (double **) &output_items[0]; //block output streams pointer
|
double **out = (double **) &output_items[0]; //block output streams pointer
|
||||||
*out[0] = 0;
|
*out[0] = 0;
|
||||||
*out[1] = 0;
|
*out[1] = 0;
|
||||||
|
@ -495,7 +526,8 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
tmp_E = std::abs<float>(d_Early);
|
tmp_E = std::abs<float>(d_Early);
|
||||||
tmp_P = std::abs<float>(d_Prompt);
|
tmp_P = std::abs<float>(d_Prompt);
|
||||||
tmp_L = std::abs<float>(d_Late);
|
tmp_L = std::abs<float>(d_Late);
|
||||||
try {
|
try
|
||||||
|
{
|
||||||
// EPR
|
// EPR
|
||||||
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
||||||
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
||||||
|
@ -530,8 +562,9 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||||
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
|
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
|
||||||
}
|
}
|
||||||
catch (std::ifstream::failure e) {
|
catch (std::ifstream::failure e)
|
||||||
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
|
{
|
||||||
|
std::cout << "Exception writing trk dump file " << e.what() << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -541,22 +574,37 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||||
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
|
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
|
||||||
}
|
}
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void gps_l1_ca_dll_pll_tracking_cc::set_acq_code_phase(float code_phase)
|
||||||
|
{
|
||||||
d_acq_code_phase_samples = code_phase;
|
d_acq_code_phase_samples = code_phase;
|
||||||
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
|
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
|
||||||
}
|
}
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::set_acq_doppler(float doppler) {
|
|
||||||
|
|
||||||
|
|
||||||
|
void gps_l1_ca_dll_pll_tracking_cc::set_acq_doppler(float doppler)
|
||||||
|
{
|
||||||
d_acq_carrier_doppler_hz = doppler;
|
d_acq_carrier_doppler_hz = doppler;
|
||||||
LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
|
LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
|
||||||
}
|
}
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::set_satellite(unsigned int satellite) {
|
|
||||||
|
|
||||||
|
void gps_l1_ca_dll_pll_tracking_cc::set_satellite(unsigned int satellite)
|
||||||
|
{
|
||||||
d_satellite = satellite;
|
d_satellite = satellite;
|
||||||
LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite;
|
LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite;
|
||||||
}
|
}
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel) {
|
void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel)
|
||||||
|
{
|
||||||
d_channel = channel;
|
d_channel = channel;
|
||||||
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
|
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
|
||||||
// ############# ENABLE DATA FILE LOG #################
|
// ############# ENABLE DATA FILE LOG #################
|
||||||
|
@ -564,25 +612,32 @@ void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel) {
|
||||||
{
|
{
|
||||||
if (d_dump_file.is_open()==false)
|
if (d_dump_file.is_open()==false)
|
||||||
{
|
{
|
||||||
try {
|
try
|
||||||
|
{
|
||||||
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
||||||
d_dump_filename.append(".dat");
|
d_dump_filename.append(".dat");
|
||||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||||
std::cout << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
|
std::cout << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
|
||||||
}
|
}
|
||||||
catch (std::ifstream::failure e) {
|
catch (std::ifstream::failure e)
|
||||||
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
|
{
|
||||||
|
std::cout << "channel "<< d_channel << " Exception opening trk dump file " << e.what() << std::endl;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
|
void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
|
||||||
{
|
{
|
||||||
d_acq_sample_stamp = sample_stamp;
|
d_acq_sample_stamp = sample_stamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void gps_l1_ca_dll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
|
void gps_l1_ca_dll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
|
||||||
{
|
{
|
||||||
d_channel_internal_queue = channel_internal_queue;
|
d_channel_internal_queue = channel_internal_queue;
|
||||||
|
|
|
@ -129,6 +129,5 @@ float carrier_lock_detector(gr_complex* Prompt_buffer, int length)
|
||||||
NBD = tmp_sum_abs_I*tmp_sum_abs_I + tmp_sum_abs_Q*tmp_sum_abs_Q;
|
NBD = tmp_sum_abs_I*tmp_sum_abs_I + tmp_sum_abs_Q*tmp_sum_abs_Q;
|
||||||
NBP = tmp_sum_sqr_I - tmp_sum_sqr_Q;
|
NBP = tmp_sum_sqr_I - tmp_sum_sqr_Q;
|
||||||
return NBD/NBP;
|
return NBD/NBP;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -38,7 +38,8 @@
|
||||||
#include "tracking_2nd_DLL_filter.h"
|
#include "tracking_2nd_DLL_filter.h"
|
||||||
|
|
||||||
|
|
||||||
void tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
|
void tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k)
|
||||||
|
{
|
||||||
// Solve natural frequency
|
// Solve natural frequency
|
||||||
float Wn;
|
float Wn;
|
||||||
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
|
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
|
||||||
|
@ -47,20 +48,26 @@ void tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float
|
||||||
*tau2 = (2.0 * zeta) / Wn;
|
*tau2 = (2.0 * zeta) / Wn;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void tracking_2nd_DLL_filter::set_DLL_BW(float dll_bw_hz)
|
void tracking_2nd_DLL_filter::set_DLL_BW(float dll_bw_hz)
|
||||||
{
|
{
|
||||||
//Calculate filter coefficient values
|
//Calculate filter coefficient values
|
||||||
d_dllnoisebandwidth =dll_bw_hz;
|
d_dllnoisebandwidth =dll_bw_hz;
|
||||||
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio, 1.0);// Calculate filter coefficient values
|
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio, 1.0);// Calculate filter coefficient values
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void tracking_2nd_DLL_filter::initialize(float d_acq_code_phase_samples)
|
void tracking_2nd_DLL_filter::initialize(float d_acq_code_phase_samples)
|
||||||
{
|
{
|
||||||
// code tracking loop parameters
|
// code tracking loop parameters
|
||||||
d_old_code_nco = 0.0;
|
d_old_code_nco = 0.0;
|
||||||
d_old_code_error = 0.0;
|
d_old_code_error = 0.0;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
|
float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
|
||||||
{
|
{
|
||||||
float code_nco;
|
float code_nco;
|
||||||
|
@ -70,6 +77,8 @@ float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
|
||||||
return code_nco;
|
return code_nco;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
tracking_2nd_DLL_filter::tracking_2nd_DLL_filter ()
|
tracking_2nd_DLL_filter::tracking_2nd_DLL_filter ()
|
||||||
{
|
{
|
||||||
d_pdi_code = 0.001;// Summation interval for code
|
d_pdi_code = 0.001;// Summation interval for code
|
||||||
|
@ -77,6 +86,5 @@ tracking_2nd_DLL_filter::tracking_2nd_DLL_filter ()
|
||||||
}
|
}
|
||||||
|
|
||||||
tracking_2nd_DLL_filter::~tracking_2nd_DLL_filter ()
|
tracking_2nd_DLL_filter::~tracking_2nd_DLL_filter ()
|
||||||
{
|
{}
|
||||||
|
|
||||||
}
|
|
||||||
|
|
|
@ -78,7 +78,9 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
|
||||||
if (prompt_s1.imag() != 0.0)
|
if (prompt_s1.imag() != 0.0)
|
||||||
{
|
{
|
||||||
return atan(prompt_s1.real() / prompt_s1.imag());
|
return atan(prompt_s1.real() / prompt_s1.imag());
|
||||||
}else{
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -240,9 +240,12 @@ signed long int gps_navigation_message::read_navigation_signed(std::bitset<GPS_S
|
||||||
if (bits[GPS_SUBFRAME_BITS-slices[0].position]==1)
|
if (bits[GPS_SUBFRAME_BITS-slices[0].position]==1)
|
||||||
{
|
{
|
||||||
value^=0xFFFFFFFF;
|
value^=0xFFFFFFFF;
|
||||||
}else{
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
value&=0;
|
value&=0;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int i=0; i<num_of_slices; i++)
|
for (int i=0; i<num_of_slices; i++)
|
||||||
{
|
{
|
||||||
for (int j=0; j<slices[i].length; j++)
|
for (int j=0; j<slices[i].length; j++)
|
||||||
|
@ -270,7 +273,8 @@ double gps_navigation_message::check_t(double time)
|
||||||
if (time > half_week)
|
if (time > half_week)
|
||||||
{
|
{
|
||||||
corrTime = time - 2*half_week;
|
corrTime = time - 2*half_week;
|
||||||
}else if (time < -half_week)
|
}
|
||||||
|
else if (time < -half_week)
|
||||||
{
|
{
|
||||||
corrTime = time + 2*half_week;
|
corrTime = time + 2*half_week;
|
||||||
}
|
}
|
||||||
|
@ -403,11 +407,6 @@ void gps_navigation_message::satellitePosition(double transmitTime)
|
||||||
d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
|
d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
|
||||||
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
|
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
|
||||||
d_satvel_Z = d_satpos_Y * sin(i);
|
d_satvel_Z = d_satpos_Y * sin(i);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -455,7 +454,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
|
||||||
//std::cout<<"subframe ID="<<subframe_ID<<std::endl;
|
//std::cout<<"subframe ID="<<subframe_ID<<std::endl;
|
||||||
|
|
||||||
// Decode all 5 sub-frames
|
// Decode all 5 sub-frames
|
||||||
switch (subframe_ID){
|
switch (subframe_ID)
|
||||||
|
{
|
||||||
//--- Decode the sub-frame id ------------------------------------------
|
//--- Decode the sub-frame id ------------------------------------------
|
||||||
// ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf
|
// ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf
|
||||||
case 1:
|
case 1:
|
||||||
|
@ -598,7 +598,6 @@ int gps_navigation_message::subframe_decoder(char *subframe)
|
||||||
if (SV_page == 18)
|
if (SV_page == 18)
|
||||||
{
|
{
|
||||||
// Page 18 - Ionospheric and UTC data
|
// Page 18 - Ionospheric and UTC data
|
||||||
|
|
||||||
d_alpha0 = (double)read_navigation_signed(subframe_bits, ALPHA_0, num_of_slices(ALPHA_0));
|
d_alpha0 = (double)read_navigation_signed(subframe_bits, ALPHA_0, num_of_slices(ALPHA_0));
|
||||||
d_alpha0 = d_alpha0 * ALPHA_0_LSB;
|
d_alpha0 = d_alpha0 * ALPHA_0_LSB;
|
||||||
d_alpha1 = (double)read_navigation_signed(subframe_bits, ALPHA_1, num_of_slices(ALPHA_1));
|
d_alpha1 = (double)read_navigation_signed(subframe_bits, ALPHA_1, num_of_slices(ALPHA_1));
|
||||||
|
@ -626,7 +625,6 @@ int gps_navigation_message::subframe_decoder(char *subframe)
|
||||||
i_WN_LSF = (int)read_navigation_unsigned(subframe_bits, WN_LSF, num_of_slices(WN_LSF));
|
i_WN_LSF = (int)read_navigation_unsigned(subframe_bits, WN_LSF, num_of_slices(WN_LSF));
|
||||||
i_DN = (int)read_navigation_unsigned(subframe_bits, DN, num_of_slices(DN));; // Right-justified ?
|
i_DN = (int)read_navigation_unsigned(subframe_bits, DN, num_of_slices(DN));; // Right-justified ?
|
||||||
d_DeltaT_LSF = (double)read_navigation_signed(subframe_bits, DELTAT_LSF, num_of_slices(DELTAT_LSF));
|
d_DeltaT_LSF = (double)read_navigation_signed(subframe_bits, DELTAT_LSF, num_of_slices(DELTAT_LSF));
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (SV_page == 25)
|
if (SV_page == 25)
|
||||||
|
@ -641,8 +639,6 @@ int gps_navigation_message::subframe_decoder(char *subframe)
|
||||||
almanacHealth[30] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV30, num_of_slices(HEALTH_SV30));
|
almanacHealth[30] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV30, num_of_slices(HEALTH_SV30));
|
||||||
almanacHealth[31] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV31, num_of_slices(HEALTH_SV31));
|
almanacHealth[31] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV31, num_of_slices(HEALTH_SV31));
|
||||||
almanacHealth[32] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV32, num_of_slices(HEALTH_SV32));
|
almanacHealth[32] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV32, num_of_slices(HEALTH_SV32));
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
@ -768,7 +764,6 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
|
||||||
double secondsOfWeekBeforeToday= 43200 * floor(gpstime_corrected / 43200);
|
double secondsOfWeekBeforeToday= 43200 * floor(gpstime_corrected / 43200);
|
||||||
t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
|
t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
|
||||||
return t_utc;
|
return t_utc;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -180,9 +180,9 @@ public:
|
||||||
double d_DeltaT_LSF; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
|
double d_DeltaT_LSF; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
|
||||||
|
|
||||||
// Satellite velocity
|
// Satellite velocity
|
||||||
double d_satvel_X;
|
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m]
|
||||||
double d_satvel_Y;
|
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m]
|
||||||
double d_satvel_Z;
|
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m]
|
||||||
|
|
||||||
// public functions
|
// public functions
|
||||||
void reset();
|
void reset();
|
||||||
|
|
Loading…
Reference in New Issue
Block a user