From bc62d8d5bea717b1038b1275b5648a978d340e76 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 11 Jan 2012 09:01:24 +0000 Subject: [PATCH] Some code cleaning git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@120 64b25241-fba3-4117-9849-534c7e92360d --- src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc | 6 +- .../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc | 4 +- src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 178 ++-- src/algorithms/PVT/libs/kml_printer.cc | 63 +- src/algorithms/PVT/libs/rinex_printer.cc | 18 +- src/algorithms/channel/adapters/channel.cc | 6 +- .../channel/libs/gps_l1_ca_channel_fsm.cc | 92 +- .../adapters/gps_l1_ca_observables.cc | 4 +- .../gps_l1_ca_observables_cc.cc | 110 +-- .../gps_l1_ca_telemetry_decoder_cc.cc | 173 ++-- .../gps_l1_ca_dll_pll_tracking_cc.cc | 861 ++++++++++-------- src/algorithms/tracking/libs/CN_estimators.cc | 51 +- .../tracking/libs/tracking_2nd_DLL_filter.cc | 22 +- .../tracking/libs/tracking_discriminators.cc | 24 +- .../gps_navigation_message.cc | 413 +++++---- .../gps_navigation_message.h | 6 +- 16 files changed, 1052 insertions(+), 979 deletions(-) diff --git a/src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc b/src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc index 6c871843d..bd3788adf 100644 --- a/src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc +++ b/src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc @@ -58,10 +58,10 @@ GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration, DLOG(INFO) << "role " << role; int averaging_depth; - averaging_depth=configuration->property(role + ".averaging_depth", 10); + averaging_depth = configuration->property(role + ".averaging_depth", 10); bool flag_averaging; - flag_averaging=configuration->property(role + ".flag_averaging", false); + flag_averaging = configuration->property(role + ".flag_averaging", false); dump_ = configuration->property(role + ".dump", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); @@ -73,7 +73,7 @@ GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration, pvt_->set_navigation_queue(&global_gps_nav_msg_queue); - DLOG(INFO) << "global navigation message queue assigned to pvt ("<< pvt_->unique_id() << ")"; + DLOG(INFO) << "global navigation message queue assigned to pvt (" << pvt_->unique_id() << ")"; } diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 696268a22..2c2221d7d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -98,8 +98,8 @@ bool pseudoranges_pairCompare_min( std::pair a, std::pair< 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++; std::map gnss_pseudoranges_map; diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc index 755fbddc1..0fb12e36e 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -40,26 +40,26 @@ using google::LogMessage; gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file) { // init empty ephemeris for all the available GNSS channels - d_nchannels=nchannels; - d_ephemeris=new gps_navigation_message[nchannels]; - d_dump_filename=dump_filename; - d_flag_dump_enabled=flag_dump_to_file; - d_averaging_depth=0; + d_nchannels = nchannels; + d_ephemeris = new gps_navigation_message[nchannels]; + d_dump_filename = dump_filename; + d_flag_dump_enabled = flag_dump_to_file; + d_averaging_depth = 0; // ############# ENABLE DATA FILE LOG ################# - if (d_flag_dump_enabled==true) + if (d_flag_dump_enabled == true) { - if (d_dump_file.is_open()==false) + if (d_dump_file.is_open() == false) { try { - 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); - std::cout<<"PVT lib dump enabled Log file: "< gnss_pseudoranges_ { std::map::iterator gnss_pseudoranges_iter; - arma::mat W=arma::eye(d_nchannels,d_nchannels); //channels weights matrix - arma::vec obs=arma::zeros(d_nchannels); // pseudoranges observation vector - arma::mat satpos=arma::zeros(3,d_nchannels); //satellite positions matrix + arma::mat W = arma::eye(d_nchannels,d_nchannels); //channels weights matrix + arma::vec obs = arma::zeros(d_nchannels); // pseudoranges observation vector + arma::mat satpos = arma::zeros(3,d_nchannels); //satellite positions matrix int GPS_week = 0; double GPS_corrected_time = 0; @@ -232,15 +232,15 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_ int valid_obs=0; //valid observations counter for (int i=0; i gnss_pseudoranges_ GPS_corrected_time = d_ephemeris[i].sv_clock_correction(GPS_current_time); GPS_week = d_ephemeris[i].i_GPS_week; - utc =d_ephemeris[i].utc_time(GPS_corrected_time); + utc = d_ephemeris[i].utc_time(GPS_corrected_time); // compute the satellite current ECEF position d_ephemeris[i].satellitePosition(GPS_corrected_time); - satpos(0,i)=d_ephemeris[i].d_satpos_X; - satpos(1,i)=d_ephemeris[i].d_satpos_Y; - satpos(2,i)=d_ephemeris[i].d_satpos_Z; - LOG_AT_LEVEL(INFO)<<"ECEF satellite SV ID="<second.pseudorange_m+d_ephemeris[i].d_satClkCorr*GPS_C_m_s; + satpos(0,i) = d_ephemeris[i].d_satpos_X; + satpos(1,i) = d_ephemeris[i].d_satpos_Y; + 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 + << " [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; valid_obs++; } else { // no valid pseudorange for the current channel - W(i,i)=0; // channel de-activated - obs(i)=1; // to avoid algorithm problems (divide by zero) + W(i,i) = 0; // channel de-activated + obs(i) = 1; // to avoid algorithm problems (divide by zero) } } else { // no valid ephemeris for the current channel - W(i,i)=0; // channel de-activated - obs(i)=1; // to avoid algorithm problems (divide by zero) + W(i,i) = 0; // channel de-activated + obs(i) = 1; // to avoid algorithm problems (divide by zero) } } std::cout<<"PVT: valid observations="< gnss_pseudoranges_ { arma::vec mypos; mypos=leastSquarePos(satpos,obs,W); - LOG_AT_LEVEL(INFO) << "Position at TOW="< gnss_pseudoranges_ } // MOVING AVERAGE PVT - if (flag_averaging==true) + if (flag_averaging == true) { - if (d_hist_longitude_d.size()==(unsigned int)d_averaging_depth) + if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth) { // Pop oldest value d_hist_longitude_d.pop_back(); @@ -339,18 +339,18 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_ d_hist_latitude_d.push_front(d_latitude_d); d_hist_height_m.push_front(d_height_m); - d_avg_latitude_d=0; - d_avg_longitude_d=0; - d_avg_height_m=0; - for (unsigned int i=0;i gnss_pseudoranges_ d_hist_latitude_d.push_front(d_latitude_d); d_hist_height_m.push_front(d_height_m); - d_avg_latitude_d=d_latitude_d; - d_avg_longitude_d=d_longitude_d; - d_avg_height_m=d_height_m; + d_avg_latitude_d = d_latitude_d; + d_avg_longitude_d = d_longitude_d; + d_avg_height_m = d_height_m; return false;//indicates that the returned position is not valid yet // output the average, although it will not have the full historic available // d_avg_latitude_d=0; @@ -411,11 +411,11 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec double lambda; lambda = atan2(Y,X); double ex2; - ex2 = (2-f[elipsoid_selection])*f[elipsoid_selection]/((1-f[elipsoid_selection])*(1-f[elipsoid_selection])); + ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection])); double c; - c = a[elipsoid_selection]*sqrt(1+ex2); + c = a[elipsoid_selection] * sqrt(1+ex2); double phi; - phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection]))*f[elipsoid_selection]))); + phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection]))); double h = 0.1; double oldh = 0; @@ -424,19 +424,19 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec do { oldh = h; - N = c/sqrt(1+ex2*(cos(phi)*cos(phi))); - phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection])*f[elipsoid_selection]*N/(N+h))))); - h = sqrt(X*X+Y*Y)/cos(phi)-N; + N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); + phi = atan(Z / ((sqrt(X*X + Y*Y) * (1 - (2 -f[elipsoid_selection]) * f[elipsoid_selection] *N / (N + h) )))); + h = sqrt(X*X + Y*Y) / cos(phi) - N; iterations = iterations + 1; if (iterations > 100) { - std::cout<<"Failed to approximate h with desired precision. h-oldh= "<\r\n" - <<"\r\n" - <<" \r\n" - <<" GNSS Track\r\n" - <<" GNSS-SDR Receiver position log file created at "<\r\n" - <<"\r\n" - <<"\r\n" - <<"GNSS-SDR PVT\r\n" - <<"GNSS-SDR position log\r\n" - <<"#yellowLineGreenPoly\r\n" - <<"\r\n" - <<"0\r\n" - <<"1\r\n" - <<"absolute\r\n" - <<"\r\n"; + DLOG(INFO) << "KML printer writing on " << filename.c_str(); + kml_file << "" << std::endl + << "" << std::endl + << " " << std::endl + << " GNSS Track" << std::endl + << " GNSS-SDR Receiver position log file created at " << asctime (timeinfo) + << " " << std::endl + << "" << std::endl + << "" << std::endl + << "GNSS-SDR PVT" << std::endl + << "GNSS-SDR position log" << std::endl + << "#yellowLineGreenPoly" << std::endl + << "" << std::endl + << "0" << std::endl + << "1" << std::endl + << "absolute" << std::endl + << "" << std::endl; return true; } else @@ -84,7 +84,7 @@ bool kml_printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v double latitude; double longitude; double height; - if (print_average_values==false) + if (print_average_values == false) { latitude=position->d_latitude_d; longitude=position->d_longitude_d; @@ -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; height=position->d_avg_height_m; } + if (kml_file.is_open()) { - kml_file<\r\n" - <<"\r\n" - <<"\r\n" - <<"\r\n" + kml_file<<"" << std::endl + <<"" << std::endl + <<"" << std::endl + <<"" << std::endl <<""; kml_file.close(); return true; diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 8c9f4e8b9..f71d1b769 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -264,12 +264,12 @@ std::string Rinex_Printer::getLocalTime() std::stringstream strmHour; int utc_hour = pt_tm.tm_hour; - if (utc_hour<10) strmHour << "0"; // two digits for hours + if (utc_hour < 10) strmHour << "0"; // two digits for hours strmHour << utc_hour; std::stringstream strmMin; int utc_minute = pt_tm.tm_min; - if (utc_minute<10) strmMin << "0"; // two digits for minutes + if (utc_minute < 10) strmMin << "0"; // two digits for minutes strmMin << utc_minute; if (version == 2) @@ -371,7 +371,7 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, gps_navigation_message // -------- Line COMMENT line.clear(); line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); - line += Rinex_Printer::leftJustify("COMMENT",20); + line += Rinex_Printer::leftJustify("COMMENT", 20); Rinex_Printer::lengthCheck(line); out << line << std::endl; @@ -547,7 +547,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, gps_navigation_message nav line += satelliteSystem["GPS"]; if (nav_msg.i_satellite_PRN < 10) line += std::string("0"); line += boost::lexical_cast(nav_msg.i_satellite_PRN); - std::string year (timestring,0,4); + std::string year (timestring, 0, 4); line += std::string(1, ' '); line += year; line += std::string(1, ' '); @@ -932,13 +932,13 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, gps_navigation_message std::string hour (timestring, 9, 2); std::string minutes (timestring, 11, 2); double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW)); - double seconds = fmod(utc_t,60); + double seconds = fmod(utc_t, 60); line += Rinex_Printer::rightJustify(year, 6); line += Rinex_Printer::rightJustify(month, 6); line += Rinex_Printer::rightJustify(day, 6); line += Rinex_Printer::rightJustify(hour, 6); line += Rinex_Printer::rightJustify(minutes, 6); - line += Rinex_Printer::rightJustify(asString(seconds,7), 13); + line += Rinex_Printer::rightJustify(asString(seconds, 7), 13); line += Rinex_Printer::rightJustify(std::string("GPS"), 8); line += std::string(9, ' '); line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); @@ -965,7 +965,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, gps_navigation_message nav if (version == 2) { line += "OBSERVATION DATA FILE FOR VERSION 2.11 STILL NOT IMPLEMENTED"; - line += std::string(80-line.size(), ' '); + line += std::string(80 - line.size(), ' '); Rinex_Printer::lengthCheck(line); out << line << std::endl; } @@ -995,7 +995,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, gps_navigation_message nav line += std::string(1, ' '); double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW)); - line += Rinex_Printer::asString(fmod(utc_t,60), 7); + line += Rinex_Printer::asString(fmod(utc_t, 60), 7); line += std::string(2, ' '); // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event line += std::string(1, '0'); @@ -1051,7 +1051,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, gps_navigation_message nav { lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); } - if (lineObs.size()<80) lineObs += std::string(80 - lineObs.size(), ' '); + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); out << lineObs << std::endl; } } diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 08e3a4c0a..a40218a85 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -72,6 +72,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, repeat_ = configuration->property("Acquisition" + boost::lexical_cast< std::string>(channel_) + ".repeat_satellite", false); + DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_ << std::endl; @@ -139,8 +140,8 @@ void Channel::disconnect(gr_top_block_sptr top_block) return; } - top_block->disconnect(acq_->get_right_block(), 0, trk_->get_left_block(),0); - top_block->disconnect(trk_->get_right_block(), 0, nav_->get_left_block(),0); + top_block->disconnect(acq_->get_right_block(), 0, trk_->get_left_block(), 0); + top_block->disconnect(trk_->get_right_block(), 0, nav_->get_left_block(), 0); acq_->disconnect(top_block); trk_->disconnect(top_block); @@ -160,7 +161,6 @@ gr_basic_block_sptr Channel::get_left_block() gr_basic_block_sptr Channel::get_right_block() { - return nav_->get_right_block(); } diff --git a/src/algorithms/channel/libs/gps_l1_ca_channel_fsm.cc b/src/algorithms/channel/libs/gps_l1_ca_channel_fsm.cc index 00c976e6a..23daeaac4 100644 --- a/src/algorithms/channel/libs/gps_l1_ca_channel_fsm.cc +++ b/src/algorithms/channel/libs/gps_l1_ca_channel_fsm.cc @@ -34,33 +34,26 @@ #include #include -struct Ev_gps_channel_start_acquisition: sc::event< -Ev_gps_channel_start_acquisition> +struct Ev_gps_channel_start_acquisition: sc::event {}; -struct Ev_gps_channel_valid_acquisition: sc::event< -Ev_gps_channel_valid_acquisition> +struct Ev_gps_channel_valid_acquisition: sc::event {}; -struct Ev_gps_channel_failed_acquisition_repeat: sc::event< -Ev_gps_channel_failed_acquisition_repeat> +struct Ev_gps_channel_failed_acquisition_repeat: sc::event {}; -struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event< -Ev_gps_channel_failed_acquisition_no_repeat> +struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event {}; -struct Ev_gps_channel_failed_tracking: sc::event< -Ev_gps_channel_failed_tracking> +struct Ev_gps_channel_failed_tracking: sc::event {}; -struct gps_channel_idle_fsm_S0: public sc::state +struct gps_channel_idle_fsm_S0: public sc::state { public: // sc::transition(event, next state) - typedef sc::transition reactions; + typedef sc::transition reactions; gps_channel_idle_fsm_S0(my_context 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 { public: - typedef mpl::list, sc::transition< - Ev_gps_channel_failed_acquisition_repeat, - gps_channel_acquiring_fsm_S1>, sc::transition< - Ev_gps_channel_valid_acquisition, gps_channel_tracking_fsm_S2> > - reactions; + typedef mpl::list, + sc::transition, + sc::transition >reactions; - gps_channel_acquiring_fsm_S1(my_context ctx) : - my_base(ctx) + gps_channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx) { //std::cout << "Enter Channel_Acq_S1 " << std::endl; context ().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 { public: - typedef sc::transition reactions; + typedef sc::transition reactions; - gps_channel_tracking_fsm_S2(my_context ctx) : - my_base(ctx) + gps_channel_tracking_fsm_S2(my_context ctx) : my_base(ctx) { //std::cout << "Enter Channel_tracking_S2 " << std::endl; context ().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 { public: - typedef sc::transition reactions; + typedef sc::transition reactions; - gps_channel_waiting_fsm_S3(my_context ctx) : - my_base(ctx) + gps_channel_waiting_fsm_S3(my_context ctx) : my_base(ctx) { //std::cout << "Enter Channel_waiting_S3 " << std::endl; context ().request_satellite(); } }; + + GpsL1CaChannelFsm::GpsL1CaChannelFsm() { initiate(); //start the FSM } -GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) : - acq_(acquisition) + + + +GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) : acq_(acquisition) { initiate(); //start the FSM } + + + void GpsL1CaChannelFsm::Event_gps_start_acquisition() { this->process_event(Ev_gps_channel_start_acquisition()); } + + void GpsL1CaChannelFsm::Event_gps_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()); } + + void GpsL1CaChannelFsm::Event_gps_failed_acquisition_no_repeat() { this->process_event(Ev_gps_channel_failed_acquisition_no_repeat()); } + + void GpsL1CaChannelFsm::Event_gps_failed_tracking() { this->process_event(Ev_gps_channel_failed_tracking()); } + + void GpsL1CaChannelFsm::set_acquisition(AcquisitionInterface *acquisition) { acq_ = acquisition; } + + void GpsL1CaChannelFsm::set_tracking(TrackingInterface *tracking) { trk_ = tracking; } + + void GpsL1CaChannelFsm::set_queue(gr_msg_queue_sptr queue) { queue_ = queue; } + + void GpsL1CaChannelFsm::set_channel(unsigned int channel) { channel_ = channel; } + + void GpsL1CaChannelFsm::start_acquisition() { acq_->reset(); } + + void GpsL1CaChannelFsm::start_tracking() { LOG_AT_LEVEL(INFO) << "Channel " << channel_ @@ -193,6 +204,8 @@ void GpsL1CaChannelFsm::start_tracking() trk_->start_tracking(); } + + void GpsL1CaChannelFsm::request_satellite() { ControlMessageFactory* cmf = new ControlMessageFactory(); @@ -201,6 +214,5 @@ void GpsL1CaChannelFsm::request_satellite() queue_->handle(cmf->GetQueueMessage(channel_, 0)); } delete cmf; - } diff --git a/src/algorithms/observables/adapters/gps_l1_ca_observables.cc b/src/algorithms/observables/adapters/gps_l1_ca_observables.cc index ef338924f..5b45df265 100644 --- a/src/algorithms/observables/adapters/gps_l1_ca_observables.cc +++ b/src/algorithms/observables/adapters/gps_l1_ca_observables.cc @@ -55,14 +55,14 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration, { int output_rate_ms; - output_rate_ms=configuration->property(role + ".output_rate_ms", 500); + output_rate_ms = configuration->property(role + ".output_rate_ms", 500); std::string default_dump_filename = "./observables.dat"; DLOG(INFO) << "role " << role; bool flag_averaging; - flag_averaging=configuration->property(role + ".flag_averaging", false); + flag_averaging = configuration->property(role + ".flag_averaging", false); dump_ = configuration->property(role + ".dump", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 4ec59e1e2..9261bd1fb 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc @@ -61,23 +61,23 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms d_queue = queue; d_dump = dump; d_nchannels = nchannels; - d_output_rate_ms=output_rate_ms; - d_history_prn_delay_ms= new std::deque[d_nchannels]; - d_dump_filename=dump_filename; - d_flag_averaging=flag_averaging; + d_output_rate_ms = output_rate_ms; + d_history_prn_delay_ms = new std::deque[d_nchannels]; + d_dump_filename = dump_filename; + d_flag_averaging = flag_averaging; // ############# ENABLE DATA FILE LOG ################# - if (d_dump==true) + if (d_dump == true) { - if (d_dump_file.is_open()==false) + if (d_dump_file.is_open() == false) { try { - 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); - std::cout<<"Observables dump enabled Log file: "< a, std::pair b) void clearQueue( std::deque &q ) { std::deque empty; - std::swap( q, empty ); + std::swap(q, empty); } @@ -127,18 +127,18 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni double pseudoranges_timestamp_ms; double traveltime_ms; double pseudorange_m; - int history_shift=0; + int history_shift = 0; double delta_timestamp_ms; double min_delta_timestamp_ms; double actual_min_prn_delay_ms; double current_prn_delay_ms; - int pseudoranges_reference_sat_ID=0; - unsigned int pseudoranges_reference_sat_channel_ID=0; + int pseudoranges_reference_sat_ID = 0; + unsigned int pseudoranges_reference_sat_channel_ID = 0; d_sample_counter++; //count for the processed samples - bool flag_history_ok=true; //flag to indicate that all the queues have filled their timestamp history + bool flag_history_ok = true; //flag to indicate that all the queues have filled their timestamp history /* * 1. Read the GNSS SYNCHRO objects from available channels to obtain the preamble timestamp, current PRN start time and accumulated carrier phase */ @@ -146,12 +146,12 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni { if (in[i][0].valid_word) //if this channel have valid word { - gps_words.insert(std::pair(in[i][0].channel_ID,in[i][0])); //record the word structure in a map for pseudoranges + gps_words.insert(std::pair(in[i][0].channel_ID, in[i][0])); //record the word structure in a map for pseudoranges // RECORD PRN start timestamps history if (d_history_prn_delay_ms[i].size()0 and flag_history_ok==true) + if(gps_words.size() > 0 and flag_history_ok == true) { /* * 2.1 find the minimum preamble timestamp (nearest satellite, will be the reference) */ // The nearest satellite, first preamble to arrive - gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare_gnss_synchro); - min_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; //[ms] + gps_words_iter = min_element(gps_words.begin(), gps_words.end(), pairCompare_gnss_synchro); + min_preamble_delay_ms = gps_words_iter->second.preamble_delay_ms; //[ms] - pseudoranges_reference_sat_ID=gps_words_iter->second.satellite_PRN; // it is the reference! - pseudoranges_reference_sat_channel_ID=gps_words_iter->second.channel_ID; + pseudoranges_reference_sat_ID = gps_words_iter->second.satellite_PRN; // it is the reference! + pseudoranges_reference_sat_channel_ID = gps_words_iter->second.channel_ID; // The farthest satellite, last preamble to arrive - gps_words_iter=max_element(gps_words.begin(),gps_words.end(),pairCompare_gnss_synchro); - max_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; - min_delta_timestamp_ms=gps_words_iter->second.prn_delay_ms-max_preamble_delay_ms; //[ms] + gps_words_iter = max_element(gps_words.begin(), gps_words.end(), pairCompare_gnss_synchro); + max_preamble_delay_ms = gps_words_iter->second.preamble_delay_ms; + min_delta_timestamp_ms = gps_words_iter->second.prn_delay_ms - max_preamble_delay_ms; //[ms] // check if this is a valid set of observations - if ((max_preamble_delay_ms-min_preamble_delay_ms) we select the corresponding history /*! @@ -204,10 +204,10 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni // find again the minimum CURRENT minimum preamble time, taking into account the preamble timeshift for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++) { - delta_timestamp_ms=(gps_words_iter->second.prn_delay_ms-gps_words_iter->second.preamble_delay_ms)-min_delta_timestamp_ms; - history_shift=round(delta_timestamp_ms); + delta_timestamp_ms = (gps_words_iter->second.prn_delay_ms - gps_words_iter->second.preamble_delay_ms) - min_delta_timestamp_ms; + history_shift = round(delta_timestamp_ms); //std::cout<<"history_shift="<(gps_words_iter->second.channel_ID,d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift])); + current_prn_timestamps_ms.insert(std::pair(gps_words_iter->second.channel_ID, d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift])); // debug: preamble position test //if ((d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]-gps_words_iter->second.preamble_delay_ms)<0.1) //{std::cout<<"ch "<second.channel_ID<<" current_prn_time-last_preamble_prn_time="<< @@ -221,13 +221,12 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni //{ //std::cout<<"PREAMBLE NAVIGATION NOW!\r\n"; //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); - actual_min_prn_delay_ms=current_prn_timestamps_ms_iter->second; + actual_min_prn_delay_ms = current_prn_timestamps_ms_iter->second; - pseudoranges_timestamp_ms=actual_min_prn_delay_ms; //save the shortest pseudorange timestamp to compute the current GNSS timestamp + pseudoranges_timestamp_ms = actual_min_prn_delay_ms; //save the shortest pseudorange timestamp to compute the current GNSS timestamp /* * 2.2 compute the pseudoranges */ @@ -236,53 +235,54 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni { // #### compute the pseudorange for this satellite ### - current_prn_delay_ms=current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID); - traveltime_ms=current_prn_delay_ms-actual_min_prn_delay_ms+GPS_STARTOFFSET_ms; //[ms] + current_prn_delay_ms = current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID); + traveltime_ms = current_prn_delay_ms - actual_min_prn_delay_ms + GPS_STARTOFFSET_ms; //[ms] //std::cout<<"delta_time_ms="<second.satellite_PRN; - current_gnss_pseudorange.valid=true; + current_gnss_pseudorange.pseudorange_m = pseudorange_m; + current_gnss_pseudorange.timestamp_ms = pseudoranges_timestamp_ms; + current_gnss_pseudorange.SV_ID = gps_words_iter->second.satellite_PRN; + current_gnss_pseudorange.valid = true; // #### write the pseudorrange block output for this satellite ### - *out[gps_words_iter->second.channel_ID]=current_gnss_pseudorange; + *out[gps_words_iter->second.channel_ID] = current_gnss_pseudorange; } } } - if(d_dump==true) { + if(d_dump == true) { // MULTIPLEXED FILE RECORDING - Record results to file try { double tmp_double; for (unsigned int i=0; id_preambles_bits, (unsigned short int*)preambles_bits, 8* sizeof(unsigned short int)); + memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, 8*sizeof(unsigned short int)); // preamble bits to sampled symbols - d_preambles_symbols=(signed int*)malloc(sizeof(signed int)*8*d_samples_per_bit); - int n=0; - for (int i=0;i<8;i++) + d_preambles_symbols = (signed int*)malloc(sizeof(signed int)*8*d_samples_per_bit); + int n = 0; + for (int i=0; i<8; i++) { - for (unsigned int j=0;j=160) { //TODO: Rewrite with state machine - if (d_stat==0) + if (d_stat == 0) { d_GPS_FSM.Event_gps_word_preamble(); - d_preamble_index=d_sample_counter;//record the preamble sample stamp - std::cout<<"Preamble detection for SAT "<6001) + preamble_diff = d_sample_counter - d_preamble_index; + if (preamble_diff > 6001) { - std::cout<<"Lost of frame sync SAT "<d_satellite<<" preamble_diff= "<d_satellite << " preamble_diff= " << preamble_diff << std::endl; + d_stat = 0; //lost of frame sync + d_flag_frame_sync = false; } } } @@ -261,22 +262,22 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i //d_preamble_phase-=in[3][0]; //******* SYMBOL TO BIT ******* - d_symbol_accumulator+=in[1][d_samples_per_bit*8-1]; // accumulate the input value in d_symbol_accumulator + d_symbol_accumulator += in[1][d_samples_per_bit*8 - 1]; // accumulate the input value in d_symbol_accumulator d_symbol_accumulator_counter++; - if (d_symbol_accumulator_counter==20) + if (d_symbol_accumulator_counter == 20) { - if (d_symbol_accumulator>0) + if (d_symbol_accumulator > 0) { //symbol to bit - d_GPS_frame_4bytes+=1; //insert the telemetry bit in LSB + d_GPS_frame_4bytes +=1; //insert the telemetry bit in LSB } - d_symbol_accumulator=0; - d_symbol_accumulator_counter=0; + d_symbol_accumulator = 0; + d_symbol_accumulator_counter = 0; //******* bits to words ****** d_frame_bit_index++; - if (d_frame_bit_index==30) + if (d_frame_bit_index == 30) { - d_frame_bit_index=0; + d_frame_bit_index = 0; //parity check //Each word in wordbuff is composed of: // Bits 0 to 29 = the GPS data word @@ -284,11 +285,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i // prepare the extended frame [-2 -1 0 ... 30] if (d_prev_GPS_frame_4bytes & 0x00000001) { - d_GPS_frame_4bytes=d_GPS_frame_4bytes|0x40000000; + d_GPS_frame_4bytes = d_GPS_frame_4bytes|0x40000000; } if (d_prev_GPS_frame_4bytes & 0x00000002) { - d_GPS_frame_4bytes=d_GPS_frame_4bytes|0x80000000; + d_GPS_frame_4bytes = d_GPS_frame_4bytes|0x80000000; } /* Check that the 2 most recently logged words pass parity. Have to first invert the data bits according to bit 30 of the previous word. */ @@ -299,17 +300,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes)) { memcpy(&d_GPS_FSM.d_GPS_frame_4bytes,&d_GPS_frame_4bytes,sizeof(char)*4); - d_GPS_FSM.d_preamble_time_ms=d_preamble_time_seconds*1000.0; + d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds*1000.0; d_GPS_FSM.Event_gps_word_valid(); - d_flag_parity=true; + d_flag_parity = true; } else { d_GPS_FSM.Event_gps_word_invalid(); - d_flag_parity=false; + d_flag_parity = false; } - d_prev_GPS_frame_4bytes=d_GPS_frame_4bytes; // save the actual frame - d_GPS_frame_4bytes=d_GPS_frame_4bytes & 0; + d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame + d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0; } else { @@ -320,15 +321,15 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i // output the frame consume_each(1); //one by one - gps_synchro.valid_word=(d_flag_frame_sync==true and d_flag_parity==true); - gps_synchro.flag_preamble=d_flag_preamble; - gps_synchro.preamble_delay_ms=d_preamble_time_seconds*1000.0; - gps_synchro.prn_delay_ms=(in[2][0]-d_preamble_duration_seconds)*1000.0; - gps_synchro.preamble_code_phase_ms=d_preamble_code_phase_seconds*1000.0; - gps_synchro.preamble_code_phase_correction_ms=(in[4][0]-d_preamble_code_phase_seconds)*1000.0; - gps_synchro.satellite_PRN=d_satellite; - gps_synchro.channel_ID=d_channel; - *out[0]=gps_synchro; + gps_synchro.valid_word = (d_flag_frame_sync == true and d_flag_parity == true); + gps_synchro.flag_preamble = d_flag_preamble; + gps_synchro.preamble_delay_ms = d_preamble_time_seconds*1000.0; + gps_synchro.prn_delay_ms = (in[2][0] - d_preamble_duration_seconds)*1000.0; + gps_synchro.preamble_code_phase_ms = d_preamble_code_phase_seconds*1000.0; + gps_synchro.preamble_code_phase_correction_ms = (in[4][0] - d_preamble_code_phase_seconds)*1000.0; + gps_synchro.satellite_PRN = d_satellite; + gps_synchro.channel_ID = d_channel; + *out[0] = gps_synchro; return 1; } @@ -336,14 +337,14 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i void gps_l1_ca_telemetry_decoder_cc::set_satellite(int satellite) { d_satellite = satellite; - d_GPS_FSM.i_satellite_PRN=satellite; + d_GPS_FSM.i_satellite_PRN = satellite; LOG_AT_LEVEL(INFO) << "Navigation Satellite set to " << d_satellite; } void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel) { d_channel = channel; - d_GPS_FSM.i_channel_ID=channel; + d_GPS_FSM.i_channel_ID = channel; LOG_AT_LEVEL(INFO) << "Navigation channel set to " << channel; } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index 8ff1f468a..b4e0fb2b7 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -64,205 +64,221 @@ using google::LogMessage; 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 - 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, 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, - gr_vector_int &ninput_items_required){ - ninput_items_required[0] =(int)d_vector_length*2; //set the required available samples in each call + gr_vector_int &ninput_items_required) +{ + 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 - 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_make_io_signature(5, 5, sizeof(double))) { + 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_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_make_io_signature(3, 3, sizeof(float)),vector_length) { - // initialize internal vars - d_queue = queue; - d_dump = dump; - d_satellite = satellite; - d_if_freq = if_freq; - d_fs_in = fs_in; - d_vector_length = vector_length; - d_dump_filename =dump_filename; + //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) { + // initialize internal vars + d_queue = queue; + d_dump = dump; + d_satellite = satellite; + d_if_freq = if_freq; + d_fs_in = fs_in; + d_vector_length = vector_length; + d_dump_filename = dump_filename; - // Initialize tracking ========================================== + // Initialize tracking ========================================== - d_code_loop_filter.set_DLL_BW(dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); + d_code_loop_filter.set_DLL_BW(dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); - //--- DLL variables -------------------------------------------------------- - d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) + //--- DLL variables -------------------------------------------------------- + d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) - // Initialization of local code replica + // Initialization of local code replica // Get space for a vector with the C/A code replica sampled 1x/chip - d_ca_code=new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+2]; + d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2]; // Get space for the resampled early / prompt / late local replicas - d_early_code= new gr_complex[d_vector_length*2]; - d_prompt_code=new gr_complex[d_vector_length*2]; - d_late_code=new gr_complex[d_vector_length*2]; + d_early_code = new gr_complex[d_vector_length*2]; + d_prompt_code = new gr_complex[d_vector_length*2]; + d_late_code = new gr_complex[d_vector_length*2]; // space for carrier wipeoff and signal baseband vectors - d_carr_sign=new gr_complex[d_vector_length*2]; + d_carr_sign = new gr_complex[d_vector_length*2]; //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO - d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ; + d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ; // define residual code phase (in chips) - d_rem_code_phase_samples = 0.0; + d_rem_code_phase_samples = 0.0; // define residual carrier phase - d_rem_carr_phase_rad = 0.0; + d_rem_carr_phase_rad = 0.0; // sample synchronization - d_sample_counter=0; - d_sample_counter_seconds=0; - d_acq_sample_stamp=0; + d_sample_counter = 0; + d_sample_counter_seconds = 0; + d_acq_sample_stamp = 0; - d_enable_tracking=false; - d_pull_in=false; - d_last_seg=0; + d_enable_tracking = false; + d_pull_in = false; + d_last_seg = 0; - d_current_prn_length_samples=(int)d_vector_length; + d_current_prn_length_samples = (int)d_vector_length; // CN0 estimation and lock detector buffers - d_cn0_estimation_counter=0; - d_Prompt_buffer=new gr_complex[CN0_ESTIMATION_SAMPLES]; - d_carrier_lock_test=1; - d_CN0_SNV_dB_Hz=0; - d_carrier_lock_fail_counter=0; - d_carrier_lock_threshold=5; + d_cn0_estimation_counter = 0; + d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES]; + d_carrier_lock_test = 1; + d_CN0_SNV_dB_Hz = 0; + d_carrier_lock_fail_counter = 0; + d_carrier_lock_threshold = 5; } -void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){ - /* - * correct the code phase according to the delay between acq and trk - */ - unsigned long int acq_trk_diff_samples; - float acq_trk_diff_seconds; - acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp;//-d_vector_length; - std::cout<<"acq_trk_diff_samples="<d_satellite<< std::endl; - DLOG(INFO) << "Start tracking for satellite "<d_satellite<<" received "; + // DEBUG OUTPUT + 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" << std::endl; - // enable tracking - d_pull_in=true; - d_enable_tracking=true; - - std::cout<<"PULL-IN Doppler [Hz]= "<MINIMUM_VALID_CN0) - { - d_carrier_lock_fail_counter++; - }else{ - if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER) - { - std::cout<<"Channel "<push(tracking_message); - d_carrier_lock_fail_counter=0; - d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine + /*! + * \todo Improve the lock detection algorithm! + */ + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt; + d_cn0_estimation_counter++; + } + else + { + 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_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // ###### TRACKING UNLOCK NOTIFICATION ##### + int tracking_message; + if (d_carrier_lock_test < d_carrier_lock_threshold or d_carrier_lock_test > MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ; + tracking_message = 3; //loss of lock + d_channel_internal_queue->push(tracking_message); + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - //std::cout<<"d_carrier_lock_fail_counter"<(d_channel)); - d_dump_filename.append(".dat"); - 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); - std::cout<<"Tracking dump enabled on channel "<(d_channel)); + d_dump_filename.append(".dat"); + 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); + std::cout << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl; + } + catch (std::ifstream::failure e) + { + 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) { d_acq_sample_stamp = sample_stamp; } + + + void gps_l1_ca_dll_pll_tracking_cc::set_channel_queue(concurrent_queue *channel_internal_queue) { d_channel_internal_queue = channel_internal_queue; diff --git a/src/algorithms/tracking/libs/CN_estimators.cc b/src/algorithms/tracking/libs/CN_estimators.cc index 25e5bc56b..2105fa58d 100644 --- a/src/algorithms/tracking/libs/CN_estimators.cc +++ b/src/algorithms/tracking/libs/CN_estimators.cc @@ -70,22 +70,22 @@ float gps_l1_ca_CN0_SNV(gr_complex* Prompt_buffer, int length, long fs_in) //SNR_SNV(count)=Psig/(Ptot-Psig); //CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length); float SNR, SNR_dB_Hz; - float tmp_abs_I,tmp_abs_Q; - float Psig,Ptot; + float tmp_abs_I, tmp_abs_Q; + float Psig, Ptot; //float M2,M4; - Psig=0; - Ptot=0; - for (int i=0;i