1
0
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:
Carles Fernandez 2012-01-11 09:01:24 +00:00
parent dab517aff0
commit bc62d8d5be
16 changed files with 1052 additions and 979 deletions

View File

@ -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;

View File

@ -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++;
} }

View File

@ -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;

View File

@ -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();
} }

View File

@ -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;
} }

View File

@ -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

View File

@ -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;

View File

@ -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;

View File

@ -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;
} }

View File

@ -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 ()
{ {}
}

View File

@ -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;
} }
} }

View File

@ -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;
} }

View File

@ -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();