mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-04 01:37:03 +00:00
Code Issues Releases Wiki Activity

Merging gnss-sdr/rinex_fix branch

Browse Source 
This commit rewrites the way pseudoranges are computed, now accounting for the receiver clock offset. It also adds more work in the QA code. If extra tests are activated by -DENABLE_UNIT_TESTING_EXTRA=ON or -DENABLE_SYSTEM_TESTING_EXTRA=ON, additional raw data files, a software-defined signal generator and GPSTk 2.9 are downloaded. Many fixes and code refactoring in tracking blocks.
This commit is contained in:
Carles Fernandez 2017-02-03 14:03:15 +01:00
commit 0362476864
50 changed files with 2836 additions and 861 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CMakeLists.txt
View File

@ -61,6 +61,7 @@ option(ENABLE_CUDA "Enable building of processing blocks implemented with CUDA (
option(ENABLE_GENERIC_ARCH "Builds a portable binary" OFF)
option(ENABLE_PACKAGING "Enable software packaging" OFF)
option(ENABLE_OWN_GLOG "Download glog and link it to gflags" OFF)
option(ENABLE_OWN_ARMADILLO "Download and build Armadillo locally" OFF)
option(ENABLE_LOG "Enable logging" ON)
if(ENABLE_PACKAGING)
set(ENABLE_GENERIC_ARCH ON)
@ -68,6 +69,7 @@ endif(ENABLE_PACKAGING)
# Testing
option(ENABLE_UNIT_TESTING "Build unit tests" ON)
option(ENABLE_UNIT_TESTING_EXTRA "Download external files and build extra unit tests" OFF)
option(ENABLE_SYSTEM_TESTING "Build system tests" OFF)
option(ENABLE_SYSTEM_TESTING_EXTRA "Download external tools and build extra system tests" OFF)
if(ENABLE_SYSTEM_TESTING_EXTRA)
@ -874,7 +876,7 @@ if(OS_IS_LINUX)
endif(OS_IS_LINUX)
find_package(Armadillo)
if(NOT ARMADILLO_FOUND)
if(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO)
message(STATUS " Armadillo has not been found.")
message(STATUS " Armadillo will be downloaded and built automatically ")
message(STATUS " when doing 'make'. ")
@ -923,11 +925,12 @@ if(NOT ARMADILLO_FOUND)
file(COPY ${CMAKE_CURRENT_BINARY_DIR}/armadillo-${armadillo_RELEASE}
DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/armadillo
)
else(NOT ARMADILLO_FOUND)
set(ARMADILLO_VERSION_STRING ${armadillo_RELEASE})
else(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO)
set(armadillo_RELEASE ${ARMADILLO_VERSION_STRING})
add_library(armadillo-${armadillo_RELEASE} UNKNOWN IMPORTED)
set_property(TARGET armadillo-${armadillo_RELEASE} PROPERTY IMPORTED_LOCATION "${ARMADILLO_LIBRARIES}")
endif(NOT ARMADILLO_FOUND)
endif(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO)

5
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
View File

@ -287,6 +287,11 @@ int galileo_e1_pvt_cc::general_work (int noutput_items __attribute__((unused)),
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;
}
if( first_fix == true)
{
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)

22
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,14 +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)
{
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
@ -399,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)

16
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
View File

@ -455,6 +455,22 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
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;
}
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
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
ttff_msgbuf ttff;
ttff.mtype = 1;
ttff.ttff = d_sample_counter;
send_sys_v_ttff_msg(ttff);
first_fix = false;
}
d_kml_dump->print_position(d_ls_pvt, d_flag_averaging);
d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging);
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);

270
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
View File

@ -48,21 +48,21 @@ galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, b
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
if (d_dump_file.is_open() == false)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
}
}
try
{
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) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
}
}
}
}
@ -78,11 +78,10 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
{
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
int valid_pseudoranges = gnss_pseudoranges_map.size();
arma::mat 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 Galileo_week_number = 0;
double utc = 0.0;
@ -96,60 +95,62 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// ********************************************************************************
int valid_obs = 0; //valid observations counter
int obs_counter = 0;
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
{
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
{
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(obs_counter, obs_counter) = 1.0;
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM
double Rx_time = galileo_current_time;
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GALILEO_C_m_s;
// COMMON RX TIME PVT ALGORITHM
double Rx_time = galileo_current_time;
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GALILEO_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X;
satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z;
//store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected pseudoranges
obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s;
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++;
// 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 * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first)->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(obs_counter) << " [m]";
}
else // the ephemeris are not available for this SV
{
// no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first;
}
obs_counter++;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first)->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++;
}
else // the ephemeris are not available for this SV
{
DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first;
}
}
// ********************************************************************************
// ****** SOLVE LEAST SQUARES******************************************************
@ -158,78 +159,91 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
LOG(INFO) << "Galileo PVT: valid observations=" << valid_obs;
if (valid_obs >= 4)
{
arma::vec rx_position_and_time;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs;
DLOG(INFO) << "W=" << W;
// check if this is the initial position computation
if (d_rx_dt_s == 0)
{
arma::vec mypos;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs;
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
// Compute Gregorian time
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
// get time string Gregorian calendar
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos;
cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
d_rx_dt_m = mypos(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
DLOG(INFO) << "Galileo 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_m << " [s]";
// ###### Compute DOPs ########
compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
tmp_double = galileo_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double = mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double = mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double = mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
}
}
// MOVING AVERAGE PVT
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
// 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) / GALILEO_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) / GALILEO_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
// Compute Gregorian time
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
// get time string Gregorian calendar
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << rx_position_and_time;
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);
d_rx_dt_s = rx_position_and_time(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds
DLOG(INFO) << "Galileo 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]";
// ###### Compute DOPs ########
compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
tmp_double = galileo_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = rx_position_and_time(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double = rx_position_and_time(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double = rx_position_and_time(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double = rx_position_and_time(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what();
}
}
// MOVING AVERAGE PVT
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
}
else
{
b_valid_position = false;
}
{
b_valid_position = false;
}
return b_valid_position;
}

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

@ -80,16 +80,15 @@ 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;
int valid_pseudoranges = gnss_pseudoranges_map.size();
arma::mat 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;
@ -97,7 +96,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// ********************************************************************************
int valid_obs = 0; //valid observations counter
int obs_counter = 0;
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
@ -109,49 +107,49 @@ 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(obs_counter, obs_counter) = 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
double Rx_time = GPS_current_time;
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
// 3- compute the current ECEF position for this SV using corrected TX time
// 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos(0, obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1, obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2, obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
//store satellite positions in a matrix
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(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s;
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + dtr * 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(obs_counter) << " [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]";
valid_obs++;
// compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
}
else // the ephemeris are not available for this SV
{
// no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
}
obs_counter++;
}
// ********************************************************************************
@ -162,17 +160,31 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
if (valid_obs >= 4)
{
arma::vec mypos;
arma::vec rx_position_and_time;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos;
// 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]
}
cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
// Execute WLS using previous position as the initialization point
rx_position_and_time = leastSquarePos(satpos, obs, W);
d_rx_dt_m = mypos(3)/GPS_C_m_s; // Convert RX time offset from meters to 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]";
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)
@ -180,10 +192,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// 22 August 1999 last GPS time roll over
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_m << " [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 ########
compute_DOP();
@ -199,16 +210,16 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
tmp_double = GPS_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = mypos(0);
tmp_double = d_rx_pos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double = mypos(1);
tmp_double = d_rx_pos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double = mypos(2);
tmp_double = d_rx_pos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double = mypos(3);
tmp_double = d_rx_dt_s;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;

509
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -47,21 +47,21 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
d_flag_averaging = false;
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
if (d_dump_file.is_open() == false)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
}
}
try
{
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) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
}
}
}
}
@ -78,11 +78,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
std::map<int,Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
int valid_observables = gnss_observables_map.size();
arma::mat W = arma::eye(valid_observables, valid_observables); // channels weights matrix
arma::vec obs = arma::zeros(valid_observables); // observables observation vector
arma::mat satpos = arma::zeros(3, valid_observables); // satellite positions matrix
arma::vec W; // channels weight vector
arma::vec obs; // pseudoranges observation vector
arma::mat satpos; // satellite positions matrix
int Galileo_week_number = 0;
int GPS_week = 0;
@ -98,168 +96,184 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// ********************************************************************************
int valid_obs = 0; //valid observations counter
int obs_counter = 0;
for(gnss_observables_iter = gnss_observables_map.begin();
gnss_observables_iter != gnss_observables_map.end();
gnss_observables_iter++)
{
switch(gnss_observables_iter->second.System)
{
if(gnss_observables_iter->second.System == 'E')
{
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(obs_counter, obs_counter) = 1;
case 'E':
{
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s;
// COMMON RX TIME PVT ALGORITHM
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos(0,obs_counter) = galileo_ephemeris_iter->second.d_satpos_X;
satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z;
//store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
// 5- fill the observations vector with the corrected observables
obs(obs_counter) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s;
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
valid_obs++;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(obs_counter) << " [m]";
}
else // the ephemeris are not available for this SV
{
// no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
}
else if(gnss_observables_iter->second.System == 'G')
{
//std::cout << "Satellite System: " << gnss_observables_iter->second.System <<std::endl;
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
std::string sig_(gnss_observables_iter->second.Signal);
if(sig_.compare("1C") == 0)
{
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(obs_counter, obs_counter) = 1;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time);
valid_obs++;
}
// 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(0, obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1, obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2, obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
// 5- fill the observations vector with the corrected observables
obs(obs_counter) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_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_observables_iter->second.CN0_dB_hz;
valid_obs++;
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
// 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(obs_counter) << " [m]";
}
else // the ephemeris are not available for this SV
{
// no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
}
if(sig_.compare("2S") == 0)
{
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(obs_counter, obs_counter) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos(0, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_X;
satpos(1, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
satpos(2, obs_counter) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
// 5- fill the observations vector with the corrected observables
obs(obs_counter) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
valid_obs++;
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
<< " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(obs_counter) << " [m]";
}
else // the ephemeris are not available for this SV
{
// no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
}
}
obs_counter++;
else // the ephemeris are not available for this SV
{
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
break;
}
case 'G':
{
// 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key
std::string sig_(gnss_observables_iter->second.Signal);
if(sig_.compare("1C") == 0)
{
gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_ephemeris_iter != gps_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
// 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix
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_observables_iter->second.Pseudorange_m + dtr * 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_observables_iter->second.CN0_dB_hz;
// 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]";
valid_obs++;
// compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
}
else // the ephemeris are not available for this SV
{
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
}
}
if(sig_.compare("2S") == 0)
{
gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W.resize(valid_obs + 1, 1);
W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X;
satpos(1, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Y;
satpos(2, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_Z;
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
<< " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
valid_obs++;
}
else // the ephemeris are not available for this SV
{
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
}
}
break;
}
default :
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
break;
}
}
// ********************************************************************************
// ****** SOLVE LEAST SQUARES******************************************************
@ -269,86 +283,101 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
if(valid_obs >= 4)
{
arma::vec rx_position_and_time;
DLOG(INFO) << "satpos=" << satpos;
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
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]
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_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]";
double secondsperweek = 604800.0;
// Compute GST and Gregorian time
if( GST != 0.0)
{
arma::vec mypos;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
d_rx_dt_m = mypos(3) / GPS_C_m_s; // Convert RX time offset from meters to seconds
double secondsperweek = 604800.0;
// Compute GST and Gregorian time
if( GST != 0.0)
{
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
}
else
{
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
}
// get time string Gregorian calendar
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
DLOG(INFO) << "HYBRID Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z) = " << mypos;
cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
DLOG(INFO) << "Hybrid 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_m << " [s]";
// ###### Compute DOPs ########
hybrid_ls_pvt::compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
tmp_double = hybrid_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double = mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double = mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double = mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
}
}
// MOVING AVERAGE PVT
pos_averaging(flag_averaging);
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
}
else
{
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week) + secondsperweek * static_cast<double>(GPS_week);
}
// get time string Gregorian calendar
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
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);
DLOG(INFO) << "Hybrid 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]";
// ###### Compute DOPs ########
hybrid_ls_pvt::compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
tmp_double = hybrid_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = rx_position_and_time(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double = rx_position_and_time(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double = rx_position_and_time(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double = rx_position_and_time(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
}
}
// MOVING AVERAGE PVT
pos_averaging(flag_averaging);
}
else
{
b_valid_position = false;
}
{
b_valid_position = false;
}
return b_valid_position;
}

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

@ -41,12 +41,141 @@ using google::LogMessage;
Ls_Pvt::Ls_Pvt() : Pvt_Solution()
{
d_x_m = 0.0;
d_y_m = 0.0;
d_z_m = 0.0;
}
arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w)
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;
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
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
//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
// 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.
* Inputs:
@ -63,7 +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::vec pos = "0.0 0.0 0.0 0.0";
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::mat A;
arma::mat omc;
arma::mat az;
@ -155,11 +287,14 @@ 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)
{
d_Q = arma::zeros(4,4);
}
return pos;
}

19
src/algorithms/PVT/libs/ls_pvt.h
View File

@ -41,13 +41,24 @@
*/
class Ls_Pvt : public Pvt_Solution
{
private:
/*!
* \brief Computes the Lorentz inner product between two vectors
*/
double lorentz(const arma::vec & x,const arma::vec & y);
public:
Ls_Pvt();
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w);
double d_x_m;
double d_y_m;
double d_z_m;
/*!
* \brief Computes the initial position solution based on the Bancroft algorithm
*/
arma::vec bancroftPos(const arma::mat & satpos, const arma::vec & obs);
/*!
* \brief Computes the Weighted Least Squares position solution
*/
arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec);
};
#endif

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

@ -57,7 +57,8 @@ Pvt_Solution::Pvt_Solution()
b_valid_position = false;
d_averaging_depth = 0;
d_valid_observations = 0;
d_rx_dt_m = 0.0;
d_rx_pos = arma::zeros(3,1);
d_rx_dt_s = 0.0;
}
arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec & X_sat)
@ -146,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;

4
src/algorithms/PVT/libs/pvt_solution.h
View File

@ -51,7 +51,9 @@ public:
double d_latitude_d; //!< RX position Latitude WGS84 [deg]
double d_longitude_d; //!< RX position Longitude WGS84 [deg]
double d_height_m; //!< RX position height WGS84 [m]
double d_rx_dt_m; //!< RX time offset [s]
arma::vec d_rx_pos;
double d_rx_dt_s; //!< RX time offset [s]
boost::posix_time::ptime d_position_UTC_time;

2
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md
View File

@ -57,7 +57,7 @@ This figure shows the role of some VOLK_GNSSSDR kernels in the context of a GNSS
If you use VOLK_GNSSSDR in your research and/or software, please cite the following paper:
* C. Fern&aacute;ndez-Prades, J. Arribas, P. Closas, *Accelerating GNSS Software Receivers*, in Proc. of the ION GNSS+ 2016 Conference, pp. 44-61, Portland, Oregon, Sept. 12-16, 2016.
* C. Fern&aacute;ndez-Prades, J. Arribas, P. Closas, [*Accelerating GNSS Software Receivers*](https://zenodo.org/record/266493#.WJR8j7bhB89), in Proc. of the ION GNSS+ 2016 Conference, pp. 44-61, Portland, Oregon, Sept. 12-16, 2016.
Citations are useful for the continued development and maintenance of the library.

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

@ -57,6 +57,7 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool
gr::block("gps_l1_ca_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)))
{
// initialize internal vars
d_dump = dump;
d_nchannels = nchannels;
@ -79,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();
}
}
}
@ -96,7 +97,6 @@ gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
d_dump_file.close();
}
bool pairCompare_gnss_synchro_Prn_delay_ms(const std::pair<int,Gnss_Synchro>& a, const std::pair<int,Gnss_Synchro>& b)
{
return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
@ -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,13 +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)
{
@ -220,8 +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;
@ -232,14 +231,11 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
coef_doppler = pinv_A * dopper_vec_hz;
arma::vec acc_phase_lin;
arma::vec carrier_doppler_lin;
acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0]; // +coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0]; // +coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
//std::cout<<"acc_phase_vec_interp_rads="<<acc_phase_vec_interp_rads[0]<<std::endl;
//std::cout<<"dopper_vec_interp_hz="<<dopper_vec_interp_hz[0]<<std::endl;
acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0];
carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0];
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];
}
}
}
@ -260,10 +256,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
//tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
//tmp_double = current_gnss_synchro[i].debug_var1;
//tmp_double = current_gnss_synchro[i].debug_var2;
//d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}

9
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -196,7 +196,6 @@ int hybrid_observables_cc::general_work (int noutput_items,
DLOG(INFO) << "d_TOW_hybrid_reference [ms] = " << d_TOW_reference * 1000;
double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms;
DLOG(INFO) << "ref_PRN_rx_time_ms [ms] = " << d_ref_PRN_rx_time_ms;
//int reference_channel= gnss_synchro_iter->second.Channel_ID;
// Now compute RX time differences due to the PRN alignment in the correlators
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
@ -235,8 +234,6 @@ int hybrid_observables_cc::general_work (int noutput_items,
dopper_vec_hz = arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
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
arma::mat A = arma::ones<arma::mat> (history_deep, 2);
@ -249,10 +246,8 @@ int hybrid_observables_cc::general_work (int noutput_items,
coef_doppler = pinv_A * dopper_vec_hz;
arma::vec acc_phase_lin;
arma::vec carrier_doppler_lin;
acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0]; // +coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0]; // +coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
//std::cout<<"acc_phase_vec_interp_rads="<<acc_phase_vec_interp_rads[0]<<std::endl;
//std::cout<<"dopper_vec_interp_hz="<<dopper_vec_interp_hz[0]<<std::endl;
acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0];
carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0];
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];
}

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

@ -145,6 +145,7 @@ bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int corr_value = 0;
int preamble_diff_ms = 0;
@ -182,7 +183,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs=" << round(in[0][0].Tracking_timestamp_secs * 1000.0);
//sync the symbol to bits integrator
d_symbol_accumulator = 0; d_symbol_accumulator_counter = 0;
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_stat = 1; // enter into frame pre-detection status
}
@ -194,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)
{
@ -326,30 +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 + GPS_SUBFRAME_SECONDS; //we decoded the current TOW when the last word of the subframe arrive, so, we have a lag of ONE SUBFRAME
d_TOW_at_current_symbol = d_TOW_at_Preamble;
Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
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;
}
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
d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_L1_CA_CODE_PERIOD;
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;
@ -361,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;
}
@ -407,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());
@ -434,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)
{

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

@ -121,8 +121,8 @@ private:
double d_preamble_time_seconds;
double d_TOW_at_Preamble;
double d_TOW_at_current_symbol;
long double d_TOW_at_Preamble;
long double d_TOW_at_current_symbol;
double Prn_timestamp_at_preamble_ms;
bool flag_TOW_set;

2
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_c_aid_tracking.cc
View File

@ -111,6 +111,7 @@ GpsL1CaDllPllCAidTracking::GpsL1CaDllPllCAidTracking(
dll_bw_hz,
pll_bw_narrow_hz,
dll_bw_narrow_hz,
extend_correlation_ms,
early_late_space_chips);
DLOG(INFO) << "tracking(" << tracking_sc->unique_id() << ")";
}
@ -226,4 +227,3 @@ gr::basic_block_sptr GpsL1CaDllPllCAidTracking::get_right_block()
return nullptr;
}
}

2
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
View File

@ -396,8 +396,6 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri
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);
//compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;

1
src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
View File

@ -406,7 +406,6 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attr
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_rem_code_phase_samples)/(double)d_fs_in;
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;

3
src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
View File

@ -421,7 +421,6 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute
current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0;
*out[0] = current_synchro_data;
consume_each(samples_offset); //shift input to perform alignment with local replica
@ -629,7 +628,6 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute
// Tracking_timestamp_secs is aligned with the PRN start sample
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
@ -642,7 +640,6 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute
current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0;
}

115
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -136,9 +136,9 @@ gps_l1_ca_dll_pll_c_aid_tracking_cc::gps_l1_ca_dll_pll_c_aid_tracking_cc(
d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_extend_correlation_ms = extend_correlation_ms;
d_code_loop_filter.set_DLL_BW(d_dll_bw_hz);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz,2);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz, 2);
//--- DLL variables --------------------------------------------------------
// --- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
// Initialization of local code replica
@ -170,10 +170,7 @@ gps_l1_ca_dll_pll_c_aid_tracking_cc::gps_l1_ca_dll_pll_c_aid_tracking_cc(
// sample synchronization
d_sample_counter = 0; //(from trk to tlm)
//d_sample_counter_seconds = 0;
d_acq_sample_stamp = 0;
d_enable_tracking = false;
d_pull_in = false;
@ -205,8 +202,7 @@ gps_l1_ca_dll_pll_c_aid_tracking_cc::gps_l1_ca_dll_pll_c_aid_tracking_cc(
d_carrier_phase_step_rad = 0.0;
d_enable_extended_integration = false;
d_preamble_synchronized = false;
d_correlation_symbol_counter = 0;
d_rem_code_phase_integer_samples = 0.0;
d_rem_code_phase_integer_samples = 0;
d_code_error_chips_Ti = 0.0;
d_code_error_filt_chips_s = 0.0;
d_carr_phase_error_secs_Ti = 0.0;
@ -229,7 +225,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
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<double>(acq_trk_diff_samples) / static_cast<double>(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
@ -238,7 +234,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1/d_code_freq_chips;
T_chip_mod_seconds = 1.0 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
@ -263,7 +259,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); //The carrier loop filter implements the Doppler accumulator
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
@ -293,8 +289,8 @@ void gps_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
d_enable_extended_integration=false;
d_preamble_synchronized=false;
d_enable_extended_integration = false;
d_preamble_synchronized = false;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
@ -329,8 +325,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
double code_error_filt_secs_Ti = 0.0;
double CURRENT_INTEGRATION_TIME_S = 0.0;
double CORRECTED_INTEGRATION_TIME_S = 0.0;
double dll_code_error_secs_Ti = 0.0;
double old_d_rem_code_phase_samples;
if (d_enable_tracking == true)
{
// Fill the acquisition data
@ -344,12 +339,14 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_correlation_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);
*out[0] = current_synchro_data;
d_sample_counter += samples_offset; //count for the processed samples
d_sample_counter += samples_offset; // count for the processed samples
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GPS_TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GPS_TWO_PI;
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
return 1;
}
@ -383,7 +380,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
{
// compute coherent integration and enable tracking loop
// perform coherent integration using correlator output history
//std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
// std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
d_correlator_outs[0] = gr_complex(0.0,0.0);
d_correlator_outs[1] = gr_complex(0.0,0.0);
d_correlator_outs[2] = gr_complex(0.0,0.0);
@ -399,9 +396,9 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_narrow_hz,2);
d_preamble_synchronized = true;
std::cout << "Enabled "<<d_extend_correlation_ms<<" [ms] extended correlator for CH "<< d_channel <<" : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<<" pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl
<<" dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl;
std::cout << "Enabled " << d_extend_correlation_ms << " [ms] extended correlator for CH "<< d_channel << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl
<< " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl;
}
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_extend_correlation_ms) * GPS_L1_CA_CODE_PERIOD;
@ -412,28 +409,26 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
if(d_preamble_synchronized == true)
{
// continue extended coherent correlation
//remnant carrier phase [rads]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GPS_TWO_PI);
// Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation
double T_chip_seconds = 1 / d_code_freq_chips;
double T_chip_seconds = 1.0 / 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);
int K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples;
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples);
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
//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]
// remnant code phase [chips]
d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GPS_TWO_PI);
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI;
// disable tracking loop and inform telemetry decoder
enable_dll_pll = false;
@ -458,10 +453,9 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
{
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; //prompt output
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
//d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_phase_error_filt_secs_ti/INTEGRATION_TIME;
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
// PLL to DLL assistance [Secs/Ti]
@ -471,46 +465,34 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
// ################## DLL ##########################################################
// DLL discriminator
d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti] //early and late
d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late
// Code discriminator filter
d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); //input [chips/Ti] -> output [chips/second]
d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second]
d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S;
code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti]
// DLL code error estimation [s/Ti]
// PLL to DLL assistance is disable due to the use of a fractional resampler that allows the correction of the code Doppler effect.
dll_code_error_secs_Ti = - code_error_filt_secs_Ti; // + d_pll_to_dll_assist_secs_Ti;
// ################## 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_prn_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 / 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_prn_samples = round(T_prn_samples);
double T_chip_seconds = 1.0 / 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_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
old_d_rem_code_phase_samples = d_rem_code_phase_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples);
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples; //round to a discrete samples
d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples + code_error_filt_secs_Ti * static_cast<double>(d_fs_in); //(code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti) * static_cast<double>(d_fs_in);
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
//################### PLL COMMANDS #################################################
//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);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
//remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
// UPDATE CARRIER PHASE ACCUULATOR
//carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
//################### PLL COMMANDS #################################################
//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);
//################### DLL COMMANDS #################################################
//code phase step (Code resampler phase increment per sample) [chips/sample]
@ -522,7 +504,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt
d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; // prompt
d_cn0_estimation_counter++;
}
else
@ -554,9 +536,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_length_samples + 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 = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
@ -575,9 +556,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_length_samples + 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 = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
@ -591,7 +571,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
}
current_synchro_data.System = {'G'};
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);
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_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);
}
//assign the GNURadio block output data
*out[0] = current_synchro_data;
@ -639,7 +620,7 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
// AUX vars (for debug purposes)
tmp_double = d_code_error_chips_Ti*CURRENT_INTEGRATION_TIME_S;
tmp_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(d_sample_counter + d_correlation_length_samples);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));

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

@ -169,7 +169,6 @@ private:
int d_extend_correlation_ms;
bool d_enable_extended_integration;
bool d_preamble_synchronized;
int d_correlation_symbol_counter;
void msg_handler_preamble_index(pmt::pmt_t msg);
//Integration period in samples

385
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -33,8 +33,10 @@
#include <iostream>
#include <memory>
#include <sstream>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <pmt/pmt.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <glog/logging.h>
#include "gnss_synchro.h"
@ -67,10 +69,11 @@ gps_l1_ca_dll_pll_c_aid_make_tracking_sc(
float dll_bw_hz,
float pll_bw_narrow_hz,
float dll_bw_narrow_hz,
int extend_correlation_ms,
float early_late_space_chips)
{
return gps_l1_ca_dll_pll_c_aid_tracking_sc_sptr(new gps_l1_ca_dll_pll_c_aid_tracking_sc(if_freq,
fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, pll_bw_narrow_hz, dll_bw_narrow_hz, early_late_space_chips));
fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, pll_bw_narrow_hz, dll_bw_narrow_hz, extend_correlation_ms, early_late_space_chips));
}
@ -85,6 +88,17 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::forecast (int noutput_items,
}
void gps_l1_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index(pmt::pmt_t msg)
{
//pmt::print(msg);
DLOG(INFO) << "Extended correlation enabled for Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN);
if (d_enable_extended_integration == false) //avoid re-setting preamble indicator
{
d_preamble_timestamp_s = pmt::to_double(msg);
d_enable_extended_integration = true;
d_preamble_synchronized = false;
}
}
gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
long if_freq,
@ -96,12 +110,15 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
float dll_bw_hz,
float pll_bw_narrow_hz,
float dll_bw_narrow_hz,
int extend_correlation_ms,
float early_late_space_chips) :
gr::block("gps_l1_ca_dll_pll_c_aid_tracking_sc", gr::io_signature::make(1, 1, sizeof(lv_16sc_t)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
// Telemetry bit synchronization message port input
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
this->set_msg_handler(pmt::mp("preamble_timestamp_s"),
boost::bind(&gps_l1_ca_dll_pll_c_aid_tracking_sc::msg_handler_preamble_index, this, _1));
this->message_port_register_out(pmt::mp("events"));
// initialize internal vars
d_dump = dump;
@ -112,14 +129,15 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
d_correlation_length_samples = static_cast<int>(d_vector_length);
// Initialize tracking ==========================================
d_pll_bw_hz=pll_bw_hz;
d_dll_bw_hz=dll_bw_hz;
d_pll_bw_hz = pll_bw_hz;
d_dll_bw_hz = dll_bw_hz;
d_pll_bw_narrow_hz = pll_bw_narrow_hz;
d_dll_bw_narrow_hz = dll_bw_narrow_hz;
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_params(10.0, pll_bw_hz,2);
d_code_loop_filter.set_DLL_BW(d_dll_bw_hz);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_hz, 2);
d_extend_correlation_ms = extend_correlation_ms;
//--- DLL variables --------------------------------------------------------
// --- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
// Initialization of local code replica
@ -133,7 +151,7 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
d_correlator_outs_16sc = static_cast<lv_16sc_t*>(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(lv_16sc_t), volk_gnsssdr_get_alignment()));
for (int n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs_16sc[n] = lv_16sc_t(0,0);
d_correlator_outs_16sc[n] = lv_cmake(0,0);
}
d_local_code_shift_chips = static_cast<float*>(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment()));
@ -153,10 +171,8 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
d_rem_carrier_phase_rad = 0.0;
// sample synchronization
d_sample_counter = 0;
//d_sample_counter_seconds = 0;
d_sample_counter = 0; //(from trk to tlm)
d_acq_sample_stamp = 0;
d_enable_tracking = false;
d_pull_in = false;
@ -171,7 +187,7 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
systemName["G"] = std::string("GPS");
systemName["S"] = std::string("SBAS");
set_relative_rate(1.0 / (static_cast<double>(d_vector_length) * 2.0));
set_relative_rate(1.0 / static_cast<double>(d_vector_length));
d_acquisition_gnss_synchro = 0;
d_channel = 0;
@ -180,11 +196,18 @@ gps_l1_ca_dll_pll_c_aid_tracking_sc::gps_l1_ca_dll_pll_c_aid_tracking_sc(
d_carrier_doppler_hz = 0.0;
d_acc_carrier_phase_cycles = 0.0;
d_code_phase_samples = 0.0;
d_enable_extended_integration = false;
d_preamble_synchronized = false;
d_rem_code_phase_integer_samples = 0;
d_code_error_chips_Ti = 0.0;
d_pll_to_dll_assist_secs_Ti = 0.0;
d_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 0.0;
d_carrier_phase_step_rad = 0.0;
d_code_error_filt_chips_s = 0.0;
d_code_error_filt_chips_Ti = 0.0;
d_preamble_timestamp_s = 0.0;
d_carr_phase_error_secs_Ti = 0.0;
//set_min_output_buffer((long int)300);
}
@ -203,7 +226,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
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<double>(acq_trk_diff_samples) / static_cast<double>(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
@ -212,7 +235,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1/d_code_freq_chips;
T_chip_mod_seconds = 1.0 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
@ -237,7 +260,7 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); //The carrier loop filter implements the Doppler accumulator
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
@ -268,6 +291,8 @@ void gps_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
d_enable_extended_integration = false;
d_preamble_synchronized = false;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
@ -301,14 +326,10 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri
Gnss_Synchro current_synchro_data = Gnss_Synchro();
// process vars
double code_error_chips_Ti = 0.0;
double code_error_filt_chips = 0.0;
double code_error_filt_secs_Ti = 0.0;
double CURRENT_INTEGRATION_TIME_S;
double CORRECTED_INTEGRATION_TIME_S;
double dll_code_error_secs_Ti = 0.0;
double carr_phase_error_secs_Ti = 0.0;
double old_d_rem_code_phase_samples;
double CURRENT_INTEGRATION_TIME_S = 0.0;
double CORRECTED_INTEGRATION_TIME_S = 0.0;
if (d_enable_tracking == true)
{
// Fill the acquisition data
@ -323,140 +344,242 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri
acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_correlation_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);
*out[0] = current_synchro_data;
d_sample_counter += samples_offset; //count for the processed samples
d_sample_counter += samples_offset; // count for the processed samples
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GPS_TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GPS_TWO_PI;
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
return 1;
}
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu_16sc.set_input_output_vectors(d_correlator_outs_16sc, in);
multicorrelator_cpu_16sc.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad, d_carrier_phase_step_rad, d_rem_code_phase_chips, d_code_phase_step_chips, d_correlation_length_samples);
multicorrelator_cpu_16sc.Carrier_wipeoff_multicorrelator_resampler(d_rem_carrier_phase_rad,
d_carrier_phase_step_rad,
d_rem_code_phase_chips,
d_code_phase_step_chips,
d_correlation_length_samples);
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
// ####### coherent intergration extension
// keep the last symbols
d_E_history.push_back(d_correlator_outs_16sc[0]); // save early output
d_P_history.push_back(d_correlator_outs_16sc[1]); // save prompt output
d_L_history.push_back(d_correlator_outs_16sc[2]); // save late output
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())) / GPS_TWO_PI; //prompt output
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
//d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_phase_error_filt_secs_ti/INTEGRATION_TIME;
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / GPS_L1_FREQ_HZ;
// code Doppler frequency update
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
// ################## DLL ##########################################################
// DLL discriminator
code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex<float>(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag()), std::complex<float>(d_correlator_outs_16sc[2].real(),d_correlator_outs_16sc[2].imag())); //[chips/Ti] //early and late
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips_Ti); //input [chips/Ti] -> output [chips/second]
code_error_filt_secs_Ti = code_error_filt_chips*CURRENT_INTEGRATION_TIME_S/d_code_freq_chips; // [s/Ti]
// DLL code error estimation [s/Ti]
// TODO: PLL carrier aid to DLL is disabled. Re-enable it and measure performance
dll_code_error_secs_Ti = - code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti;
// ################## 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 / 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 - dll_code_error_secs_Ti * static_cast<double>(d_fs_in);
d_correlation_length_samples = round(K_blk_samples); //round to a discrete samples
old_d_rem_code_phase_samples = d_rem_code_phase_samples;
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_correlation_length_samples); //rounding error < 1 sample
// UPDATE REMNANT CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
//remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
// UPDATE CARRIER PHASE ACCUULATOR
//carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
//################### PLL COMMANDS #################################################
//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);
//################### DLL COMMANDS #################################################
//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_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
// ####### CN0 ESTIMATION AND LOCK DETECTORS #######################################
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
if (static_cast<int>(d_P_history.size()) > d_extend_correlation_ms)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = std::complex<float>(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag()); //prompt
d_cn0_estimation_counter++;
d_E_history.pop_front();
d_P_history.pop_front();
d_L_history.pop_front();
}
else
bool enable_dll_pll;
if (d_enable_extended_integration == true)
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
long int symbol_diff = round(1000.0 * ((static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in) - d_preamble_timestamp_s));
if (symbol_diff > 0 and symbol_diff % d_extend_correlation_ms == 0)
{
d_carrier_lock_fail_counter++;
// compute coherent integration and enable tracking loop
// perform coherent integration using correlator output history
// std::cout<<"##### RESET COHERENT INTEGRATION ####"<<std::endl;
d_correlator_outs_16sc[0] = lv_cmake(0,0);
d_correlator_outs_16sc[1] = lv_cmake(0,0);
d_correlator_outs_16sc[2] = lv_cmake(0,0);
for (int n = 0; n < d_extend_correlation_ms; n++)
{
d_correlator_outs_16sc[0] += d_E_history.at(n);
d_correlator_outs_16sc[1] += d_P_history.at(n);
d_correlator_outs_16sc[2] += d_L_history.at(n);
}
if (d_preamble_synchronized == false)
{
d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz);
d_carrier_loop_filter.set_params(10.0, d_pll_bw_narrow_hz,2);
d_preamble_synchronized = true;
std::cout << "Enabled " << d_extend_correlation_ms << " [ms] extended correlator for CH "<< d_channel << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl
<< " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl;
}
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_extend_correlation_ms) * GPS_L1_CA_CODE_PERIOD;
enable_dll_pll = true;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout << "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
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
if(d_preamble_synchronized == true)
{
// continue extended coherent correlation
// Compute the next buffer length based on the period of the PRN sequence and the code phase error estimation
double T_chip_seconds = 1.0 / 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);
int K_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples;
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
// 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_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GPS_TWO_PI);
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI;
// disable tracking loop and inform telemetry decoder
enable_dll_pll = false;
}
else
{
// perform basic (1ms) correlation
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
enable_dll_pll = true;
}
}
}
else
{
// UPDATE INTEGRATION TIME
CURRENT_INTEGRATION_TIME_S = static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in);
enable_dll_pll = true;
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data;
if (enable_dll_pll == true)
{
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(),d_correlator_outs_16sc[1].imag())) / GPS_TWO_PI; //prompt output
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
// Input [s/Ti] -> output [Hz]
d_carrier_doppler_hz = d_carrier_loop_filter.get_carrier_error(0.0, d_carr_phase_error_secs_Ti, CURRENT_INTEGRATION_TIME_S);
// PLL to DLL assistance [Secs/Ti]
d_pll_to_dll_assist_secs_Ti = (d_carrier_doppler_hz * CURRENT_INTEGRATION_TIME_S) / GPS_L1_FREQ_HZ;
// code Doppler frequency update
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
// ################## DLL ##########################################################
// DLL discriminator
d_code_error_chips_Ti = dll_nc_e_minus_l_normalized(std::complex<float>(d_correlator_outs_16sc[0].real(),d_correlator_outs_16sc[0].imag()), std::complex<float>(d_correlator_outs_16sc[2].real(),d_correlator_outs_16sc[2].imag())); // [chips/Ti] //early and late
// Code discriminator filter
d_code_error_filt_chips_s = d_code_loop_filter.get_code_nco(d_code_error_chips_Ti); // input [chips/Ti] -> output [chips/second]
d_code_error_filt_chips_Ti = d_code_error_filt_chips_s * CURRENT_INTEGRATION_TIME_S;
code_error_filt_secs_Ti = d_code_error_filt_chips_Ti / d_code_freq_chips; // [s/Ti]
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
double T_chip_seconds = 1.0 / 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_prn_samples = round(T_prn_samples);
double K_T_prn_error_samples = K_prn_samples - T_prn_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - K_T_prn_error_samples + code_error_filt_secs_Ti * static_cast<double>(d_fs_in); //(code_error_filt_secs_Ti + d_pll_to_dll_assist_secs_Ti) * static_cast<double>(d_fs_in);
d_rem_code_phase_integer_samples = round(d_rem_code_phase_samples); // round to a discrete number of samples
d_correlation_length_samples = K_prn_samples + d_rem_code_phase_integer_samples;
d_rem_code_phase_samples = d_rem_code_phase_samples - d_rem_code_phase_integer_samples;
//################### PLL COMMANDS #################################################
//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);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GPS_TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
//remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
//################### DLL COMMANDS #################################################
//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_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
// ####### CN0 ESTIMATION AND LOCK DETECTORS #######################################
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = lv_cmake(static_cast<float>(d_correlator_outs_16sc[1].real()), static_cast<float>(d_correlator_outs_16sc[1].imag()) ); // prompt
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
{
d_carrier_lock_fail_counter++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout << "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
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_length_samples + 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 = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
if (d_preamble_synchronized == true)
{
current_synchro_data.correlation_length_ms = d_extend_correlation_ms;
}
else
{
current_synchro_data.correlation_length_ms = 1;
}
}
else
{
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs_16sc[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_length_samples + 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 = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
}
}
else
{
for (int n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs_16sc[n] = lv_16sc_t(0,0);
d_correlator_outs_16sc[n] = lv_cmake(0,0);
}
current_synchro_data.System = {'G'};
current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
*out[0] = current_synchro_data;
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_correlation_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);
}
*out[0] = current_synchro_data;
if(d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
@ -489,19 +612,19 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
//PLL commands
d_dump_file.write(reinterpret_cast<char*>(&carr_phase_error_secs_Ti), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_carr_phase_error_secs_Ti), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
//DLL commands
d_dump_file.write(reinterpret_cast<char*>(&code_error_chips_Ti), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_code_error_chips_Ti), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_code_error_filt_chips_Ti), sizeof(double));
// CN0 and carrier lock test
d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
// AUX vars (for debug purposes)
tmp_double = d_rem_code_phase_samples;
tmp_double = d_code_error_chips_Ti * CURRENT_INTEGRATION_TIME_S;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(d_sample_counter + d_correlation_length_samples);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));

18
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.h
View File

@ -65,6 +65,7 @@ gps_l1_ca_dll_pll_c_aid_make_tracking_sc(long if_freq,
float dll_bw_hz,
float pll_bw_narrow_hz,
float dll_bw_narrow_hz,
int extend_correlation_ms,
float early_late_space_chips);
@ -97,6 +98,7 @@ private:
float dll_bw_hz,
float pll_bw_narrow_hz,
float dll_bw_narrow_hz,
int extend_correlation_ms,
float early_late_space_chips);
gps_l1_ca_dll_pll_c_aid_tracking_sc(long if_freq,
@ -108,6 +110,7 @@ private:
float dll_bw_hz,
float pll_bw_narrow_hz,
float dll_bw_narrow_hz,
int extend_correlation_ms,
float early_late_space_chips);
// tracking configuration vars
@ -135,6 +138,7 @@ private:
double d_rem_code_phase_samples;
double d_rem_code_phase_chips;
double d_rem_carrier_phase_rad;
int d_rem_code_phase_integer_samples;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
@ -156,6 +160,20 @@ private:
double d_acc_carrier_phase_cycles;
double d_code_phase_samples;
double d_pll_to_dll_assist_secs_Ti;
double d_carr_phase_error_secs_Ti;
double d_code_error_chips_Ti;
double d_preamble_timestamp_s;
int d_extend_correlation_ms;
bool d_enable_extended_integration;
bool d_preamble_synchronized;
double d_code_error_filt_chips_s;
double d_code_error_filt_chips_Ti;
void msg_handler_preamble_index(pmt::pmt_t msg);
// symbol history to detect bit transition
std::deque<lv_16sc_t> d_E_history;
std::deque<lv_16sc_t> d_P_history;
std::deque<lv_16sc_t> d_L_history;
//Integration period in samples
int d_correlation_length_samples;

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

@ -174,7 +174,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
d_carrier_doppler_hz = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_code_phase_samples = 0.0;
d_acc_code_phase_secs = 0.0;
d_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 0.0;
d_carrier_phase_step_rad = 0.0;
@ -194,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
@ -247,7 +246,6 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_rem_carr_phase_rad = 0.0;
d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_acc_code_phase_secs = 0.0;
d_code_phase_samples = d_acq_code_phase_samples;
@ -311,10 +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 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;
}
@ -330,52 +332,45 @@ 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
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz;
// New code Doppler frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * ( d_if_freq + d_carrier_doppler_hz ) * GPS_L1_CA_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## 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]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
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);
// 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
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_chips = d_rem_code_phase_samples * (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
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 ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
@ -404,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
}
@ -413,14 +408,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
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);
//compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
// 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.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;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
@ -434,7 +424,8 @@ 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) + 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'};
}
@ -447,6 +438,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
double tmp_double;
unsigned long int tmp_long;
prompt_I = d_correlator_outs[1].real();
prompt_Q = d_correlator_outs[1].imag();
tmp_E = std::abs<float>(d_correlator_outs[0]);
@ -462,8 +454,8 @@ 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_float=(float)d_sample_counter;
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
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));
@ -471,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));
d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_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));
@ -486,7 +478,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__
// AUX vars (for debug purposes)
tmp_double = d_rem_code_phase_samples;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
tmp_double = static_cast<double>(d_sample_counter);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
@ -496,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;

2
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc
View File

@ -456,11 +456,9 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attri
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Tracking_timestamp_secs = d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz;
current_synchro_data.Code_phase_secs = (double)d_code_phase_samples * (1/(float)d_fs_in);
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms=1;

68
src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
View File

@ -179,7 +179,6 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc(
d_carrier_doppler_hz = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_code_phase_samples = 0.0;
d_acc_code_phase_secs = 0.0;
d_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 0.0;
@ -203,7 +202,7 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
LOG(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_L2_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L2_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
@ -253,7 +252,6 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
d_rem_carr_phase_rad = 0;
d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0;
d_acc_code_phase_secs = 0;
d_code_phase_samples = d_acq_code_phase_samples;
@ -318,8 +316,11 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
acq_trk_shif_correction_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);//+(1.5*(d_fs_in/GPS_L2_M_CODE_RATE_HZ)));
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);
*out[0] = current_synchro_data;
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
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;
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
return 1;
@ -343,45 +344,39 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz;
// New code Doppler frequency estimation
d_code_freq_chips = GPS_L2_M_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L2_M_CODE_RATE_HZ) / GPS_L2_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad -= GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_L2_TWO_PI * d_carrier_doppler_hz * GPS_L2_M_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L2_TWO_PI);
// ################## DLL ##########################################################
// DLL discriminator
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); //[chips/Ti]
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti]
// 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_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_CODE_RATE_HZ; //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
double code_error_filt_secs = (GPS_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_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.0 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * GPS_L2_M_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 / d_code_freq_chips;
double T_prn_seconds = T_chip_seconds * GPS_L2_M_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_L2_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// 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_L2_TWO_PI);
// 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_chips = d_rem_code_phase_samples * (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
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 ######
if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES)
@ -418,20 +413,13 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>(d_correlator_outs[1].real());
current_synchro_data.Prompt_Q = static_cast<double>(d_correlator_outs[1].imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
//compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
// 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.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms=20;
current_synchro_data.correlation_length_ms = 20;
}
else
@ -440,7 +428,7 @@ int gps_l2_m_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) + 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);
}
//assign the GNURadio block output data
*out[0] = current_synchro_data;
@ -500,8 +488,8 @@ int gps_l2_m_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
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
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
}

5
src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h
View File

@ -136,12 +136,11 @@ 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
// PRN period in samples
int d_current_prn_length_samples;
//processing samples counters
// processing samples counters
unsigned long int d_sample_counter;
unsigned long int d_acq_sample_stamp;

2
src/core/system_parameters/gnss_synchro.h
View File

@ -57,8 +57,8 @@ public:
double CN0_dB_hz; //!< Set by Tracking processing block
double Carrier_Doppler_hz; //!< Set by Tracking processing block
double Carrier_phase_rads; //!< Set by Tracking processing block
double Code_phase_secs; //!< Set by Tracking processing block
double Tracking_timestamp_secs; //!< Set by Tracking processing block
double Rem_code_phase_secs; //!< Set by Tracking processing block
bool Flag_valid_symbol_output; //!< Set by Tracking processing block
int correlation_length_ms; //!< Set by Tracking processing block

40
src/core/system_parameters/gps_ephemeris.cc
View File

@ -119,7 +119,13 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime)
{
double dt;
dt = check_t(transmitTime - d_Toc);
d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime);
for (int i = 0; i < 2; i++)
{
dt -= d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt);
}
d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt);
return d_satClkDrift;
}
@ -174,7 +180,7 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
}
void Gps_Ephemeris::satellitePosition(double transmitTime)
double Gps_Ephemeris::satellitePosition(double transmitTime)
{
double tk;
double a;
@ -194,13 +200,13 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
// Find satellite's position ----------------------------------------------
// Restore semi-major axis
a = d_sqrt_A*d_sqrt_A;
a = d_sqrt_A * d_sqrt_A;
// Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe);
// Computed mean motion
n0 = sqrt(GM / (a*a*a));
n0 = sqrt(GM / (a * a * a));
// Corrected mean motion
n = n0 + d_Delta_n;
@ -209,17 +215,17 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2*GPS_PI), (2*GPS_PI));
M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
// Initial guess of eccentric anomaly
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1; ii<20; ii++)
for (int ii = 1; ii < 20; ii++)
{
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2*GPS_PI);
dE = fmod(E - E_old, 2.0 * GPS_PI);
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
@ -236,22 +242,22 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2*GPS_PI));
phi = fmod((phi), (2.0 * GPS_PI));
// Correct argument of latitude
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi);
u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi);
// Correct radius
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi);
r = a * (1.0 - d_e_eccentricity*cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi);
// Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi);
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi);
// Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2*GPS_PI), (2*GPS_PI));
Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -263,4 +269,14 @@ void Gps_Ephemeris::satellitePosition(double transmitTime)
d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i);
// Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc);
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */
dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
return dtr_s;
}

3
src/core/system_parameters/gps_ephemeris.h
View File

@ -183,8 +183,9 @@ public:
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200E)
* and compute the clock bias term including relativistic effect (return value)
*/
void satellitePosition(double transmitTime);
double satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction

15
src/core/system_parameters/gps_navigation_message.cc
View File

@ -428,14 +428,13 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
case 1:
//--- It is subframe 1 -------------------------------------
// Compute the time of week (TOW) of the first sub-frames in the array ====
// Also correct the TOW. The transmitted TOW is actual TOW of the next
// subframe and we need the TOW of the first subframe in this data block
// The transmitted TOW is actual TOW of the next subframe
// (the variable subframe at this point contains bits of the last subframe).
//TOW = bin2dec(subframe(31:47)) * 6 - 30;
//TOW = bin2dec(subframe(31:47)) * 6;
d_TOW_SF1 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
//we are in the first subframe (the transmitted TOW is the start time of the next subframe) !
d_TOW_SF1 = d_TOW_SF1 * 6;
d_TOW = d_TOW_SF1 - 6; // Set transmission time
d_TOW = d_TOW_SF1; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -461,7 +460,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
case 2: //--- It is subframe 2 -------------------
d_TOW_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF2 = d_TOW_SF2 * 6;
d_TOW = d_TOW_SF2 - 6; // Set transmission time
d_TOW = d_TOW_SF2; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -491,7 +490,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
case 3: // --- It is subframe 3 -------------------------------------
d_TOW_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF3 = d_TOW_SF3 * 6;
d_TOW = d_TOW_SF3 - 6; // Set transmission time
d_TOW = d_TOW_SF3; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -520,7 +519,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
int SV_page;
d_TOW_SF4 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF4 = d_TOW_SF4 * 6;
d_TOW = d_TOW_SF4 - 6; // Set transmission time
d_TOW = d_TOW_SF4; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
@ -596,7 +595,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
int SV_page_5;
d_TOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF5 = d_TOW_SF5 * 6;
d_TOW = d_TOW_SF5 - 6; // Set transmission time
d_TOW = d_TOW_SF5; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);

53
src/tests/CMakeLists.txt
View File

@ -16,6 +16,9 @@
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
add_subdirectory(unit-tests/signal-processing-blocks/libs)
################################################################################
# Google Test - https://github.com/google/googletest
################################################################################
@ -136,7 +139,7 @@ endif(ENABLE_CUDA)
################################################################################
# Optional generator
################################################################################
if(ENABLE_SYSTEM_TESTING_EXTRA)
if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA)
ExternalProject_Add(
gnss-sim
GIT_REPOSITORY https://bitbucket.org/jarribas/gnss-simulator
@ -152,31 +155,28 @@ if(ENABLE_SYSTEM_TESTING_EXTRA)
add_definitions(-DSW_GENERATOR_BIN="${SW_GENERATOR_BIN}")
add_definitions(-DDEFAULT_RINEX_NAV="${CMAKE_CURRENT_BINARY_DIR}/../../../thirdparty/gnss-sim/brdc3540.14n")
add_definitions(-DDEFAULT_POSITION_FILE="${CMAKE_CURRENT_BINARY_DIR}/../../../thirdparty/gnss-sim/circle.csv")
################################################################################
# Local installation of GPSTk http://www.gpstk.org/
################################################################################
find_package(GPSTK)
if(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
set(gpstk_RELEASE "2.5")
set(gpstk_md5 "9d79f6838d274f5edfd46c780a6b1b72")
set(gpstk_RELEASE "2.9")
ExternalProject_Add(
gpstk-${gpstk_RELEASE}
URL https://sourceforge.net/projects/gpstk/files/gpstk/${gpstk_RELEASE}/gpstk-${gpstk_RELEASE}.src.tar.gz
URL_MD5 ${gpstk_md5}
SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk
BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk
CONFIGURE_COMMAND ""
BUILD_COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk/script_gpstk.sh -c
GIT_REPOSITORY https://github.com/SGL-UT/GPSTk
GIT_TAG v${gpstk_RELEASE}
SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk-${gpstk_RELEASE}
BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/../../gpstk-${gpstk_RELEASE}
CMAKE_ARGS ${GTEST_COMPILER} ${TOOLCHAIN_ARG} -DCMAKE_INSTALL_PREFIX=${CMAKE_SOURCE_DIR}/thirdparty/gpstk-${gpstk_RELEASE}/install -DBUILD_EXT=OFF -DBUILD_PYTHON=OFF
UPDATE_COMMAND ""
PATCH_COMMAND ""
INSTALL_COMMAND ""
)
set(GPSTK_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk/dev/install/include CACHE PATH "Local GPSTK headers")
set(GPSTK_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk-${gpstk_RELEASE}/install/include CACHE PATH "Local GPSTK headers")
add_library(gpstk UNKNOWN IMPORTED)
set_property(TARGET gpstk PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk/dev/install/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gpstk${CMAKE_SHARED_LIBRARY_SUFFIX})
set_property(TARGET gpstk PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk-${gpstk_RELEASE}/install/lib/${CMAKE_FIND_LIBRARY_PREFIXES}gpstk${CMAKE_SHARED_LIBRARY_SUFFIX})
add_dependencies(gpstk gpstk-${gpstk_RELEASE})
set(GPSTK_BINDIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk/dev/install/bin/ )
set(GPSTK_BINDIR ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/gpstk-${gpstk_RELEASE}/install/bin/ )
add_definitions(-DGPSTK_BINDIR="${GPSTK_BINDIR}")
set(gpstk_libs gpstk)
else(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
@ -185,10 +185,23 @@ if(ENABLE_SYSTEM_TESTING_EXTRA)
set(GPSTK_BINDIR ${GPSTK_LIBRARY}/../bin/ )
add_definitions(-DGPSTK_BINDIR="${GPSTK_BINDIR}")
endif(NOT GPSTK_FOUND OR ENABLE_OWN_GPSTK)
endif(ENABLE_SYSTEM_TESTING_EXTRA)
endif(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA)
add_definitions(-DTEST_PATH="${CMAKE_SOURCE_DIR}/src/tests/")
if(ENABLE_UNIT_TESTING_EXTRA)
add_definitions(-DEXTRA_TESTS)
if(NOT EXISTS ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples/gps_l2c_m_prn7_5msps.dat)
message(STATUS "Downloading some data files for testing...")
file(DOWNLOAD https://sourceforge.net/projects/gnss-sdr/files/data/gps_l2c_m_prn7_5msps.dat ${CMAKE_CURRENT_SOURCE_DIR}/../../thirdparty/signal_samples/gps_l2c_m_prn7_5msps.dat
SHOW_PROGRESS
EXPECTED_HASH MD5=a6fcbefe155137945d3c33c5ef7bd0f9 )
endif(NOT EXISTS ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples/gps_l2c_m_prn7_5msps.dat)
endif(ENABLE_UNIT_TESTING_EXTRA)
file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/GSoC_CTTC_capture_2012_07_26_4Msps_4ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples)
file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/Galileo_E1_ID_1_Fs_4Msps_8ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples)
file(COPY ${CMAKE_SOURCE_DIR}/src/tests/signal_samples/GPS_L1_CA_ID_1_Fs_4Msps_2ms.dat DESTINATION ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples)
add_definitions(-DTEST_PATH="${CMAKE_SOURCE_DIR}/thirdparty/")
include_directories(
${GTEST_INCLUDE_DIRECTORIES}
@ -220,6 +233,8 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/adapters
${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/gnuradio_blocks
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs
${CMAKE_SOURCE_DIR}/src/tests/unit-tests/signal-processing-blocks/libs
${CMAKE_SOURCE_DIR}/src/tests/common-files
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
${GNURADIO_RUNTIME_INCLUDE_DIRS}
@ -237,6 +252,9 @@ include_directories(
# Unit testing
################################################################################
if(ENABLE_UNIT_TESTING)
if( ${ARMADILLO_VERSION_STRING} STRGREATER "5.300") # make sure interp1 is present
add_definitions(-DMODERN_ARMADILLO)
endif( ${ARMADILLO_VERSION_STRING} STRGREATER "5.300")
add_executable(run_tests ${CMAKE_CURRENT_SOURCE_DIR}/test_main.cc)
add_custom_command(TARGET run_tests POST_BUILD
@ -261,6 +279,7 @@ if(ENABLE_UNIT_TESTING)
signal_generator_blocks
signal_generator_adapters
pvt_gr_blocks
signak_processing_testing_lib
${VOLK_GNSSSDR_LIBRARIES}
${GNSS_SDR_TEST_OPTIONAL_LIBS}
)
@ -489,7 +508,7 @@ if(ENABLE_SYSTEM_TESTING)
)
if(ENABLE_SYSTEM_TESTING_EXTRA)
add_executable(obs_gps_l1_system_test ${CMAKE_CURRENT_SOURCE_DIR}/system-tests/obs_gps_l1_system_test.cc )
add_executable(obs_gps_l1_system_test ${CMAKE_CURRENT_SOURCE_DIR}/system-tests/obs_gps_l1_system_test.cc)
if(NOT ${GTEST_DIR_LOCAL})
add_dependencies(obs_gps_l1_system_test gtest-${gtest_RELEASE} )
else(NOT ${GTEST_DIR_LOCAL})

48
src/tests/common-files/signal_generator_flags.h Normal file
View File

@ -0,0 +1,48 @@
/*!
* \file signal_generator_flags.h
* \brief Helper file for unit testing
* \author Carles Fernandez-Prades, 2017. cfernandez(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_SIGNAL_GENERATOR_FLAGS_H_
#define GNSS_SDR_SIGNAL_GENERATOR_FLAGS_H_
#include <gflags/gflags.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, 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");
DEFINE_string(filename_raw_data, "signal_out.bin", "Filename of output raw data file");
DEFINE_int32(fs_gen_hz, 2600000, "Samppling frequency [Hz]");
DEFINE_int32(test_satellite_PRN, 1, "PRN of the satellite under test (must be visible during the observation time)");
#endif

BIN
src/tests/data/gps_l2c_m_prn7_5msps.dat
View File

Binary file not shown.

41
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,20 +46,20 @@
#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"
#include "signal_generator_flags.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_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");
DEFINE_string(filename_raw_data, "signal_out.bin", "Filename of output raw data file");
DECLARE_string(generator_binary);
DECLARE_string(rinex_nav_file);
DECLARE_int32(duration);
DECLARE_string(static_position);
DECLARE_string(dynamic_position);
DECLARE_string(filename_rinex_obs);
DECLARE_string(filename_raw_data);
// For GPS NAVIGATION (L1)
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
@ -131,7 +131,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
{
@ -162,7 +163,7 @@ int Obs_Gps_L1_System_Test::generate_signal()
}
wait_result = waitpid(pid, &child_status, 0);
if (wait_result == -1) perror("waitpid error");
EXPECT_EQ(true, check_valid_rinex_obs(filename_rinex_obs));
std::cout << "Signal and Observables RINEX files created." << std::endl;
return 0;
@ -173,9 +174,7 @@ int Obs_Gps_L1_System_Test::configure_receiver()
{
config = std::make_shared<InMemoryConfiguration>();
const double central_freq = 1575420000.0;
const int sampling_rate_internal = baseband_sampling_freq;
const double gain_dB = 40.0;
const int number_of_taps = 11;
const int number_of_bands = 2;
@ -214,9 +213,7 @@ int Obs_Gps_L1_System_Test::configure_receiver()
const int display_rate_ms = 500;
const int output_rate_ms = 1000;
const int averaging_depth = 10;
bool false_bool = false;
const int averaging_depth = 1;
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(sampling_rate_internal));
@ -361,7 +358,7 @@ int Obs_Gps_L1_System_Test::run_receiver()
std::cout << "Failed to run command: " << argum2 << std::endl;
return -1;
}
char * without_trailing;
char * without_trailing = (char*)"0";
while (fgets(buffer, sizeof(buffer), fp) != NULL)
{
std::string aux = std::string(buffer);
@ -411,7 +408,7 @@ void Obs_Gps_L1_System_Test::check_results()
}
else
{
dataobj = r_ref_data.getObs(prn, "P1", r_ref_header);
dataobj = r_ref_data.getObs(prn, "C1C", r_ref_header);
double P1 = dataobj.data;
std::pair<double, double> pseudo(sow,P1);
pseudorange_ref.at(myprn).push_back(pseudo);
@ -471,7 +468,7 @@ void Obs_Gps_L1_System_Test::check_results()
}
else
{
dataobj = r_meas_data.getObs(prn, "C1", r_meas_header);
dataobj = r_meas_data.getObs(prn, "C1C", r_meas_header);
double P1 = dataobj.data;
std::pair<double, double> pseudo(sow, P1);
pseudorange_meas.at(myprn).push_back(pseudo);
@ -594,8 +591,8 @@ 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;
// double stdev_ = compute_stdev(*iter_diff);
// std::cout << " +/- " << stdev_ ;
double stdev_ = compute_stdev(*iter_diff);
std::cout << " +/- " << stdev_ ;
std::cout << " [m]" << std::endl;
}
else

14
src/tests/test_main.cc
View File

@ -94,7 +94,6 @@ DECLARE_string(log_dir);
#include "unit-tests/signal-processing-blocks/resampler/direct_resampler_conditioner_cc_test.cc"
#include "unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_test.cc"
#include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc"
#include "unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_acquisition_gsoc2013_test.cc"
#include "unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_quicksync_acquisition_gsoc2014_test.cc"
#include "unit-tests/signal-processing-blocks/acquisition/gps_l1_ca_pcps_tong_acquisition_gsoc2013_test.cc"
@ -113,17 +112,26 @@ DECLARE_string(log_dir);
#include "unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/galileo_e5a_tracking_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc"
#if CUDA_BLOCKS_TEST
#include "unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc"
#endif
#include "unit-tests/signal-processing-blocks/pvt/rtcm_test.cc"
#include "unit-tests/signal-processing-blocks/pvt/rtcm_printer_test.cc"
#include "unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc"
#if EXTRA_TESTS
#include "unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc"
#if MODERN_ARMADILLO
#include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc"
#include "unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc"
#endif
#endif
// For GPS NAVIGATION (L1)
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;

4
src/tests/unit-tests/signal-processing-blocks/acquisition/galileo_e1_pcps_ambiguous_acquisition_test.cc
View File

@ -172,7 +172,7 @@ TEST_F(GalileoE1PcpsAmbiguousAcquisitionTest, ConnectAndRun)
init();
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(config, "Acquisition", "Galileo_E1_PCPS_Ambiguous_Acquisition", 1, 1);
std::shared_ptr<AcquisitionInterface> acquisition = std::dynamic_pointer_cast<AcquisitionInterface>(acq_);
boost::shared_ptr<GpsL2MPcpsAcquisitionTest_msg_rx> msg_rx = GpsL2MPcpsAcquisitionTest_msg_rx_make();
boost::shared_ptr<GalileoE1PcpsAmbiguousAcquisitionTest_msg_rx> msg_rx = GalileoE1PcpsAmbiguousAcquisitionTest_msg_rx_make();
ASSERT_NO_THROW( {
acquisition->connect(top_block);
@ -206,7 +206,7 @@ TEST_F(GalileoE1PcpsAmbiguousAcquisitionTest, ValidationOfResults)
top_block = gr::make_top_block("Acquisition test");
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(config, "Acquisition", "Galileo_E1_PCPS_Ambiguous_Acquisition", 1, 1);
std::shared_ptr<GalileoE1PcpsAmbiguousAcquisition> acquisition = std::dynamic_pointer_cast<GalileoE1PcpsAmbiguousAcquisition>(acq_);
boost::shared_ptr<GpsL2MPcpsAcquisitionTest_msg_rx> msg_rx = GpsL2MPcpsAcquisitionTest_msg_rx_make();
boost::shared_ptr<GalileoE1PcpsAmbiguousAcquisitionTest_msg_rx> msg_rx = GalileoE1PcpsAmbiguousAcquisitionTest_msg_rx_make();
ASSERT_NO_THROW( {
acquisition->set_channel(gnss_synchro.Channel_ID);

2
src/tests/unit-tests/signal-processing-blocks/acquisition/gps_l2_m_pcps_acquisition_test.cc
View File

@ -241,7 +241,7 @@ TEST_F(GpsL2MPcpsAcquisitionTest, ValidationOfResults)
ASSERT_NO_THROW( {
std::string path = std::string(TEST_PATH);
//std::string file = path + "signal_samples/GSoC_CTTC_capture_2012_07_26_4Msps_4ms.dat";
std::string file = path + "/data/gps_l2c_m_prn7_5msps.dat";
std::string file = path + "signal_samples/gps_l2c_m_prn7_5msps.dat";
//std::string file = "/datalogger/signals/Fraunhofer/L125_III1b_210s_L2_resampled.bin";
const char * file_name = file.c_str();
gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(gr_complex), file_name, false);

37
src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt Normal file
View File

@ -0,0 +1,37 @@
# Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors)
#
# This file is part of GNSS-SDR.
#
# GNSS-SDR is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# GNSS-SDR is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
set(SIGNAL_PROCESSING_TESTING_LIB_SOURCES
tracking_dump_reader.cc
tlm_dump_reader.cc
tracking_true_obs_reader.cc
)
include_directories(
$(CMAKE_CURRENT_SOURCE_DIR)
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
)
file(GLOB SIGNAL_PROCESSING_TESTING_LIB_HEADERS "*.h")
list(SORT SIGNAL_PROCESSING_TESTING_LIB_HEADERS)
add_library(signak_processing_testing_lib ${SIGNAL_PROCESSING_TESTING_LIB_SOURCES} ${SIGNAL_PROCESSING_TESTING_LIB_HEADERS})
source_group(Headers FILES ${SIGNAL_PROCESSING_TESTING_LIB_HEADERS})

110
src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc Normal file
View File

@ -0,0 +1,110 @@
/*!
* \file tlm_dump_reader.cc
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "tlm_dump_reader.h"
bool tlm_dump_reader::read_binary_obs()
{
try
{
d_dump_file.read((char *) &TOW_at_current_symbol, sizeof(double));
d_dump_file.read((char *) &Prn_timestamp_ms, sizeof(double));
d_dump_file.read((char *) &d_TOW_at_Preamble, sizeof(double));
}
catch (const std::ifstream::failure &e)
{
return false;
}
return true;
}
bool tlm_dump_reader::restart()
{
if (d_dump_file.is_open())
{
d_dump_file.clear();
d_dump_file.seekg(0, std::ios::beg);
return true;
}
else
{
return false;
}
}
long int tlm_dump_reader::num_epochs()
{
std::ifstream::pos_type size;
int number_of_vars_in_epoch = 3;
int epoch_size_bytes = sizeof(double)*number_of_vars_in_epoch;
std::ifstream tmpfile( d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
if (tmpfile.is_open())
{
size = tmpfile.tellg();
long int nepoch=size / epoch_size_bytes;
return nepoch;
}
else
{
return 0;
}
}
bool tlm_dump_reader::open_obs_file(std::string out_file)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename=out_file;
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::in | std::ios::binary);
std::cout << "TLM dump enabled, Log file: " << d_dump_filename.c_str()<< std::endl;
return true;
}
catch (const std::ifstream::failure & e)
{
std::cout << "Problem opening TLM dump Log file: " << d_dump_filename.c_str()<< std::endl;
return false;
}
}
else
{
return false;
}
}
tlm_dump_reader::~tlm_dump_reader()
{
if (d_dump_file.is_open() == true)
{
d_dump_file.close();
}
}

60
src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h Normal file
View File

@ -0,0 +1,60 @@
/*!
* \file tlm_dump_reader.h
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_tlm_dump_reader_H
#define GNSS_SDR_tlm_dump_reader_H
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
class tlm_dump_reader {
public:
~tlm_dump_reader();
bool read_binary_obs();
bool restart();
long int num_epochs();
bool open_obs_file(std::string out_file);
//telemetry decoder dump variables
double TOW_at_current_symbol;
double Prn_timestamp_ms;
double d_TOW_at_Preamble;
private:
std::string d_dump_filename;
std::ifstream d_dump_file;
};
#endif //GNSS_SDR_tlm_dump_reader_H

127
src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.cc Normal file
View File

@ -0,0 +1,127 @@
/*!
* \file tracking_dump_reader.cc
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "tracking_dump_reader.h"
bool tracking_dump_reader::read_binary_obs()
{
try {
d_dump_file.read((char *) &abs_E, sizeof(float));
d_dump_file.read((char *) &abs_P, sizeof(float));
d_dump_file.read((char *) &abs_L, sizeof(float));
d_dump_file.read((char *) &prompt_I, sizeof(float));
d_dump_file.read((char *) &prompt_Q, sizeof(float));
d_dump_file.read((char *) &PRN_start_sample_count, sizeof(unsigned long int));
d_dump_file.read((char *) &acc_carrier_phase_rad, sizeof(double));
d_dump_file.read((char *) &carrier_doppler_hz, sizeof(double));
d_dump_file.read((char *) &code_freq_chips, sizeof(double));
d_dump_file.read((char *) &carr_error_hz, sizeof(double));
d_dump_file.read((char *) &carr_error_filt_hz, sizeof(double));
d_dump_file.read((char *) &code_error_chips, sizeof(double));
d_dump_file.read((char *) &code_error_filt_chips, sizeof(double));
d_dump_file.read((char *) &CN0_SNV_dB_Hz, sizeof(double));
d_dump_file.read((char *) &carrier_lock_test, sizeof(double));
d_dump_file.read((char *) &aux1, sizeof(double));
d_dump_file.read((char *) &aux2, sizeof(double));
}
catch (const std::ifstream::failure &e)
{
return false;
}
return true;
}
bool tracking_dump_reader::restart()
{
if (d_dump_file.is_open())
{
d_dump_file.clear();
d_dump_file.seekg(0, std::ios::beg);
return true;
}
else
{
return false;
}
}
long int tracking_dump_reader::num_epochs()
{
std::ifstream::pos_type size;
int number_of_double_vars=11;
int number_of_float_vars=5;
int epoch_size_bytes=sizeof(unsigned long int) +
sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars;
std::ifstream tmpfile( d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
if (tmpfile.is_open())
{
size = tmpfile.tellg();
long int nepoch = size / epoch_size_bytes;
return nepoch;
}
else
{
return 0;
}
}
bool tracking_dump_reader::open_obs_file(std::string out_file)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename = out_file;
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::in | std::ios::binary);
std::cout << "Tracking dump enabled, Log file: " << d_dump_filename.c_str() << std::endl;
return true;
}
catch (const std::ifstream::failure & e)
{
std::cout << "Problem opening Tracking dump Log file: " << d_dump_filename.c_str() << std::endl;
return false;
}
}else{
return false;
}
}
tracking_dump_reader::~tracking_dump_reader()
{
if (d_dump_file.is_open() == true)
{
d_dump_file.close();
}
}

89
src/tests/unit-tests/signal-processing-blocks/libs/tracking_dump_reader.h Normal file
View File

@ -0,0 +1,89 @@
/*!
* \file tracking_dump_reader.h
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_tracking_dump_reader_H
#define GNSS_SDR_tracking_dump_reader_H
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
class tracking_dump_reader {
public:
~tracking_dump_reader();
bool read_binary_obs();
bool restart();
long int num_epochs();
bool open_obs_file(std::string out_file);
//tracking dump variables
// EPR
float abs_E;
float abs_P;
float abs_L;
// PROMPT I and Q (to analyze navigation symbols)
float prompt_I;
float prompt_Q;
// PRN start sample stamp
unsigned long int PRN_start_sample_count;
// accumulated carrier phase
double acc_carrier_phase_rad;
// carrier and code frequency
double carrier_doppler_hz;
double code_freq_chips;
// PLL commands
double carr_error_hz;
double carr_error_filt_hz;
// DLL commands
double code_error_chips;
double code_error_filt_chips;
// CN0 and carrier lock test
double CN0_SNV_dB_Hz;
double carrier_lock_test;
// AUX vars (for debug purposes)
double aux1;
double aux2;
private:
std::string d_dump_filename;
std::ifstream d_dump_file;
};
#endif //GNSS_SDR_tracking_dump_reader_H

112
src/tests/unit-tests/signal-processing-blocks/libs/tracking_true_obs_reader.cc Normal file
View File

@ -0,0 +1,112 @@
/*!
* \file tracking_true_obs_reader.cc
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "tracking_true_obs_reader.h"
bool tracking_true_obs_reader::read_binary_obs()
{
try
{
d_dump_file.read((char *) &signal_timestamp_s, sizeof(double));
d_dump_file.read((char *) &acc_carrier_phase_cycles, sizeof(double));
d_dump_file.read((char *) &doppler_l1_hz, sizeof(double));
d_dump_file.read((char *) &prn_delay_chips, sizeof(double));
d_dump_file.read((char *) &tow, sizeof(double));
}
catch (const std::ifstream::failure &e)
{
return false;
}
return true;
}
bool tracking_true_obs_reader::restart()
{
if (d_dump_file.is_open())
{
d_dump_file.clear();
d_dump_file.seekg(0, std::ios::beg);
return true;
}
else
{
return false;
}
}
long int tracking_true_obs_reader::num_epochs()
{
std::ifstream::pos_type size;
int number_of_vars_in_epoch = 5;
int epoch_size_bytes = sizeof(double) * number_of_vars_in_epoch;
std::ifstream tmpfile( d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
if (tmpfile.is_open())
{
size = tmpfile.tellg();
long int nepoch = size / epoch_size_bytes;
return nepoch;
}
else
{
return 0;
}
}
bool tracking_true_obs_reader::open_obs_file(std::string out_file)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename = out_file;
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::in | std::ios::binary);
std::cout << "Observables dump enabled, Log file: " << d_dump_filename.c_str() << std::endl;
return true;
}
catch (const std::ifstream::failure & e)
{
std::cout << "Problem opening Observables dump Log file: " << d_dump_filename.c_str() << std::endl;
return false;
}
}
else
{
return false;
}
}
tracking_true_obs_reader::~tracking_true_obs_reader()
{
if (d_dump_file.is_open() == true)
{
d_dump_file.close();
}
}

62
src/tests/unit-tests/signal-processing-blocks/libs/tracking_true_obs_reader.h Normal file
View File

@ -0,0 +1,62 @@
/*!
* \file tracking_true_obs_reader.h
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_tracking_true_obs_reader_H
#define GNSS_SDR_tracking_true_obs_reader_H
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
class tracking_true_obs_reader {
public:
~tracking_true_obs_reader();
bool read_binary_obs();
bool restart();
long int num_epochs();
bool open_obs_file(std::string out_file);
bool d_dump;
double signal_timestamp_s;
double acc_carrier_phase_cycles;
double doppler_l1_hz;
double prn_delay_chips;
double tow;
private:
std::string d_dump_filename;
std::ifstream d_dump_file;
};
#endif //GNSS_SDR_tracking_true_obs_reader_H

482
src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc Normal file
View File

@ -0,0 +1,482 @@
/*!
* \file gps_l1_ca_dll_pll_tracking_test.cc
* \brief This class implements a tracking test for Galileo_E5a_DLL_PLL_Tracking
* implementation based on some input parameters.
* \author Javier Arribas, 2015. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <exception>
#include <cstring>
#include <ctime>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <sys/wait.h>
#include <unistd.h>
#include <armadillo>
#include <gnuradio/top_block.h>
#include <gnuradio/blocks/file_source.h>
#include <gnuradio/analog/sig_source_waveform.h>
#include <gnuradio/analog/sig_source_c.h>
#include <gnuradio/msg_queue.h>
#include <gnuradio/blocks/interleaved_char_to_complex.h>
#include <gnuradio/blocks/null_sink.h>
#include <gnuradio/blocks/skiphead.h>
#include <gtest/gtest.h>
#include "GPS_L1_CA.h"
#include "gnss_block_factory.h"
#include "gnss_block_interface.h"
#include "tracking_interface.h"
#include "telemetry_decoder_interface.h"
#include "in_memory_configuration.h"
#include "gnss_synchro.h"
#include "gps_l1_ca_telemetry_decoder.h"
#include "tracking_true_obs_reader.h"
#include "tracking_dump_reader.h"
#include "tlm_dump_reader.h"
#include "gps_l1_ca_dll_pll_tracking.h"
#include "gps_l1_ca_dll_pll_c_aid_tracking.h"
#include "signal_generator_flags.h"
DECLARE_string(generator_binary);
DECLARE_string(rinex_nav_file);
DECLARE_int32(duration); // 20
DECLARE_int32(fs_gen_hz);
DECLARE_string(static_position);
DECLARE_string(dynamic_position);
DECLARE_string(filename_rinex_obs);
DECLARE_string(filename_raw_data);
DECLARE_int32(test_satellite_PRN);
// ######## GNURADIO BLOCK MESSAGE RECEVER FOR TRACKING MESSAGES #########
class GpsL1CADllPllTelemetryDecoderTest_msg_rx;
typedef boost::shared_ptr<GpsL1CADllPllTelemetryDecoderTest_msg_rx> GpsL1CADllPllTelemetryDecoderTest_msg_rx_sptr;
GpsL1CADllPllTelemetryDecoderTest_msg_rx_sptr GpsL1CADllPllTelemetryDecoderTest_msg_rx_make();
class GpsL1CADllPllTelemetryDecoderTest_msg_rx : public gr::block
{
private:
friend GpsL1CADllPllTelemetryDecoderTest_msg_rx_sptr GpsL1CADllPllTelemetryDecoderTest_msg_rx_make();
void msg_handler_events(pmt::pmt_t msg);
GpsL1CADllPllTelemetryDecoderTest_msg_rx();
public:
int rx_message;
~GpsL1CADllPllTelemetryDecoderTest_msg_rx(); //!< Default destructor
};
GpsL1CADllPllTelemetryDecoderTest_msg_rx_sptr GpsL1CADllPllTelemetryDecoderTest_msg_rx_make()
{
return GpsL1CADllPllTelemetryDecoderTest_msg_rx_sptr(new GpsL1CADllPllTelemetryDecoderTest_msg_rx());
}
void GpsL1CADllPllTelemetryDecoderTest_msg_rx::msg_handler_events(pmt::pmt_t msg)
{
try
{
long int message = pmt::to_long(msg);
rx_message = message;
}
catch(boost::bad_any_cast& e)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
rx_message = 0;
}
}
GpsL1CADllPllTelemetryDecoderTest_msg_rx::GpsL1CADllPllTelemetryDecoderTest_msg_rx() :
gr::block("GpsL1CADllPllTelemetryDecoderTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
{
this->message_port_register_in(pmt::mp("events"));
this->set_msg_handler(pmt::mp("events"), boost::bind(&GpsL1CADllPllTelemetryDecoderTest_msg_rx::msg_handler_events, this, _1));
rx_message = 0;
}
GpsL1CADllPllTelemetryDecoderTest_msg_rx::~GpsL1CADllPllTelemetryDecoderTest_msg_rx()
{}
// ###########################################################
// ######## GNURADIO BLOCK MESSAGE RECEVER FOR TLM MESSAGES #########
class GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx;
typedef boost::shared_ptr<GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx> GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_sptr;
GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_sptr GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_make();
class GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx : public gr::block
{
private:
friend GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_sptr GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_make();
void msg_handler_events(pmt::pmt_t msg);
GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx();
public:
int rx_message;
~GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx(); //!< Default destructor
};
GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_sptr GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_make()
{
return GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_sptr(new GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx());
}
void GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx::msg_handler_events(pmt::pmt_t msg)
{
try
{
long int message = pmt::to_long(msg);
rx_message = message;
}
catch(boost::bad_any_cast& e)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
rx_message = 0;
}
}
GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx::GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx() :
gr::block("GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
{
this->message_port_register_in(pmt::mp("events"));
this->set_msg_handler(pmt::mp("events"), boost::bind(&GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx::msg_handler_events, this, _1));
rx_message = 0;
}
GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx::~GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx()
{}
// ###########################################################
class GpsL1CATelemetryDecoderTest: public ::testing::Test
{
public:
std::string generator_binary;
std::string p1;
std::string p2;
std::string p3;
std::string p4;
std::string p5;
const int baseband_sampling_freq = FLAGS_fs_gen_hz;
std::string filename_rinex_obs = FLAGS_filename_rinex_obs;
std::string filename_raw_data = FLAGS_filename_raw_data;
int configure_generator();
int generate_signal();
void check_results(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value);
GpsL1CATelemetryDecoderTest()
{
factory = std::make_shared<GNSSBlockFactory>();
config = std::make_shared<InMemoryConfiguration>();
item_size = sizeof(gr_complex);
gnss_synchro = Gnss_Synchro();
}
~GpsL1CATelemetryDecoderTest()
{}
void configure_receiver();
gr::msg_queue::sptr queue;
gr::top_block_sptr top_block;
std::shared_ptr<GNSSBlockFactory> factory;
std::shared_ptr<InMemoryConfiguration> config;
Gnss_Synchro gnss_synchro;
size_t item_size;
};
int GpsL1CATelemetryDecoderTest::configure_generator()
{
// Configure signal generator
generator_binary = FLAGS_generator_binary;
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);
}
else
{
p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position);
}
p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output
p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples
p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps]
return 0;
}
int GpsL1CATelemetryDecoderTest::generate_signal()
{
int child_status;
char *const parmList[] = { &generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL };
int pid;
if ((pid = fork()) == -1)
perror("fork err");
else if (pid == 0)
{
execv(&generator_binary[0], parmList);
std::cout << "Return not expected. Must be an execv err." << std::endl;
std::terminate();
}
waitpid(pid, &child_status, 0);
std::cout << "Signal and Observables RINEX and RAW files created." << std::endl;
return 0;
}
void GpsL1CATelemetryDecoderTest::configure_receiver()
{
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'G';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal, 2, 0);
gnss_synchro.PRN = FLAGS_test_satellite_PRN;
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(baseband_sampling_freq));
// Set Tracking
config->set_property("Tracking_1C.item_type", "gr_complex");
config->set_property("Tracking_1C.if", "0");
config->set_property("Tracking_1C.dump", "true");
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
config->set_property("Tracking_1C.pll_bw_hz", "20.0");
config->set_property("Tracking_1C.dll_bw_hz", "1.5");
config->set_property("Tracking_1C.early_late_space_chips", "0.5");
config->set_property("TelemetryDecoder_1C.dump","true");
config->set_property("TelemetryDecoder_1C.decimation_factor","1");
}
void GpsL1CATelemetryDecoderTest::check_results(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value)
{
//1. True value interpolation to match the measurement times
arma::vec true_value_interp;
arma::interp1(true_time_s,true_value,meas_time_s,true_value_interp);
//2. RMSE
arma::vec err;
err=meas_value-true_value_interp;
arma::vec err2=arma::square(err);
double rmse=sqrt(arma::mean(err2));
//3. Mean err and variance
double error_mean=arma::mean(err);
double error_var=arma::var(err);
// 4. Peaks
double max_error=arma::max(err);
double min_error=arma::min(err);
//5. report
std::cout<< std::setprecision(10)<<"TLM TOW RMSE="
<<rmse<<", mean="<<error_mean
<<", stdev="<<sqrt(error_var)<<" (max,min)="<<max_error<<","<<min_error<<" [Chips]"<<std::endl;
}
TEST_F(GpsL1CATelemetryDecoderTest, ValidationOfResults)
{
// Configure the signal generator
configure_generator();
// Generate signal raw signal samples and observations RINEX file
generate_signal();
struct timeval tv;
long long int begin = 0;
long long int end = 0;
configure_receiver();
//open true observables log file written by the simulator
tracking_true_obs_reader true_obs_data;
int test_satellite_PRN = FLAGS_test_satellite_PRN;
std::cout<<"Testing satellite PRN="<<test_satellite_PRN<<std::endl;
std::string true_obs_file=std::string("./gps_l1_ca_obs_prn");
true_obs_file.append(std::to_string(test_satellite_PRN));
true_obs_file.append(".dat");
ASSERT_NO_THROW({
if (true_obs_data.open_obs_file(true_obs_file)==false)
{
throw std::exception();
};
})<< "Failure opening true observables file" << std::endl;
queue = gr::msg_queue::make(0);
top_block = gr::make_top_block("Telemetry_Decoder test");
std::shared_ptr<TrackingInterface> tracking = std::make_shared<GpsL1CaDllPllTracking>(config.get(), "Tracking_1C", 1, 1);
//std::shared_ptr<TrackingInterface> tracking = std::make_shared<GpsL1CaDllPllCAidTracking>(config.get(), "Tracking_1C", 1, 1);
boost::shared_ptr<GpsL1CADllPllTelemetryDecoderTest_msg_rx> msg_rx = GpsL1CADllPllTelemetryDecoderTest_msg_rx_make();
// load acquisition data based on the first epoch of the true observations
ASSERT_NO_THROW({
if (true_obs_data.read_binary_obs()==false)
{
throw std::exception();
};
})<< "Failure reading true observables file" << std::endl;
//restart the epoch counter
true_obs_data.restart();
std::cout<<"Initial Doppler [Hz]="<<true_obs_data.doppler_l1_hz<<" Initial code delay [Chips]="<<true_obs_data.prn_delay_chips<<std::endl;
gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS-true_obs_data.prn_delay_chips/GPS_L1_CA_CODE_LENGTH_CHIPS)*baseband_sampling_freq*GPS_L1_CA_CODE_PERIOD;
gnss_synchro.Acq_doppler_hz = true_obs_data.doppler_l1_hz;
gnss_synchro.Acq_samplestamp_samples = 0;
std::shared_ptr<TelemetryDecoderInterface> tlm(new GpsL1CaTelemetryDecoder(config.get(), "TelemetryDecoder_1C",1, 1));
tlm->set_channel(0);
boost::shared_ptr<GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx> tlm_msg_rx = GpsL1CADllPllTelemetryDecoderTest_tlm_msg_rx_make();
ASSERT_NO_THROW( {
tracking->set_channel(gnss_synchro.Channel_ID);
}) << "Failure setting channel." << std::endl;
ASSERT_NO_THROW( {
tracking->set_gnss_synchro(&gnss_synchro);
}) << "Failure setting gnss_synchro." << std::endl;
ASSERT_NO_THROW( {
tracking->connect(top_block);
}) << "Failure connecting tracking to the top_block." << std::endl;
ASSERT_NO_THROW( {
std::string file = "./" + filename_raw_data;
const char * file_name = file.c_str();
gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false);
gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make();
gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro));
top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0);
top_block->connect(gr_interleaved_char_to_complex, 0, tracking->get_left_block(), 0);
top_block->connect(tracking->get_right_block(), 0, tlm->get_left_block(), 0);
top_block->connect(tlm->get_right_block(), 0, sink, 0);
top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events"));
}) << "Failure connecting the blocks." << std::endl;
tracking->start_tracking();
EXPECT_NO_THROW( {
gettimeofday(&tv, NULL);
begin = tv.tv_sec *1000000 + tv.tv_usec;
top_block->run(); // Start threads and wait
gettimeofday(&tv, NULL);
end = tv.tv_sec *1000000 + tv.tv_usec;
}) << "Failure running the top_block." << std::endl;
//check results
//load the true values
long int nepoch =true_obs_data.num_epochs();
std::cout<<"True observation epochs="<<nepoch<<std::endl;
arma::vec true_timestamp_s=arma::zeros(nepoch,1);
arma::vec true_acc_carrier_phase_cycles=arma::zeros(nepoch,1);
arma::vec true_Doppler_Hz=arma::zeros(nepoch,1);
arma::vec true_prn_delay_chips=arma::zeros(nepoch,1);
arma::vec true_tow_s=arma::zeros(nepoch,1);
long int epoch_counter=0;
while(true_obs_data.read_binary_obs())
{
true_timestamp_s(epoch_counter)=true_obs_data.signal_timestamp_s;
true_acc_carrier_phase_cycles(epoch_counter)=true_obs_data.acc_carrier_phase_cycles;
true_Doppler_Hz(epoch_counter)=true_obs_data.doppler_l1_hz;
true_prn_delay_chips(epoch_counter)=true_obs_data.prn_delay_chips;
true_tow_s(epoch_counter)=true_obs_data.tow;
epoch_counter++;
}
//load the measured values
tlm_dump_reader tlm_dump;
ASSERT_NO_THROW({
if (tlm_dump.open_obs_file(std::string("./telemetry0.dat"))==false)
{
throw std::exception();
};
})<< "Failure opening telemetry dump file" << std::endl;
nepoch =tlm_dump.num_epochs();
std::cout<<"Measured observation epochs="<<nepoch<<std::endl;
arma::vec tlm_timestamp_s=arma::zeros(nepoch,1);
arma::vec tlm_TOW_at_Preamble=arma::zeros(nepoch,1);
arma::vec tlm_tow_s=arma::zeros(nepoch,1);
epoch_counter=0;
while(tlm_dump.read_binary_obs())
{
tlm_timestamp_s(epoch_counter)=tlm_dump.Prn_timestamp_ms/1000.0;
tlm_TOW_at_Preamble(epoch_counter)=tlm_dump.d_TOW_at_Preamble;
tlm_tow_s(epoch_counter)=tlm_dump.TOW_at_current_symbol;
epoch_counter++;
}
//Cut measurement initial transitory of the measurements
arma::uvec initial_meas_point = arma::find(tlm_tow_s >= true_tow_s(0), 1, "first");
tlm_timestamp_s=tlm_timestamp_s.subvec(initial_meas_point(0),tlm_timestamp_s.size()-1);
tlm_tow_s=tlm_tow_s.subvec(initial_meas_point(0),tlm_tow_s.size()-1);
check_results(true_timestamp_s,true_tow_s,tlm_timestamp_s,tlm_tow_s);
std::cout << "Test completed in " << (end - begin) << " microseconds" << std::endl;
}

497
src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc Normal file
View File

@ -0,0 +1,497 @@
/*!
* \file gps_l1_ca_dll_pll_tracking_test.cc
* \brief This class implements a tracking test for Galileo_E5a_DLL_PLL_Tracking
* implementation based on some input parameters.
* \author Javier Arribas, 2015. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <ctime>
#include <iostream>
#include <unistd.h>
#include <armadillo>
#include <gnuradio/top_block.h>
#include <gnuradio/blocks/file_source.h>
#include <gnuradio/analog/sig_source_waveform.h>
#include <gnuradio/analog/sig_source_c.h>
#include <gnuradio/msg_queue.h>
#include <gnuradio/blocks/interleaved_char_to_complex.h>
#include <gnuradio/blocks/null_sink.h>
#include <gnuradio/blocks/skiphead.h>
#include <gtest/gtest.h>
#include "GPS_L1_CA.h"
#include "gnss_block_factory.h"
#include "gnss_block_interface.h"
#include "tracking_interface.h"
#include "in_memory_configuration.h"
#include "gnss_synchro.h"
#include "gps_l1_ca_dll_pll_tracking.h"
#include "tracking_true_obs_reader.h"
#include "tracking_dump_reader.h"
#include "signal_generator_flags.h"
DECLARE_string(generator_binary);
DECLARE_string(rinex_nav_file);
DECLARE_int32(duration);
DECLARE_int32(fs_gen_hz);
DECLARE_string(static_position);
DECLARE_string(dynamic_position);
DECLARE_string(filename_rinex_obs);
DECLARE_string(filename_raw_data);
DECLARE_int32(test_satellite_PRN);
// ######## GNURADIO BLOCK MESSAGE RECEVER #########
class GpsL1CADllPllTrackingTest_msg_rx;
typedef boost::shared_ptr<GpsL1CADllPllTrackingTest_msg_rx> GpsL1CADllPllTrackingTest_msg_rx_sptr;
GpsL1CADllPllTrackingTest_msg_rx_sptr GpsL1CADllPllTrackingTest_msg_rx_make();
class GpsL1CADllPllTrackingTest_msg_rx : public gr::block
{
private:
friend GpsL1CADllPllTrackingTest_msg_rx_sptr GpsL1CADllPllTrackingTest_msg_rx_make();
void msg_handler_events(pmt::pmt_t msg);
GpsL1CADllPllTrackingTest_msg_rx();
public:
int rx_message;
~GpsL1CADllPllTrackingTest_msg_rx(); //!< Default destructor
};
GpsL1CADllPllTrackingTest_msg_rx_sptr GpsL1CADllPllTrackingTest_msg_rx_make()
{
return GpsL1CADllPllTrackingTest_msg_rx_sptr(new GpsL1CADllPllTrackingTest_msg_rx());
}
void GpsL1CADllPllTrackingTest_msg_rx::msg_handler_events(pmt::pmt_t msg)
{
try
{
long int message = pmt::to_long(msg);
rx_message = message;
}
catch(boost::bad_any_cast& e)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
rx_message = 0;
}
}
GpsL1CADllPllTrackingTest_msg_rx::GpsL1CADllPllTrackingTest_msg_rx() :
gr::block("GpsL1CADllPllTrackingTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
{
this->message_port_register_in(pmt::mp("events"));
this->set_msg_handler(pmt::mp("events"), boost::bind(&GpsL1CADllPllTrackingTest_msg_rx::msg_handler_events, this, _1));
rx_message = 0;
}
GpsL1CADllPllTrackingTest_msg_rx::~GpsL1CADllPllTrackingTest_msg_rx()
{}
// ###########################################################
class GpsL1CADllPllTrackingTest: public ::testing::Test
{
public:
std::string generator_binary;
std::string p1;
std::string p2;
std::string p3;
std::string p4;
std::string p5;
const int baseband_sampling_freq = FLAGS_fs_gen_hz;
std::string filename_rinex_obs = FLAGS_filename_rinex_obs;
std::string filename_raw_data = FLAGS_filename_raw_data;
int configure_generator();
int generate_signal();
void check_results_doppler(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value);
void check_results_acc_carrier_phase(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value);
void check_results_codephase(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value);
GpsL1CADllPllTrackingTest()
{
factory = std::make_shared<GNSSBlockFactory>();
config = std::make_shared<InMemoryConfiguration>();
item_size = sizeof(gr_complex);
gnss_synchro = Gnss_Synchro();
}
~GpsL1CADllPllTrackingTest()
{}
void configure_receiver();
gr::msg_queue::sptr queue;
gr::top_block_sptr top_block;
std::shared_ptr<GNSSBlockFactory> factory;
std::shared_ptr<InMemoryConfiguration> config;
Gnss_Synchro gnss_synchro;
size_t item_size;
};
int GpsL1CADllPllTrackingTest::configure_generator()
{
// Configure signal generator
generator_binary = FLAGS_generator_binary;
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);
}
else
{
p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position);
}
p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output
p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples
p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps]
return 0;
}
int GpsL1CADllPllTrackingTest::generate_signal()
{
int child_status;
char *const parmList[] = { &generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL };
int pid;
if ((pid = fork()) == -1)
perror("fork err");
else if (pid == 0)
{
execv(&generator_binary[0], parmList);
std::cout << "Return not expected. Must be an execv err." << std::endl;
std::terminate();
}
waitpid(pid, &child_status, 0);
std::cout << "Signal and Observables RINEX and RAW files created." << std::endl;
return 0;
}
void GpsL1CADllPllTrackingTest::configure_receiver()
{
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'G';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal, 2, 0);
gnss_synchro.PRN = FLAGS_test_satellite_PRN;
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(baseband_sampling_freq));
// Set Tracking
config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking");
config->set_property("Tracking_1C.item_type", "gr_complex");
config->set_property("Tracking_1C.if", "0");
config->set_property("Tracking_1C.dump", "true");
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
config->set_property("Tracking_1C.pll_bw_hz", "30.0");
config->set_property("Tracking_1C.dll_bw_hz", "2.0");
config->set_property("Tracking_1C.early_late_space_chips", "0.5");
}
void GpsL1CADllPllTrackingTest::check_results_doppler(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value)
{
//1. True value interpolation to match the measurement times
arma::vec true_value_interp;
arma::interp1(true_time_s,true_value,meas_time_s,true_value_interp);
//2. RMSE
arma::vec err;
err=meas_value-true_value_interp;
arma::vec err2=arma::square(err);
double rmse=sqrt(arma::mean(err2));
//3. Mean err and variance
double error_mean=arma::mean(err);
double error_var=arma::var(err);
// 5. Peaks
double max_error=arma::max(err);
double min_error=arma::min(err);
//5. report
std::cout<< std::setprecision(10)<<"TRK Doppler RMSE="<<rmse
<<", mean="<<error_mean
<<", stdev="<<sqrt(error_var)<<" (max,min)="<<max_error<<","<<min_error<<" [Hz]"<<std::endl;
}
void GpsL1CADllPllTrackingTest::check_results_acc_carrier_phase(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value)
{
//1. True value interpolation to match the measurement times
arma::vec true_value_interp;
arma::interp1(true_time_s,true_value,meas_time_s,true_value_interp);
//2. RMSE
arma::vec err;
err=meas_value-true_value_interp;
arma::vec err2=arma::square(err);
double rmse=sqrt(arma::mean(err2));
//3. Mean err and variance
double error_mean=arma::mean(err);
double error_var=arma::var(err);
// 4. Peaks
double max_error=arma::max(err);
double min_error=arma::min(err);
//5. report
std::cout<< std::setprecision(10)<<"TRK acc carrier phase RMSE="<<rmse
<<", mean="<<error_mean
<<", stdev="<<sqrt(error_var)<<" (max,min)="<<max_error<<","<<min_error<<" [Hz]"<<std::endl;
}
void GpsL1CADllPllTrackingTest::check_results_codephase(arma::vec true_time_s,
arma::vec true_value,
arma::vec meas_time_s,
arma::vec meas_value)
{
//1. True value interpolation to match the measurement times
arma::vec true_value_interp;
arma::interp1(true_time_s,true_value,meas_time_s,true_value_interp);
//2. RMSE
arma::vec err;
err=meas_value-true_value_interp;
arma::vec err2=arma::square(err);
double rmse=sqrt(arma::mean(err2));
//3. Mean err and variance
double error_mean=arma::mean(err);
double error_var=arma::var(err);
// 4. Peaks
double max_error=arma::max(err);
double min_error=arma::min(err);
//5. report
std::cout<< std::setprecision(10)<<"TRK code phase RMSE="<<rmse
<<", mean="<<error_mean
<<", stdev="<<sqrt(error_var)<<" (max,min)="<<max_error<<","<<min_error<<" [Chips]"<<std::endl;
}
TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
{
// Configure the signal generator
configure_generator();
// Generate signal raw signal samples and observations RINEX file
generate_signal();
struct timeval tv;
long long int begin = 0;
long long int end = 0;
configure_receiver();
//open true observables log file written by the simulator
tracking_true_obs_reader true_obs_data;
int test_satellite_PRN = FLAGS_test_satellite_PRN;
std::cout<<"Testing satellite PRN="<<test_satellite_PRN<<std::endl;
std::string true_obs_file=std::string("./gps_l1_ca_obs_prn");
true_obs_file.append(std::to_string(test_satellite_PRN));
true_obs_file.append(".dat");
ASSERT_NO_THROW({
if (true_obs_data.open_obs_file(true_obs_file)==false)
{
throw std::exception();
};
})<< "Failure opening true observables file" << std::endl;
queue = gr::msg_queue::make(0);
top_block = gr::make_top_block("Tracking test");
std::shared_ptr<TrackingInterface> tracking = std::make_shared<GpsL1CaDllPllTracking>(config.get(), "Tracking_1C", 1, 1);
//std::shared_ptr<TrackingInterface> tracking = std::make_shared<GpsL1CaDllPllCAidTracking>(config.get(), "Tracking_1C", 1, 1);
boost::shared_ptr<GpsL1CADllPllTrackingTest_msg_rx> msg_rx = GpsL1CADllPllTrackingTest_msg_rx_make();
// load acquisition data based on the first epoch of the true observations
ASSERT_NO_THROW({
if (true_obs_data.read_binary_obs()==false)
{
throw std::exception();
};
})<< "Failure reading true observables file" << std::endl;
//restart the epoch counter
true_obs_data.restart();
std::cout<<"Initial Doppler [Hz]="<<true_obs_data.doppler_l1_hz<<" Initial code delay [Chips]="<<true_obs_data.prn_delay_chips<<std::endl;
gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS-true_obs_data.prn_delay_chips/GPS_L1_CA_CODE_LENGTH_CHIPS)*baseband_sampling_freq*GPS_L1_CA_CODE_PERIOD;
gnss_synchro.Acq_doppler_hz = true_obs_data.doppler_l1_hz;
gnss_synchro.Acq_samplestamp_samples = 0;
ASSERT_NO_THROW( {
tracking->set_channel(gnss_synchro.Channel_ID);
}) << "Failure setting channel." << std::endl;
ASSERT_NO_THROW( {
tracking->set_gnss_synchro(&gnss_synchro);
}) << "Failure setting gnss_synchro." << std::endl;
ASSERT_NO_THROW( {
tracking->connect(top_block);
}) << "Failure connecting tracking to the top_block." << std::endl;
ASSERT_NO_THROW( {
std::string file = "./" + filename_raw_data;
const char * file_name = file.c_str();
gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false);
gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make();
gr::blocks::null_sink::sptr sink = gr::blocks::null_sink::make(sizeof(Gnss_Synchro));
top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0);
top_block->connect(gr_interleaved_char_to_complex, 0, tracking->get_left_block(), 0);
top_block->connect(tracking->get_right_block(), 0, sink, 0);
top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events"));
}) << "Failure connecting the blocks of tracking test." << std::endl;
tracking->start_tracking();
EXPECT_NO_THROW( {
gettimeofday(&tv, NULL);
begin = tv.tv_sec *1000000 + tv.tv_usec;
top_block->run(); // Start threads and wait
gettimeofday(&tv, NULL);
end = tv.tv_sec *1000000 + tv.tv_usec;
}) << "Failure running the top_block." << std::endl;
//check results
//load the true values
long int nepoch =true_obs_data.num_epochs();
std::cout<<"True observation epochs="<<nepoch<<std::endl;
arma::vec true_timestamp_s=arma::zeros(nepoch,1);
arma::vec true_acc_carrier_phase_cycles=arma::zeros(nepoch,1);
arma::vec true_Doppler_Hz=arma::zeros(nepoch,1);
arma::vec true_prn_delay_chips=arma::zeros(nepoch,1);
arma::vec true_tow_s=arma::zeros(nepoch,1);
long int epoch_counter=0;
while(true_obs_data.read_binary_obs())
{
true_timestamp_s(epoch_counter)=true_obs_data.signal_timestamp_s;
true_acc_carrier_phase_cycles(epoch_counter)=true_obs_data.acc_carrier_phase_cycles;
true_Doppler_Hz(epoch_counter)=true_obs_data.doppler_l1_hz;
true_prn_delay_chips(epoch_counter)=true_obs_data.prn_delay_chips;
true_tow_s(epoch_counter)=true_obs_data.tow;
epoch_counter++;
}
//load the measured values
tracking_dump_reader trk_dump;
ASSERT_NO_THROW({
if (trk_dump.open_obs_file(std::string("./tracking_ch_0.dat"))==false)
{
throw std::exception();
};
})<< "Failure opening tracking dump file" << std::endl;
nepoch =trk_dump.num_epochs();
std::cout<<"Measured observation epochs="<<nepoch<<std::endl;
arma::vec trk_timestamp_s=arma::zeros(nepoch,1);
arma::vec trk_acc_carrier_phase_cycles=arma::zeros(nepoch,1);
arma::vec trk_Doppler_Hz=arma::zeros(nepoch,1);
arma::vec trk_prn_delay_chips=arma::zeros(nepoch,1);
epoch_counter=0;
while(trk_dump.read_binary_obs())
{
trk_timestamp_s(epoch_counter)=static_cast<double>(trk_dump.PRN_start_sample_count)/static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter)=trk_dump.acc_carrier_phase_rad/GPS_TWO_PI;
trk_Doppler_Hz(epoch_counter)=trk_dump.carrier_doppler_hz;
double delay_chips=GPS_L1_CA_CODE_LENGTH_CHIPS
-GPS_L1_CA_CODE_LENGTH_CHIPS
*(fmod((static_cast<double>(trk_dump.PRN_start_sample_count)+trk_dump.aux1)/static_cast<double>(baseband_sampling_freq),1.0e-3)/1.0e-3);
trk_prn_delay_chips(epoch_counter)=delay_chips;
epoch_counter++;
}
//Align initial measurements and cut the tracking pull-in transitory
double pull_in_offset_s=1.0;
arma::uvec initial_meas_point = arma::find(trk_timestamp_s >= (true_timestamp_s(0)+pull_in_offset_s), 1, "first");
trk_timestamp_s=trk_timestamp_s.subvec(initial_meas_point(0),trk_timestamp_s.size()-1);
trk_acc_carrier_phase_cycles=trk_acc_carrier_phase_cycles.subvec(initial_meas_point(0),trk_acc_carrier_phase_cycles.size()-1);
trk_Doppler_Hz=trk_Doppler_Hz.subvec(initial_meas_point(0),trk_Doppler_Hz.size()-1);
trk_prn_delay_chips=trk_prn_delay_chips.subvec(initial_meas_point(0),trk_prn_delay_chips.size()-1);
check_results_doppler(true_timestamp_s,true_Doppler_Hz,trk_timestamp_s,trk_Doppler_Hz);
check_results_codephase(true_timestamp_s,true_prn_delay_chips,trk_timestamp_s,trk_prn_delay_chips);
check_results_acc_carrier_phase(true_timestamp_s,true_acc_carrier_phase_cycles,trk_timestamp_s,trk_acc_carrier_phase_cycles);
std::cout << "Signal tracking completed in " << (end - begin) << " microseconds" << std::endl;
}

2
src/tests/unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc
View File

@ -181,7 +181,7 @@ TEST_F(GpsL2MDllPllTrackingTest, ValidationOfResults)
ASSERT_NO_THROW( {
//gr::analog::sig_source_c::sptr source = gr::analog::sig_source_c::make(fs_in, gr::analog::GR_SIN_WAVE, 1000, 1, gr_complex(0));
std::string path = std::string(TEST_PATH);
std::string file = path + "/data/gps_l2c_m_prn7_5msps.dat";
std::string file = path + "signal_samples/gps_l2c_m_prn7_5msps.dat";
const char * file_name = file.c_str();
gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(gr_complex), file_name, false);
boost::shared_ptr<gr::block> valve = gnss_sdr_make_valve(sizeof(gr_complex), nsamples, queue);