From 4438ffe9169226178392b2383a2c862309cbceb7 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 28 Jan 2017 15:31:04 +0100 Subject: [PATCH] Code cleaning --- .../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc | 33 +-- src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 64 ++--- src/algorithms/PVT/libs/ls_pvt.cc | 244 +++++++++--------- src/algorithms/PVT/libs/pvt_solution.cc | 18 +- .../gps_l1_ca_observables_cc.cc | 12 +- .../gps_l1_ca_telemetry_decoder_cc.cc | 60 ++--- .../gps_l1_ca_telemetry_decoder_cc.h | 3 - .../gps_l1_ca_dll_pll_tracking_cc.cc | 62 ++--- .../gps_l1_ca_dll_pll_tracking_cc.h | 1 - .../system-tests/obs_gps_l1_system_test.cc | 44 ++-- 10 files changed, 266 insertions(+), 275 deletions(-) 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 8c774fa87..dddb3ee16 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 @@ -227,22 +227,22 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&gps_l1_ca_pvt_cc::msg_handler_telemetry, this, _1)); - //initialize kml_printer + // initialize kml_printer std::string kml_dump_filename; kml_dump_filename = d_dump_filename; d_kml_printer = std::make_shared(); d_kml_printer->set_headers(kml_dump_filename); - //initialize geojson_printer + // initialize geojson_printer std::string geojson_dump_filename; geojson_dump_filename = d_dump_filename; d_geojson_printer = std::make_shared(); d_geojson_printer->set_headers(geojson_dump_filename); - //initialize nmea_printer + // initialize nmea_printer d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); - //initialize rtcm_printer + // initialize rtcm_printer std::string rtcm_dump_filename; rtcm_dump_filename = d_dump_filename; d_rtcm_tcp_port = rtcm_tcp_port; @@ -330,7 +330,7 @@ void gps_l1_ca_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchroniza d_last_status_print_seg = current_rx_seg; std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush; //DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) - // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; + // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; } } @@ -340,7 +340,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g { gnss_observables_map.clear(); d_sample_counter++; - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer + Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer print_receiver_status(in); @@ -366,25 +366,18 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g if (gnss_observables_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() > 0) { // compute on the fly PVT solution - //mod 8/4/2012 Set the PVT computation rate in this block if ((d_sample_counter % d_output_rate_ms) == 0) { bool pvt_result; pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); if (pvt_result == true) { - //correct the observable to account for the receiver clock offset - - for (std::map::iterator it=gnss_observables_map.begin(); it!=gnss_observables_map.end(); ++it) - { - it->second.Pseudorange_m=it->second.Pseudorange_m-d_ls_pvt->d_rx_dt_s*GPS_C_m_s; - } - // send asynchronous message to observables block - // time offset is expressed as the equivalent travel distance [m] - //pmt::pmt_t value = pmt::from_double(d_ls_pvt->d_rx_dt_s); - //this->message_port_pub(pmt::mp("rx_dt_s"), value); - //std::cout<<"d_rx_dt_s*GPS_C_m_s="<d_rx_dt_s*GPS_C_m_s<::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) + { + it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s; + } + if(first_fix == true) { std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d @@ -410,7 +403,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g b_rinex_header_written = true; // do not write header anymore } } - if(b_rinex_header_written) // Put here another condition to separate annotations (e.g 30 s) + if(b_rinex_header_written) { // Limit the RINEX navigation output rate to 1/6 seg // Notice that d_sample_counter period is 1ms (for GPS correlators) 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 4f5cc1247..1e73144f3 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -81,14 +81,14 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, std::map::iterator gnss_pseudoranges_iter; std::map::iterator gps_ephemeris_iter; - arma::vec W;//= arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix - arma::vec obs;// = arma::zeros(valid_pseudoranges); // pseudoranges observation vector - arma::mat satpos;// = arma::zeros(3, valid_pseudoranges); //satellite positions matrix + arma::vec W; // channels weight vector + arma::vec obs; // pseudoranges observation vector + arma::mat satpos; // satellite positions matrix int GPS_week = 0; - double utc = 0; + double utc = 0.0; double TX_time_corrected_s; - double SV_clock_bias_s = 0; + double SV_clock_bias_s = 0.0; d_flag_averaging = flag_averaging; @@ -107,8 +107,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, /*! * \todo Place here the satellite CN0 (power level, or weight factor) */ - W.resize(valid_obs+1,1); - W(valid_obs)=1; + W.resize(valid_obs + 1, 1); + W(valid_obs) = 1; // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // first estimate of transmit time @@ -121,23 +121,24 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, // 3- compute the current ECEF position for this SV using corrected TX time TX_time_corrected_s = Tx_time - SV_clock_bias_s; gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); - satpos.resize(3,valid_obs+1); + satpos.resize(3, valid_obs + 1); satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y; satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z; + // 4- fill the observations vector with the corrected pseudoranges - obs.resize(valid_obs+1,1); - obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s-d_rx_dt_s*GPS_C_m_s; + obs.resize(valid_obs + 1, 1); + obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s - d_rx_dt_s * GPS_C_m_s; d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz; valid_obs++; // SV ECEF DEBUG OUTPUT DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN - << " X=" << gps_ephemeris_iter->second.d_satpos_X - << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y - << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z - << " [m] PR_obs=" << obs(valid_obs) << " [m]"; + << " X=" << gps_ephemeris_iter->second.d_satpos_X + << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y + << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z + << " [m] PR_obs=" << obs(valid_obs) << " [m]"; // compute the UTC time for this SV (just to print the associated UTC timestamp) GPS_week = gps_ephemeris_iter->second.i_GPS_week; @@ -162,25 +163,27 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, DLOG(INFO) << "obs=" << obs; DLOG(INFO) << "W=" << W; - //check if this is the initial position computation - if (d_rx_dt_s==0) - { - //execute Bancroft's algorithm to estimate initial receiver position and time - std::cout<<"Executing Bancroft algorithm...\n"; - rx_position_and_time =bancroftPos(satpos.t(), obs); - d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration - d_rx_dt_s=rx_position_and_time(3)/GPS_C_m_s; //save time for the next iteration [meters]->[seconds] - } + // check if this is the initial position computation + if (d_rx_dt_s == 0) + { + // execute Bancroft's algorithm to estimate initial receiver position and time + DLOG(INFO) << " Executing Bancroft algorithm..."; + rx_position_and_time = bancroftPos(satpos.t(), obs); + d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration + d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds] + } - //Execute WLS using previos position as the initialization point + // Execute WLS using previous position as the initialization point rx_position_and_time = leastSquarePos(satpos, obs, W); - d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration - d_rx_dt_s+=rx_position_and_time(3)/GPS_C_m_s; //accumulate the rx time error for the next iteration [meters]->[seconds] + + d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration + d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; - DLOG(INFO) <<"Accumulated rx clock error="<(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); + // Compute UTC time and print PVT solution double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60) boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast(GPS_week)); @@ -188,13 +191,12 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); d_position_UTC_time = p_time; DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time) - << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d - << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; + << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d + << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; // ###### Compute DOPs ######## - std::cout<<"c\r"; compute_DOP(); - std::cout<<"d\r"; + // ######## LOG FILE ######### if(d_flag_dump_enabled == true) { diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index a4b315f1d..0e9c79f46 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -44,135 +44,137 @@ Ls_Pvt::Ls_Pvt() : Pvt_Solution() } -arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) { - -// %BANCROFT Calculation of preliminary coordinates -// % for a GPS receiver based on pseudoranges -// % to 4 or more satellites. The ECEF -// % coordinates are stored in satpos. The observed pseudoranges are stored in obs -// %Reference: Bancroft, S. (1985) An Algebraic Solution -// % of the GPS Equations, IEEE Trans. Aerosp. -// % and Elec. Systems, AES-21, 56--59 -// %Kai Borre 04-30-95, improved by C.C. Goad 11-24-96 -// %Copyright (c) by Kai Borre -// %$Revision: 1.0 $ $Date: 1997/09/26 $ -// -// % Test values to use in debugging -// % B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029; -// % -12082643.974 -20428242.179 11741374.154 21492579.823; -// % 14373286.650 -10448439.349 19596404.858 21492492.771; -// % 10278432.244 -21116508.618 -12689101.970 25284588.982]; -// % Solution: 595025.053 -4856501.221 4078329.981 -// -// % Test values to use in debugging -// % B_pass = [14177509.188 -18814750.650 12243944.449 21119263.116; -// % 15097198.146 -4636098.555 21326705.426 22527063.486; -// % 23460341.997 -9433577.991 8174873.599 23674159.579; -// % -8206498.071 -18217989.839 17605227.065 20951643.862; -// % 1399135.830 -17563786.820 19705534.862 20155386.649; -// % 6995655.459 -23537808.269 -9927906.485 24222112.972]; -// % Solution: 596902.683 -4847843.316 4088216.740 +arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) +{ + // BANCROFT Calculation of preliminary coordinates for a GPS receiver based on pseudoranges + // to 4 or more satellites. The ECEF coordinates are stored in satpos. + // The observed pseudoranges are stored in obs + // Reference: Bancroft, S. (1985) An Algebraic Solution of the GPS Equations, + // IEEE Trans. Aerosp. and Elec. Systems, AES-21, Issue 1, pp. 56--59 + // Based on code by: + // Kai Borre 04-30-95, improved by C.C. Goad 11-24-96 + // Copyright (c) by Kai Borre + // $Revision: 1.0 $ $Date: 1997/09/26 $ + // + // Test values to use in debugging + // B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029; + // -12082643.974 -20428242.179 11741374.154 21492579.823; + // 14373286.650 -10448439.349 19596404.858 21492492.771; + // 10278432.244 -21116508.618 -12689101.970 25284588.982 ]; + // Solution: 595025.053 -4856501.221 4078329.981 + // + // Test values to use in debugging + // B_pass = [14177509.188 -18814750.650 12243944.449 21119263.116; + // 15097198.146 -4636098.555 21326705.426 22527063.486; + // 23460341.997 -9433577.991 8174873.599 23674159.579; + // -8206498.071 -18217989.839 17605227.065 20951643.862; + // 1399135.830 -17563786.820 19705534.862 20155386.649; + // 6995655.459 -23537808.269 -9927906.485 24222112.972 ]; + // Solution: 596902.683 -4847843.316 4088216.740 arma::vec pos = arma::zeros(4,1); - arma::mat B_pass=arma::zeros(obs.size(),4); - B_pass.submat(0,0,obs.size()-1,2)=satpos; - B_pass.col(3)=obs; + arma::mat B_pass = arma::zeros(obs.size(), 4); + B_pass.submat(0, 0, obs.size() - 1, 2) = satpos; + B_pass.col(3) = obs; arma::mat B; arma::mat BBB; - double traveltime=0; - for (int iter = 0; iter<2; iter++) - { - B = B_pass; - int m=arma::size(B,0); - for (int i=0;i 3) - { - BBB = arma::inv(B.t()*B)*B.t(); - } - else - { - BBB = arma::inv(B); - } - arma::vec e = arma::ones(m,1); - arma::vec alpha = arma::zeros(m,1); - for (int i =0; i 3) + { + BBB = arma::inv(B.t() * B) * B.t(); + } + else + { + BBB = arma::inv(B); + } + arma::vec e = arma::ones(m,1); + arma::vec alpha = arma::zeros(m,1); + for (int i = 0; i < m; i++) + { + alpha(i) = lorentz(B.row(i).t(), B.row(i).t()) / 2.0; + } + arma::mat BBBe = BBB * e; + arma::mat BBBalpha = BBB * alpha; + double a = lorentz(BBBe, BBBe); + double b = lorentz(BBBe, BBBalpha) - 1; + double c = lorentz(BBBalpha, BBBalpha); + double root = sqrt(b * b - a * c); + arma::vec r = {(-b - root) / a, (-b + root) / a}; + arma::mat possible_pos = arma::zeros(4,2); + for (int i = 0; i < 2; i++) + { + possible_pos.col(i) = r(i) * BBBe + BBBalpha; + possible_pos(3,i) = -possible_pos(3,i); + } - arma::vec abs_omc=arma::zeros(2,1); - for (int j=0; j abs_omc(1)) - { - pos = possible_pos.col(1); - } - else - { - pos = possible_pos.col(0); - } - }// % iter loop + //discrimination between roots + if (abs_omc(0) > abs_omc(1)) + { + pos = possible_pos.col(1); + } + else + { + pos = possible_pos.col(0); + } + }// % iter loop return pos; } -double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y) { -// %LORENTZ Calculates the Lorentz inner product of the two -// % 4 by 1 vectors x and y -// -// %Kai Borre 04-22-95 -// %Copyright (c) by Kai Borre -// %$Revision: 1.0 $ $Date: 1997/09/26 $ -// -// % M = diag([1 1 1 -1]); -// % p = x'*M*y; - return(x(0)*y(0) + x(1)*y(1) + x(2)*y(2) - x(3)*y(3)); +double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y) +{ + // LORENTZ Calculates the Lorentz inner product of the two + // 4 by 1 vectors x and y + // Based ob code by: + // Kai Borre 04-22-95 + // Copyright (c) by Kai Borre + // $Revision: 1.0 $ $Date: 1997/09/26 $ + // + // M = diag([1 1 1 -1]); + // p = x'*M*y; + + return(x(0) * y(0) + x(1) * y(1) + x(2) * y(2) - x(3) * y(3)); } + arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec) { /* Computes the Least Squares Solution. @@ -190,10 +192,10 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs int nmbOfIterations = 10; // TODO: include in config int nmbOfSatellites; nmbOfSatellites = satpos.n_cols; //Armadillo - arma::mat w=arma::zeros(nmbOfSatellites,nmbOfSatellites); - w.diag()=w_vec; //diagonal weight matrix + arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites); + w.diag() = w_vec; //diagonal weight matrix - arma::vec pos = {{d_rx_pos(0)},{d_rx_pos(0)},{d_rx_pos(0)},0}; //time error in METERS (time x speed) + arma::vec pos = {d_rx_pos(0), d_rx_pos(0), d_rx_pos(0), 0}; // time error in METERS (time x speed) arma::mat A; arma::mat omc; arma::mat az; @@ -251,7 +253,9 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs { //receiver is above the troposphere trop = 0.0; - }else{ + } + else + { //--- Find delay due to troposphere (in meters) Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); if(trop > 5.0 ) trop = 0.0; //check for erratic values @@ -284,7 +288,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs try { //-- compute the Dilution Of Precision values - d_Q = arma::inv(arma::htrans(A)*A); + d_Q = arma::inv(arma::htrans(A) * A); } catch(std::exception& e) { diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index a3913dc1c..69ebd6e6d 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -57,7 +57,7 @@ Pvt_Solution::Pvt_Solution() b_valid_position = false; d_averaging_depth = 0; d_valid_observations = 0; - d_rx_pos=arma::zeros(3,1); + d_rx_pos = arma::zeros(3,1); d_rx_dt_s = 0.0; } @@ -147,24 +147,24 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) { /* Subroutine to calculate geodetic coordinates latitude, longitude, - height given Cartesian coordinates X,Y,Z, and reference ellipsoid - values semi-major axis (a) and the inverse of flattening (finv). + height given Cartesian coordinates X,Y,Z, and reference ellipsoid + values semi-major axis (a) and the inverse of flattening (finv). - The output units of angular quantities will be in decimal degrees - (15.5 degrees not 15 deg 30 min). The output units of h will be the - same as the units of X,Y,Z,a. + The output units of angular quantities will be in decimal degrees + (15.5 degrees not 15 deg 30 min). The output units of h will be the + same as the units of X,Y,Z,a. - Inputs: + Inputs: a - semi-major axis of the reference ellipsoid finv - inverse of flattening of the reference ellipsoid X,Y,Z - Cartesian coordinates - Outputs: + Outputs: dphi - latitude dlambda - longitude h - height above reference ellipsoid - Based in a Matlab function by Kai Borre + Based in a Matlab function by Kai Borre */ *h = 0; 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 f1ce131b0..d10af4ddb 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 @@ -80,11 +80,11 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool { 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); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str() << std::endl; + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); } catch (const std::ifstream::failure & e) { - LOG(WARNING) << "Exception opening observables dump file " << e.what() << std::endl; + LOG(WARNING) << "Exception opening observables dump file " << e.what(); } } } @@ -185,7 +185,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_d_TOW_at_current_symbol); double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol; double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms; - //int reference_channel= gnss_synchro_iter->second.Channel_ID; // Now compute RX time differences due to the PRN alignment in the correlators double traveltime_ms; @@ -202,14 +201,14 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni // compute the required symbol history shift in order to match the reference symbol delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms; //compute the pseudorange - traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol) * 1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms; + traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_at_current_symbol) * 1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms; //convert to meters and remove the receiver time offset in meters pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] // update the pseudorange object current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GPS_STARTOFFSET_ms/1000.0; + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference * 1000.0) / 1000.0 + GPS_STARTOFFSET_ms / 1000.0; if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) { @@ -221,7 +220,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0; // arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz); // arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads); - // Curve fitting to cuadratic function + // Curve fitting to quadratic function arma::mat A = arma::ones (history_deep, 2); A.col(1) = symbol_TOW_vec_s; @@ -237,7 +236,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0]; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0]; } - } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 52e4844f1..aa7322281 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -110,8 +110,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_channel = 0; Prn_timestamp_at_preamble_ms = 0.0; flag_PLL_180_deg_phase_locked = false; - - tmp_counter=0; } @@ -198,7 +196,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs=" << round(in[0][0].Tracking_timestamp_secs * 1000.0); d_GPS_FSM.Event_gps_word_preamble(); d_flag_preamble = true; - d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs;// - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble + d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the PRN start sample index associated to the preamble if (!d_flag_frame_sync) { @@ -330,38 +328,29 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ } } // output the frame - consume_each(1); //one by one - Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block - //1. Copy the current tracking output - current_synchro_data = in[0][0]; - //2. Add the telemetry decoder information - if (this->d_flag_preamble == true and d_GPS_FSM.d_nav.d_TOW > 0) - //update TOW at the preamble instant (todo: check for valid d_TOW) - // JAVI: 30/06/2014 - // TOW, in GPS, is referred to the START of the SUBFRAME, that is, THE FIRST SYMBOL OF THAT SUBFRAME, NOT THE PREAMBLE. - // thus, no correction should be done. d_TOW_at_Preamble should be renamed to d_TOW_at_subframe_start. - // Sice we detected the preable, then, we are in the last symbol of that preamble, or just at the start of the first subframe symbol. - { - d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW; - Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; - //std::cout.precision(17); - //std::cout<<"symbol diff="<d_flag_preamble == true and d_GPS_FSM.d_nav.d_TOW > 0) + { + // update TOW at the preamble instant + d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW; + Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; + d_TOW_at_current_symbol = d_TOW_at_Preamble; + if (flag_TOW_set == false) + { + flag_TOW_set = true; + } + } + else + { + d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD; + } current_synchro_data.d_TOW = d_TOW_at_Preamble; current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; @@ -373,7 +362,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ if (flag_PLL_180_deg_phase_locked == true) { - //correct the accumulated phase for the costas loop phase shift, if required + //correct the accumulated phase for the Costas loop phase shift, if required current_synchro_data.Carrier_phase_rads += GPS_PI; } @@ -419,6 +408,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ d_decimation_output_factor = decimation; } + void gps_l1_ca_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite) { d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); @@ -446,7 +436,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ 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); LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel - << " Log file: " << d_dump_filename.c_str(); + << " Log file: " << d_dump_filename.c_str(); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index 8f3bd5386..495b5b252 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -98,9 +98,6 @@ private: double d_symbol_accumulator; short int d_symbol_accumulator_counter; - //debug - int tmp_counter; - //bits and frame unsigned short int d_frame_bit_index; unsigned int d_GPS_frame_4bytes; 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 74aab4f09..fb50e9563 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 @@ -193,10 +193,10 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() long int acq_trk_diff_samples; double acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); - //doppler effect + // Doppler effect // Fd=(C/(C+Vr))*F double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ; // new chip and prn sequence periods based on acq Doppler @@ -309,14 +309,14 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples d_pull_in = false; - //take into account the carrier cycles accumulated in the pull in signal alignement + // take into account the carrier cycles accumulated in the pull in signal alignment d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; *out[0] = current_synchro_data; - consume_each(samples_offset); //shift input to perform alignment with local replica + consume_each(samples_offset); // shift input to perform alignment with local replica return 1; } @@ -332,7 +332,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ // ################## PLL ########################################################## // PLL discriminator // Update PLL discriminator [rads/Ti -> Secs/Ti] - carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; //prompt output + carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output // Carrier discriminator filter carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); // New carrier Doppler frequency estimation @@ -342,40 +342,34 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ // ################## DLL ########################################################## // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] //early and late + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] - //Code phase accumulator - double code_error_filt_secs; - code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds] + code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] + double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; // [seconds] // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - double T_chip_seconds; - double T_prn_seconds; - double T_prn_samples; - double K_blk_samples; // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / static_cast(d_code_freq_chips); - T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples + double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples //################### PLL COMMANDS ################################################# - //carrier phase step (NCO phase increment per sample) [rads/sample] + // carrier phase step (NCO phase increment per sample) [rads/sample] d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - //remanent carrier phase to prevent overflow in the code NCO + // remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); - //carrier phase accumulator for (K) doppler estimation + // carrier phase accumulator d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; //################### DLL COMMANDS ################################################# - //code phase step (Code resampler phase increment per sample) [chips/sample] + // code phase step (Code resampler phase increment per sample) [chips/sample] d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - //remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### @@ -405,7 +399,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } @@ -415,7 +409,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter+d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter + d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; @@ -430,7 +424,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ d_correlator_outs[n] = gr_complex(0,0); } - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter+d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter + d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); current_synchro_data.System = {'G'}; } @@ -460,7 +454,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - tmp_long = d_sample_counter+d_current_prn_length_samples; + tmp_long = d_sample_counter + d_current_prn_length_samples; d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); // accumulated carrier phase d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); @@ -469,11 +463,11 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - //PLL commands + // PLL commands d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - //DLL commands + // DLL commands d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); @@ -494,9 +488,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ } consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates - d_sample_counter += d_current_prn_length_samples; //count for the processed samples + d_sample_counter += d_current_prn_length_samples; // count for the processed samples - 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 } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h index b75db865e..92a5d4afb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h @@ -139,7 +139,6 @@ private: double d_carrier_phase_step_rad; double d_acc_carrier_phase_rad; double d_code_phase_samples; - double d_acc_code_phase_secs; //PRN period in samples int d_current_prn_length_samples; diff --git a/src/tests/system-tests/obs_gps_l1_system_test.cc b/src/tests/system-tests/obs_gps_l1_system_test.cc index 06c606320..c2e95de80 100644 --- a/src/tests/system-tests/obs_gps_l1_system_test.cc +++ b/src/tests/system-tests/obs_gps_l1_system_test.cc @@ -29,7 +29,7 @@ * ------------------------------------------------------------------------- */ - +#include #include #include #include @@ -46,16 +46,15 @@ #include "Rinex3ObsData.hpp" #include "Rinex3ObsHeader.hpp" #include "Rinex3ObsStream.hpp" -#include "control_thread.h" #include "concurrent_map.h" #include "concurrent_queue.h" +#include "control_thread.h" #include "in_memory_configuration.h" - DEFINE_string(generator_binary, std::string(SW_GENERATOR_BIN), "Path of software-defined signal generator binary"); DEFINE_string(rinex_nav_file, std::string(DEFAULT_RINEX_NAV), "Input RINEX navigation file"); -DEFINE_int32(duration, 100, "Duration of the experiment [in seconds]"); +DEFINE_int32(duration, 100, "Duration of the experiment [in seconds, max = 300]"); DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver position [log,lat,height]"); DEFINE_string(dynamic_position, "", "Observer positions file, in .csv or .nmea format"); DEFINE_string(filename_rinex_obs, "sim.16o", "Filename of output RINEX navigation file"); @@ -75,7 +74,7 @@ public: std::string p4; std::string p5; - const int baseband_sampling_freq = 2.6e6; + const double baseband_sampling_freq = 2.6e6; std::string filename_rinex_obs = FLAGS_filename_rinex_obs; std::string filename_raw_data = FLAGS_filename_raw_data; @@ -131,7 +130,8 @@ int Obs_Gps_L1_System_Test::configure_generator() p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file; if(FLAGS_dynamic_position.empty()) { - p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(FLAGS_duration * 10); + p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(std::min(FLAGS_duration * 10, 3000)); + if(FLAGS_duration > 300) std::cout << "WARNING: Duration has been set to its maximum value of 300 s" << std::endl; } else { @@ -208,6 +208,9 @@ int Obs_Gps_L1_System_Test::configure_receiver() const float pll_bw_hz = 30.0; const float dll_bw_hz = 4.0; const float early_late_space_chips = 0.5; + const float pll_bw_narrow_hz = 20.0; + const float dll_bw_narrow_hz = 2.0; + const int extend_correlation_ms = 1; const int display_rate_ms = 500; const int output_rate_ms = 1000; @@ -287,6 +290,7 @@ int Obs_Gps_L1_System_Test::configure_receiver() // Set Tracking config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking"); + //config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking"); config->set_property("Tracking_1C.item_type", "gr_complex"); config->set_property("Tracking_1C.if", std::to_string(zero)); config->set_property("Tracking_1C.dump", "false"); @@ -295,6 +299,10 @@ int Obs_Gps_L1_System_Test::configure_receiver() config->set_property("Tracking_1C.dll_bw_hz", std::to_string(dll_bw_hz)); config->set_property("Tracking_1C.early_late_space_chips", std::to_string(early_late_space_chips)); + config->set_property("Tracking_1C.pll_bw_narrow_hz", std::to_string(pll_bw_narrow_hz)); + config->set_property("Tracking_1C.dll_bw_narrow_hz", std::to_string(dll_bw_narrow_hz)); + config->set_property("Tracking_1C.extend_correlation_ms", std::to_string(extend_correlation_ms)); + // Set Telemetry config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); config->set_property("TelemetryDecoder_1C.dump", "false"); @@ -304,6 +312,7 @@ int Obs_Gps_L1_System_Test::configure_receiver() config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); config->set_property("Observables.dump", "false"); config->set_property("Observables.dump_filename", "./observables.dat"); + config->set_property("Observables.averaging_depth", std::to_string(100)); // Set PVT config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); @@ -344,12 +353,13 @@ int Obs_Gps_L1_System_Test::run_receiver() } // Get the name of the RINEX obs file generated by the receiver FILE *fp; - std::string argum2 = std::string("/bin/ls *O | tail -1"); + std::string argum2 = std::string("/bin/ls *O | grep GSDR | tail -1"); char buffer[1035]; fp = popen(&argum2[0], "r"); if (fp == NULL) { std::cout << "Failed to run command: " << argum2 << std::endl; + return -1; } char * without_trailing; while (fgets(buffer, sizeof(buffer), fp) != NULL) @@ -519,10 +529,11 @@ void Obs_Gps_L1_System_Test::check_results() // If a measure exists for this sow, store it for(it2 = pseudorange_meas.at(prn_id).begin(); it2 != pseudorange_meas.at(prn_id).end(); it2++) { - if(std::abs(it->first - it2->first) < 0.001) // store measures closer than 1 ms. + if(std::abs(it->first - it2->first) < 0.01) // store measures closer than 10 ms. { pseudorange_ref_aligned.at(prn_id).push_back(*it); pr_diff.at(prn_id).push_back(it->second - it2->second ); + //std::cout << "Sat " << prn_id << ": " << "PR_ref=" << it->second << " PR_meas=" << it2->second << " Diff:" << it->second - it2->second << std::endl; } } } @@ -537,7 +548,7 @@ void Obs_Gps_L1_System_Test::check_results() // If a measure exists for this sow, store it for(it2 = carrierphase_meas.at(prn_id).begin(); it2 != carrierphase_meas.at(prn_id).end(); it2++) { - if(std::abs(it->first - it2->first) < 0.001) // store measures closer than 1 ms. + if(std::abs(it->first - it2->first) < 0.01) // store measures closer than 10 ms. { carrierphase_ref_aligned.at(prn_id).push_back(*it); cp_diff.at(prn_id).push_back(it->second - it2->second ); @@ -555,7 +566,7 @@ void Obs_Gps_L1_System_Test::check_results() // If a measure exists for this sow, store it for(it2 = doppler_meas.at(prn_id).begin(); it2 != doppler_meas.at(prn_id).end(); it2++) { - if(std::abs(it->first - it2->first) < 0.001) // store measures closer than 1 ms. + if(std::abs(it->first - it2->first) < 0.01) // store measures closer than 10 ms. { doppler_ref_aligned.at(prn_id).push_back(*it); doppler_diff.at(prn_id).push_back(it->second - it2->second ); @@ -582,7 +593,10 @@ void Obs_Gps_L1_System_Test::check_results() { mean_diff = mean_diff / number_obs; mean_pr_diff_v.push_back(mean_diff); - std::cout << "-- Mean pseudorange difference for sat " << prn_id << ": " << mean_diff << " [m]" << std::endl; + std::cout << "-- Mean pseudorange difference for sat " << prn_id << ": " << mean_diff; + double stdev_ = compute_stdev(*iter_diff); + std::cout << " +/- " << stdev_ ; + std::cout << " [m]" << std::endl; } else { @@ -662,7 +676,7 @@ TEST_F(Obs_Gps_L1_System_Test, Observables_system_test) { std::cout << "Validating input RINEX nav file: " << FLAGS_rinex_nav_file << " ..." << std::endl; bool is_rinex_nav_valid = check_valid_rinex_nav(FLAGS_rinex_nav_file); - ASSERT_EQ(true, is_rinex_nav_valid); + EXPECT_EQ(true, is_rinex_nav_valid) << "The RINEX navigation file " << FLAGS_rinex_nav_file << " is not well formed."; std::cout << "The file is valid." << std::endl; // Configure the signal generator @@ -673,18 +687,18 @@ TEST_F(Obs_Gps_L1_System_Test, Observables_system_test) std::cout << "Validating generated reference RINEX obs file: " << FLAGS_filename_rinex_obs << " ..." << std::endl; bool is_gen_rinex_obs_valid = check_valid_rinex_obs( "./" + FLAGS_filename_rinex_obs); - ASSERT_EQ(true, is_gen_rinex_obs_valid); + EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << FLAGS_filename_rinex_obs << ", generated by gnss-sim, is not well formed."; std::cout << "The file is valid." << std::endl; // Configure receiver configure_receiver(); // Run the receiver - run_receiver(); + EXPECT_EQ( run_receiver(), 0) << "Problem executing the software-defined signal generator"; std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << generated_rinex_obs << " ..." << std::endl; is_gen_rinex_obs_valid = check_valid_rinex_obs( "./" + generated_rinex_obs); - ASSERT_EQ(true, is_gen_rinex_obs_valid); + EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << generated_rinex_obs << ", generated by GNSS-SDR, is not well formed."; std::cout << "The file is valid." << std::endl; // Check results