mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Code cleaning

This commit is contained in:
Carles Fernandez 2017-01-28 15:31:04 +01:00
parent d2c7bb62a1
commit 4438ffe916
10 changed files with 266 additions and 275 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
View File

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

64
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@ -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]
}
// 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="<<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)
{

244
src/algorithms/PVT/libs/ls_pvt.cc
View File

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

18
src/algorithms/PVT/libs/pvt_solution.cc
View File

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

12
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

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

60
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -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";
consume_each(1); // one by one
d_TOW_at_current_symbol = d_TOW_at_Preamble;
if (flag_TOW_set == false)
{
flag_TOW_set = true;
}
Gnss_Synchro current_synchro_data; // structure to save the synchronization information and send the output object to the next block
tmp_counter=0;
}
else
{
tmp_counter++;
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD;
}
//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)
{

3
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

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

62
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

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

1
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
View File

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

44
src/tests/system-tests/obs_gps_l1_system_test.cc
View File

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