[hamradio-commits] [gnss-sdr] 245/303: Code cleaning
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Mon Feb 13 22:36:05 UTC 2017
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch master
in repository gnss-sdr.
commit 4438ffe9169226178392b2383a2c862309cbceb7
Author: Carles Fernandez <carles.fernandez at gmail.com>
Date: Sat Jan 28 15:31:04 2017 +0100
Code cleaning
---
.../PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc | 33 ++-
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc | 66 +++---
src/algorithms/PVT/libs/ls_pvt.cc | 244 +++++++++++----------
src/algorithms/PVT/libs/pvt_solution.cc | 18 +-
.../gnuradio_blocks/gps_l1_ca_observables_cc.cc | 12 +-
.../gps_l1_ca_telemetry_decoder_cc.cc | 62 +++---
.../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 -
src/tests/system-tests/obs_gps_l1_system_test.cc | 44 ++--
10 files changed, 268 insertions(+), 277 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 8c774fa..dddb3ee 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<Kml_Printer>();
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<GeoJSON_Printer>();
d_geojson_printer->set_headers(geojson_dump_filename);
- //initialize nmea_printer
+ // initialize nmea_printer
d_nmea_printer = std::make_shared<Nmea_Printer>(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<int,Gnss_Synchro>::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_ls_pvt->d_rx_dt_s*GPS_C_m_s<<std::endl;
- if( first_fix == true)
+ // correct the observable to account for the receiver clock offset
+ for (std::map<int,Gnss_Synchro>::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 4f5cc12..1e73144 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<int,Gnss_Synchro> gnss_pseudoranges_map,
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
std::map<int,Gps_Ephemeris>::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<int,Gnss_Synchro> 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<int,Gnss_Synchro> 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<int,Gnss_Synchro> 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]
- }
-
- //Execute WLS using previos position as the initialization point
+ // 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 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="<<d_rx_dt_s<<" clock error for this iteration="<<rx_position_and_time(3)/GPS_C_m_s<<" [s]"<<std::endl;
+ DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(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<double>(GPS_week));
@@ -188,13 +191,12 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> 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 a4b315f..0e9c79f 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<m;i++)
- {
- int x = B(i,0);
- int y = B(i,1);
- if (iter == 0)
- {
- traveltime = 0.072;
- }
- else
- {
- int z = B(i,2);
- double rho = (x-pos(0))*(x-pos(0))+(y-pos(1))*(y-pos(1))+(z-pos(2))*(z-pos(2));
- traveltime = sqrt(rho)/GPS_C_m_s;
- }
- double angle = traveltime*7.292115147e-5;
- double cosa = cos(angle);
- double sina = sin(angle);
- B(i,0) = cosa*x + sina*y;
- B(i,1) = -sina*x + cosa*y;
- }// % i-loop
+ double traveltime = 0;
+ for (int iter = 0; iter < 2; iter++)
+ {
+ B = B_pass;
+ int m = arma::size(B,0);
+ for (int i = 0; i < m; i++)
+ {
+ int x = B(i,0);
+ int y = B(i,1);
+ if (iter == 0)
+ {
+ traveltime = 0.072;
+ }
+ else
+ {
+ int z = B(i,2);
+ double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2));
+ traveltime = sqrt(rho) / GPS_C_m_s;
+ }
+ double angle = traveltime * 7.292115147e-5;
+ double cosa = cos(angle);
+ double sina = sin(angle);
+ B(i,0) = cosa * x + sina * y;
+ B(i,1) = -sina * x + cosa * y;
+ }// % i-loop
- if (m > 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);
- }
+ if (m > 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<m; j++)
- {
- for (int i =0;i<2;i++)
- {
- double c_dt = possible_pos(3,i);
- double calc = arma::norm(satpos.row(i).t() -possible_pos.col(i).rows(0,2))+c_dt;
- double omc = obs(j)-calc;
- abs_omc(i) = std::abs(omc);
- }
- }// % j-loop
+ arma::vec abs_omc = arma::zeros(2,1);
+ for (int j = 0; j < m; j++)
+ {
+ for (int i = 0; i < 2; i++)
+ {
+ double c_dt = possible_pos(3,i);
+ double calc = arma::norm(satpos.row(i).t() - possible_pos.col(i).rows(0,2)) + c_dt;
+ double omc = obs(j) - calc;
+ abs_omc(i) = std::abs(omc);
+ }
+ }// % j-loop
- //discrimination between roots
- if (abs_omc(0) > 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 a3913dc..69ebd6e 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 f1ce131..d10af4d 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<arma::mat> (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 52e4844..aa73222 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="<<tmp_counter<<" Preable TOW="<<std::fixed<<d_TOW_at_Preamble
- // <<" with DeltaTOW="<<d_TOW_at_Preamble-d_TOW_at_current_symbol
- // <<" decoded at "<<Prn_timestamp_at_preamble_ms/1000<<"[s]\r\n";
-
- d_TOW_at_current_symbol = d_TOW_at_Preamble;
- if (flag_TOW_set == false)
- {
- flag_TOW_set = true;
- }
+ consume_each(1); // one by one
- tmp_counter=0;
- }
- else
- {
- tmp_counter++;
- d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD;
- }
+ 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
+ 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 8f3bd53..495b5b2 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 74aab4f..fb50e95 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<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
+ acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(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<float>(acq_trk_diff_samples) / static_cast<float>(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<float>(acq_to_trk_delay_samples), static_cast<float>(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<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(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<double>(d_code_freq_chips);
- T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
- T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
- K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(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<double>(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<double>(d_fs_in);
+ double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(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<double>(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<double>(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<double>(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<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample
- current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter+d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
+ current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter + d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(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<double>(d_sample_counter+d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
+ current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter + d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast<double>(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<char*>(&prompt_I), sizeof(float));
d_dump_file.write(reinterpret_cast<char*>(&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<char*>(&tmp_long), sizeof(unsigned long int));
// accumulated carrier phase
d_dump_file.write(reinterpret_cast<char*>(&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<char*>(&d_carrier_doppler_hz), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
- //PLL commands
+ // PLL commands
d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
- //DLL commands
+ // DLL commands
d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&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 b75db86..92a5d4a 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 06c6063..c2e95de 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 <algorithm>
#include <exception>
#include <iostream>
#include <cstring>
@@ -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
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list