mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-13 03:30:33 +00:00
Code Issues Releases Wiki Activity

Now the Rinex_Printer prints all the satellites' nav data available in the channels.

Browse Source 
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@121 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez 2012-01-12 00:21:29 +00:00
parent bc62d8d5be
commit 76b33f232e
8 changed files with 328 additions and 356 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -77,6 +77,11 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
b_rinex_header_writen = false;
rp = new Rinex_Printer();
for (unsigned int i=0; i<nchannels; i++)
{
nav_data_map[i] = gps_navigation_message();
}
}
@ -139,6 +144,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
<< ")" << std::endl;
d_last_nav_msg = nav_msg;
d_ls_pvt->d_ephemeris[nav_msg.i_channel_ID] = nav_msg;
nav_data_map[nav_msg.i_channel_ID] = nav_msg;
// **** update pseudoranges clock ****
if (nav_msg.i_satellite_PRN == gnss_pseudoranges_iter->second.SV_ID)
@ -166,9 +172,9 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
rp->rinex_obs_header(rp->obsFile, d_last_nav_msg);
b_rinex_header_writen = true; // do not write header anymore
}
if(b_rinex_header_writen) // Put here another condition to separate annotations
if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s)
{
rp->log_rinex_nav(rp->navFile, d_last_nav_msg);
rp->log_rinex_nav(rp->navFile, nav_data_map);
rp->log_rinex_obs(rp->obsFile, d_last_nav_msg, pseudoranges);
}
}

1
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h
View File

@ -87,6 +87,7 @@ private:
double d_ephemeris_timestamp_ms;
gps_l1_ca_ls_pvt *d_ls_pvt;
std::map<int,gps_navigation_message> nav_data_map;
public:

503
src/algorithms/PVT/libs/rinex_printer.cc
View File

@ -498,286 +498,297 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, gps_navigation_message
void Rinex_Printer::log_rinex_nav(std::ofstream& out, gps_navigation_message nav_msg)
void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,gps_navigation_message> nav_msg)
{
// this has to be done for all satellites!
std::string line;
boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_time(nav_msg);
std::string timestring=boost::posix_time::to_iso_string(p_utc_time);
std::string month (timestring, 4, 2);
std::string day (timestring, 6, 2);
std::string hour (timestring, 9, 2);
std::string minutes (timestring, 11, 2);
std::string seconds (timestring, 13, 2);
if (version == 2)
for (int i=0; i < (int)nav_msg.size(); i++)
{
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg.i_satellite_PRN), 2);
line += std::string(1, ' ');
std::string year (timestring, 2, 2);
line += year;
line += std::string(1, ' ');
line += month;
line += std::string(1, ' ');
line += day;
line += std::string(1, ' ');
line += hour;
line += std::string(1, ' ');
line += minutes;
line += std::string(1, ' ');
line += seconds;
line += std::string(1, '.');
std::string decimal = std::string("0");
if (timestring.size() > 16)
// -------- SV / EPOCH / SV CLK
boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_time(nav_msg[i]);
std::string timestring = boost::posix_time::to_iso_string(p_utc_time);
std::string month (timestring, 4, 2);
std::string day (timestring, 6, 2);
std::string hour (timestring, 9, 2);
std::string minutes (timestring, 11, 2);
std::string seconds (timestring, 13, 2);
if (version == 2)
{
std::string decimal (timestring, 16, 1);
line += Rinex_Printer::rightJustify(boost::lexical_cast<std::string>(nav_msg[i].i_satellite_PRN), 2);
line += std::string(1, ' ');
std::string year (timestring, 2, 2);
line += year;
line += std::string(1, ' ');
line += month;
line += std::string(1, ' ');
line += day;
line += std::string(1, ' ');
line += hour;
line += std::string(1, ' ');
line += minutes;
line += std::string(1, ' ');
line += seconds;
line += std::string(1, '.');
std::string decimal = std::string("0");
if (timestring.size() > 16)
{
std::string decimal (timestring, 16, 1);
}
line += decimal;
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
line += std::string(1, ' ');
}
if (version == 3)
{
line += satelliteSystem["GPS"];
if (nav_msg[i].i_satellite_PRN < 10) line += std::string("0");
line += boost::lexical_cast<std::string>(nav_msg[i].i_satellite_PRN);
std::string year (timestring, 0, 4);
line += std::string(1, ' ');
line += year;
line += std::string(1, ' ');
line += month;
line += std::string(1, ' ');
line += day;
line += std::string(1, ' ');
line += hour;
line += std::string(1, ' ');
line += minutes;
line += std::string(1, ' ');
line += seconds;
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg[i].d_A_f0, 18, 2);
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
// -------- BROADCAST ORBIT - 1
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
if (nav_msg[i].d_IODE_SF2 == nav_msg[i].d_IODE_SF3)
{
line += Rinex_Printer::doub2for(nav_msg[i].d_IODE_SF2, 18, 2);
}
else
{
LOG_AT_LEVEL(ERROR) << "Discontinued reception of Frame 2 and 3 " << std::endl;
}
line += decimal;
line += std::string(1, ' ');
line += doub2for(nav_msg.d_A_f0, 18, 2);
line += Rinex_Printer::doub2for(nav_msg[i].d_Crs, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_A_f0, 18, 2);
line += Rinex_Printer::doub2for(nav_msg[i].d_Delta_n, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_A_f0, 18, 2);
line += Rinex_Printer::doub2for(nav_msg[i].d_M_0, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
// -------- BROADCAST ORBIT - 2
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_Cuc, 18, 2);
line += std::string(1, ' ');
}
if (version == 3)
{
line += satelliteSystem["GPS"];
if (nav_msg.i_satellite_PRN < 10) line += std::string("0");
line += boost::lexical_cast<std::string>(nav_msg.i_satellite_PRN);
std::string year (timestring, 0, 4);
line += Rinex_Printer::doub2for(nav_msg[i].d_e_eccentricity, 18, 2);
line += std::string(1, ' ');
line += year;
line += Rinex_Printer::doub2for(nav_msg[i].d_Cus, 18, 2);
line += std::string(1, ' ');
line += month;
line += Rinex_Printer::doub2for(nav_msg[i].d_sqrt_A, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
// -------- BROADCAST ORBIT - 3
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_Toe, 18, 2);
line += std::string(1, ' ');
line += day;
line += Rinex_Printer::doub2for(nav_msg[i].d_Cic, 18, 2);
line += std::string(1, ' ');
line += hour;
line += Rinex_Printer::doub2for(nav_msg[i].d_OMEGA0, 18, 2);
line += std::string(1, ' ');
line += minutes;
line += Rinex_Printer::doub2for(nav_msg[i].d_Cis, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
// -------- BROADCAST ORBIT - 4
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_i_0, 18, 2);
line += std::string(1, ' ');
line += seconds;
line += Rinex_Printer::doub2for(nav_msg[i].d_Crc, 18, 2);
line += std::string(1, ' ');
line += doub2for(nav_msg.d_A_f0, 18, 2);
line += Rinex_Printer::doub2for(nav_msg[i].d_OMEGA, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_A_f0, 18, 2);
line += Rinex_Printer::doub2for(nav_msg[i].d_OMEGA_DOT, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
// -------- BROADCAST ORBIT - 5
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_IDOT, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_A_f0, 18, 2);
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
if (nav_msg.d_IODE_SF2 == nav_msg.d_IODE_SF3)
{
line += Rinex_Printer::doub2for(nav_msg.d_IODE_SF2, 18, 2);
}
else
{
LOG_AT_LEVEL(ERROR) << "Discontinued reception of Frame 2 and 3 " << std::endl;
}
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_Crs, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_Delta_n, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_M_0, 18, 2);
if (version == 2)
{
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_code_on_L2), 18, 2);
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg.d_Cuc, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_e_eccentricity, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_Cus, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_sqrt_A, 18, 2);
if (version == 2)
{
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_code_on_L2), 18, 2);
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
double GPS_week_continuous_number = (double)(nav_msg[i].i_GPS_week + 1024); // valid until April 7, 2019 (check http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm)
line += Rinex_Printer::doub2for(GPS_week_continuous_number, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg.d_Toe, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_Cic, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_OMEGA0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_Cis, 18, 2);
if (version == 2)
{
// -------- BROADCAST ORBIT - 6
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_SV_accuracy), 18, 2);
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg.d_i_0, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_Crc, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_OMEGA, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_OMEGA_DOT, 18, 2);
if (version == 2)
{
line += Rinex_Printer::doub2for((double)(nav_msg[i].i_SV_health), 18, 2);
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg.d_IDOT, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(nav_msg.i_code_on_L2), 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(nav_msg.i_code_on_L2), 18, 2);
line += std::string(1, ' ');
double GPS_week_continuous_number = (double)(nav_msg.i_GPS_week + 1024); // valid until April 7, 2019 (check http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm)
line += Rinex_Printer::doub2for(GPS_week_continuous_number, 18, 2);
if (version == 2)
{
line += Rinex_Printer::doub2for(nav_msg[i].d_TGD, 18, 2);
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line += Rinex_Printer::doub2for(nav_msg[i].d_IODC, 18, 2);
if (version == 2)
{
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for((double)(nav_msg.i_SV_accuracy), 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(nav_msg.i_SV_health), 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_TGD, 18, 2);
line += std::string(1, ' ');
line += Rinex_Printer::doub2for(nav_msg.d_IODC, 18, 2);
if (version == 2)
{
// -------- BROADCAST ORBIT - 7
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg[i].d_TOW, 18, 2);
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
double curve_fit_interval = 4;
if (nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIA"))
{
// Block II/IIA (Table 20-XI IS-GPS-200E )
if ( (nav_msg[i].d_IODC > 239) && (nav_msg[i].d_IODC < 248) ) curve_fit_interval = 8;
if ( ( (nav_msg[i].d_IODC > 247) && (nav_msg[i].d_IODC < 256) ) || (nav_msg[i].d_IODC == 496) ) curve_fit_interval = 14;
if ( (nav_msg[i].d_IODC > 496) && (nav_msg[i].d_IODC < 504) ) curve_fit_interval = 26;
if ( (nav_msg[i].d_IODC > 503) && (nav_msg[i].d_IODC < 511) ) curve_fit_interval = 50;
if ( ( (nav_msg[i].d_IODC > 751) && (nav_msg[i].d_IODC < 757) ) || (nav_msg[i].d_IODC == 511) ) curve_fit_interval = 74;
if ( (nav_msg[i].d_IODC == 757)) curve_fit_interval = 98;
}
line.clear();
if (version == 2)
{
line += std::string(4, ' ');
}
if (version == 3)
{
line += std::string(5, ' ');
}
line += Rinex_Printer::doub2for(nav_msg.d_TOW, 18, 2);
line += std::string(1, ' ');
double curve_fit_interval = 4;
if (nav_msg.satelliteBlock[nav_msg.i_satellite_PRN].compare("IIA"))
{
// Block II/IIA (Table 20-XI IS-GPS-200E )
if ( (nav_msg.d_IODC > 239) && (nav_msg.d_IODC < 248) ) curve_fit_interval = 8;
if ( ( (nav_msg.d_IODC > 247) && (nav_msg.d_IODC < 256) ) || (nav_msg.d_IODC == 496) ) curve_fit_interval = 14;
if ( (nav_msg.d_IODC > 496) && (nav_msg.d_IODC < 504) ) curve_fit_interval = 26;
if ( (nav_msg.d_IODC > 503) && (nav_msg.d_IODC < 511) ) curve_fit_interval = 50;
if ( ( (nav_msg.d_IODC > 751) && (nav_msg.d_IODC < 757) ) || (nav_msg.d_IODC == 511) ) curve_fit_interval = 74;
if ( (nav_msg.d_IODC == 757)) curve_fit_interval = 98;
}
if ((nav_msg.satelliteBlock[nav_msg.i_satellite_PRN].compare("IIR") == 0) ||
(nav_msg.satelliteBlock[nav_msg.i_satellite_PRN].compare("IIR-M") == 0) ||
(nav_msg.satelliteBlock[nav_msg.i_satellite_PRN].compare("IIF") == 0) ||
(nav_msg.satelliteBlock[nav_msg.i_satellite_PRN].compare("IIIA") == 0) )
{
// Block IIR/IIR-M/IIF/IIIA (Table 20-XII IS-GPS-200E )
if ( (nav_msg.d_IODC > 239) && (nav_msg.d_IODC < 248)) curve_fit_interval = 8;
if ( ( (nav_msg.d_IODC > 247) && (nav_msg.d_IODC < 256)) || (nav_msg.d_IODC == 496) ) curve_fit_interval = 14;
if ( ( (nav_msg.d_IODC > 496) && (nav_msg.d_IODC < 504)) || ( (nav_msg.d_IODC > 1020) && (nav_msg.d_IODC < 1024) ) ) curve_fit_interval = 26;
}
line += Rinex_Printer::doub2for(curve_fit_interval, 18, 2);
line += std::string(1, ' ');
line += std::string(18, ' '); // spare
line += std::string(1, ' ');
line += std::string(18, ' '); // spare
if (version == 2)
{
if ((nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIR") == 0) ||
(nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIR-M") == 0) ||
(nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIF") == 0) ||
(nav_msg[i].satelliteBlock[nav_msg[i].i_satellite_PRN].compare("IIIA") == 0) )
{
// Block IIR/IIR-M/IIF/IIIA (Table 20-XII IS-GPS-200E )
if ( (nav_msg[i].d_IODC > 239) && (nav_msg[i].d_IODC < 248)) curve_fit_interval = 8;
if ( ( (nav_msg[i].d_IODC > 247) && (nav_msg[i].d_IODC < 256)) || (nav_msg[i].d_IODC == 496) ) curve_fit_interval = 14;
if ( ( (nav_msg[i].d_IODC > 496) && (nav_msg[i].d_IODC < 504)) || ( (nav_msg[i].d_IODC > 1020) && (nav_msg[i].d_IODC < 1024) ) ) curve_fit_interval = 26;
}
line += Rinex_Printer::doub2for(curve_fit_interval, 18, 2);
line += std::string(1, ' ');
line += std::string(18, ' '); // spare
line += std::string(1, ' ');
line += std::string(18, ' '); // spare
if (version == 2)
{
line += std::string(1, ' ');
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
line.clear();
}
Rinex_Printer::lengthCheck(line);
out << line << std::endl;
}
void Rinex_Printer::rinex_obs_header(std::ofstream& out, gps_navigation_message nav_msg)
{

2
src/algorithms/PVT/libs/rinex_printer.h
View File

@ -100,7 +100,7 @@ public:
/*!
* \brief Writes data from the navigation message into the RINEX file
*/
void log_rinex_nav(std::ofstream& out, gps_navigation_message nav_msg);
void log_rinex_nav(std::ofstream& out, std::map<int,gps_navigation_message> nav_msg);
/*!
* \brief Writes observables into the RINEX file

16
src/algorithms/observables/adapters/gps_l1_ca_observables.cc
View File

@ -53,7 +53,6 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
out_streams_(out_streams),
queue_(queue)
{
int output_rate_ms;
output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
@ -68,7 +67,7 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
fs_in_ = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
observables_ = gps_l1_ca_make_observables_cc(in_streams_, queue_, dump_,dump_filename_,output_rate_ms,flag_averaging);
observables_ = gps_l1_ca_make_observables_cc(in_streams_, queue_, dump_, dump_filename_, output_rate_ms, flag_averaging);
observables_->set_fs_in(fs_in_);
DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")";
@ -77,30 +76,41 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
observables_->set_navigation_queue(&global_gps_nav_msg_queue);
DLOG(INFO) << "global navigation message queue assigned to observables ("<< observables_->unique_id() << ")";
}
GpsL1CaObservables::~GpsL1CaObservables()
{}
void GpsL1CaObservables::connect(gr_top_block_sptr top_block)
{
// Nothing to connect internally
DLOG(INFO) << "nothing to connect internally";
}
void GpsL1CaObservables::disconnect(gr_top_block_sptr top_block)
{
// Nothing to disconnect
}
gr_basic_block_sptr GpsL1CaObservables::get_left_block()
{
return observables_;
}
gr_basic_block_sptr GpsL1CaObservables::get_right_block()
{
return observables_;

3
src/algorithms/observables/adapters/gps_l1_ca_observables.h
View File

@ -70,7 +70,8 @@ public:
{
return;
};
// all blocks must have an intem_size() function implementation
//!< All blocks must have an item_size() function implementation
size_t item_size()
{
return sizeof(gr_complex);

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

@ -71,7 +71,8 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
{
if (d_dump_file.is_open() == false)
{
try {
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);
std::cout << "Observables dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
@ -81,25 +82,40 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
}
}
}
}
gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
{
d_dump_file.close();
delete[] d_history_prn_delay_ms;
}
bool pairCompare_gnss_synchro( std::pair<int,gnss_synchro> a, std::pair<int,gnss_synchro> b)
{
return (a.second.preamble_delay_ms) < (b.second.preamble_delay_ms);
}
bool pairCompare_double( std::pair<int,double> a, std::pair<int,double> b)
{
return (a.second) < (b.second);
}
void clearQueue( std::deque<double> &q )
{
std::deque<double> empty;
@ -107,6 +123,9 @@ void clearQueue( std::deque<double> &q )
}
int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
@ -117,22 +136,20 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
std::map<int,gnss_synchro> gps_words;
std::map<int,gnss_synchro>::iterator gps_words_iter;
std::map<int,double>::iterator current_prn_timestamps_ms_iter;
std::map<int,double> current_prn_timestamps_ms;
double min_preamble_delay_ms;
double max_preamble_delay_ms;
double pseudoranges_timestamp_ms;
double traveltime_ms;
double pseudorange_m;
int history_shift = 0;
double delta_timestamp_ms;
double min_delta_timestamp_ms;
double actual_min_prn_delay_ms;
double current_prn_delay_ms;
int history_shift = 0;
int pseudoranges_reference_sat_ID = 0;
unsigned int pseudoranges_reference_sat_channel_ID = 0;
@ -251,9 +268,11 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
}
}
if(d_dump == true) {
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try {
try
{
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
@ -271,9 +290,10 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
}
catch (std::ifstream::failure e)
{
std::cout << "Exception writing observables dump file "<< e.what() << std::endl;
std::cout << "Exception writing observables dump file " << e.what() << std::endl;
}
}
}
consume_each(1); //one by one
if ((d_sample_counter % d_output_rate_ms) == 0)

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

@ -333,17 +333,17 @@ void gps_navigation_message::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*GPS_PI), (2*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*GPS_PI);
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
@ -357,13 +357,13 @@ void gps_navigation_message::satellitePosition(double transmitTime)
// Compute the true anomaly
double tmp_Y = sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E);
double tmp_X = cos(E) - d_e_eccentricity;
nu = atan2(tmp_Y, tmp_X);
nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude)
phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad
phi = fmod((phi),(2*GPS_PI));
phi = fmod((phi), (2*GPS_PI));
// Correct argument of latitude
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi);
@ -371,7 +371,6 @@ void gps_navigation_message::satellitePosition(double transmitTime)
// Correct radius
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi);
// Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi);
@ -379,33 +378,17 @@ void gps_navigation_message::satellitePosition(double transmitTime)
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));
// debug
/*
if (this->i_channel_ID==0){
std::cout<<"tk"<<tk<<std::endl;
std::cout<<"E="<<E<<std::endl;
std::cout<<"d_dtr="<<d_dtr<<std::endl;
std::cout<<"nu="<<nu<<std::endl;
std::cout<<"phi="<<phi<<std::endl;
std::cout<<"u="<<u<<" r="<<r<<" Omega="<<Omega<<std::endl;
std::cout<<"i="<<i<<"\r\n";
std::cout<<"tmp_Y="<<tmp_Y<<"\r\n";
std::cout<<"tmp_X="<<tmp_X<<"\r\n";
}
*/
Omega = fmod((Omega + 2*GPS_PI), (2*GPS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega);
d_satpos_Z = sin(u) * r * sin(i);
/* Satellite's velocity. Can be useful for Vector Tracking loops */
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT;
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_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);
}
@ -433,7 +416,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
std::bitset<GPS_WORD_BITS+2> word_bits;
for (int i=0; i<10; i++)
{
memcpy(&gps_word,&subframe[i*4], sizeof(char)*4);
memcpy(&gps_word, &subframe[i*4], sizeof(char)*4);
word_bits = std::bitset<(GPS_WORD_BITS+2)>(gps_word);
for (int j=0; j<GPS_WORD_BITS; j++)
{
@ -441,17 +424,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
}
}
// *** DEBUG
//std::cout<<"bitset subframe="<<subframe_bits<<std::endl;
/*
for (int i=0; i<10;i++)
{
memcpy(&gps_word,&d_subframe[i*4],sizeof(char)*4);
print_gps_word_bytes(gps_word);
}
*/
subframe_ID = (int)read_navigation_unsigned(subframe_bits, SUBFRAME_ID, num_of_slices(SUBFRAME_ID));
//std::cout<<"subframe ID="<<subframe_ID<<std::endl;
// Decode all 5 sub-frames
switch (subframe_ID)
@ -460,7 +433,6 @@ int gps_navigation_message::subframe_decoder(char *subframe)
// ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf
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
@ -468,16 +440,12 @@ int gps_navigation_message::subframe_decoder(char *subframe)
//TOW = bin2dec(subframe(31:47)) * 6 - 30;
d_TOW = (double)read_navigation_unsigned(subframe_bits, TOW, num_of_slices(TOW));
d_TOW = d_TOW*6-6; //we are in the first subframe (the transmitted TOW is the start time of the next subframe, thus we need to substract one subframe (6 seconds)) !
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);
// It contains WN, SV clock corrections, health and accuracy
i_GPS_week = (int)read_navigation_unsigned(subframe_bits, GPS_WEEK, num_of_slices(GPS_WEEK));
i_SV_accuracy = (int)read_navigation_unsigned(subframe_bits, SV_ACCURACY, num_of_slices(SV_ACCURACY)); // (20.3.3.3.1.3)
i_SV_health = (int)read_navigation_unsigned(subframe_bits, SV_HEALTH, num_of_slices(SV_HEALTH));
b_L2_P_data_flag = read_navigation_bool(subframe_bits, L2_P_DATA_FLAG); //
i_code_on_L2 = (int)read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2, num_of_slices(CA_OR_P_ON_L2));
d_TGD = (double)read_navigation_signed(subframe_bits, T_GD, num_of_slices(T_GD));
@ -492,22 +460,9 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_A_f2 = (double)read_navigation_signed(subframe_bits, A_F2, num_of_slices(A_F2));
d_A_f2 = d_A_f2*A_F2_LSB;
/*
eph.weekNumber = bin2dec(subframe(61:70)) + 1024;
eph.accuracy = bin2dec(subframe(73:76));
eph.health = bin2dec(subframe(77:82));
eph.T_GD = twosComp2dec(subframe(197:204)) * 2^(-31);
eph.IODC = bin2dec([subframe(83:84) subframe(197:204)]);
eph.t_oc = bin2dec(subframe(219:234)) * 2^4;
eph.a_f2 = twosComp2dec(subframe(241:248)) * 2^(-55);
eph.a_f1 = twosComp2dec(subframe(249:264)) * 2^(-43);
eph.a_f0 = twosComp2dec(subframe(271:292)) * 2^(-31);
*/
break;
case 2: //--- It is subframe 2 -------------------
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);
@ -529,25 +484,12 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_Toe = (double)read_navigation_unsigned(subframe_bits, T_OE, num_of_slices(T_OE));
d_Toe = d_Toe * T_OE_LSB;
b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG);
i_AODO = (int)read_navigation_unsigned(subframe_bits, AODO, num_of_slices(AODO));
i_AODO = i_AODO * AODO_LSB;
break;
/*
eph.IODE_sf2 = bin2dec(subframe(61:68));
eph.C_rs = twosComp2dec(subframe(69: 84)) * 2^(-5);
eph.deltan = twosComp2dec(subframe(91:106)) * 2^(-43) * gpsPi;
eph.M_0 = twosComp2dec([subframe(107:114) subframe(121:144)])* 2^(-31) * gpsPi;
eph.C_uc = twosComp2dec(subframe(151:166)) * 2^(-29);
eph.e = bin2dec([subframe(167:174) subframe(181:204)])* 2^(-33);
eph.C_us = twosComp2dec(subframe(211:226)) * 2^(-29);
eph.sqrtA = bin2dec([subframe(227:234) subframe(241:264)])* 2^(-19);
eph.t_oe = bin2dec(subframe(271:286)) * 2^4;
*/
case 3: // --- It is subframe 3 -------------------------------------
//tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
//std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
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);
@ -570,23 +512,11 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_IDOT = d_IDOT*I_DOT_LSB;
break;
/*
eph.C_ic = twosComp2dec(subframe(61:76)) * 2^(-29);
eph.omega_0 = twosComp2dec([subframe(77:84) subframe(91:114)])* 2^(-31) * gpsPi;
eph.C_is = twosComp2dec(subframe(121:136)) * 2^(-29);
eph.i_0 = twosComp2dec([subframe(137:144) subframe(151:174)])* 2^(-31) * gpsPi;
eph.C_rc = twosComp2dec(subframe(181:196)) * 2^(-5);
eph.omega = twosComp2dec([subframe(197:204) subframe(211:234)])* 2^(-31) * gpsPi;
eph.omegaDot = twosComp2dec(subframe(241:264)) * 2^(-43) * gpsPi;
eph.IODE_sf3 = bin2dec(subframe(271:278));
eph.iDot = twosComp2dec(subframe(279:292)) * 2^(-43) * gpsPi;
*/
case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32)
case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32)
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);
SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID, num_of_slices(SV_DATA_ID));
SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE, num_of_slices(SV_PAGE));
@ -649,7 +579,6 @@ int gps_navigation_message::subframe_decoder(char *subframe)
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID, num_of_slices(SV_DATA_ID));
SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE, num_of_slices(SV_PAGE));
if (SV_page < 25)
{
//! \TODO read almanac
@ -659,7 +588,6 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_Toa = (double)read_navigation_unsigned(subframe_bits,T_OA,num_of_slices(T_OA));
d_Toa = d_Toa * T_OA_LSB;
i_WN_A = (int)read_navigation_unsigned(subframe_bits,WN_A,num_of_slices(WN_A));
almanacHealth[1] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV1, num_of_slices(HEALTH_SV1));
almanacHealth[2] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV2, num_of_slices(HEALTH_SV2));
almanacHealth[3] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV3, num_of_slices(HEALTH_SV3));
@ -684,7 +612,6 @@ int gps_navigation_message::subframe_decoder(char *subframe)
almanacHealth[22] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV22, num_of_slices(HEALTH_SV22));
almanacHealth[23] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV23, num_of_slices(HEALTH_SV23));
almanacHealth[24] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV24, num_of_slices(HEALTH_SV24));
}
break;
default:
@ -695,11 +622,13 @@ int gps_navigation_message::subframe_decoder(char *subframe)
}
double gps_navigation_message::utc_time(double gpstime_corrected)
{
double t_utc;
double t_utc_daytime;
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T));
// Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week;
@ -708,15 +637,12 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
{
//Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
int secondOfLeapSecondEvent = i_DN * 24 * 60 * 60;
if (weeksToLeapSecondEvent > 0)
{
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
else //we are in the same week than the leap second event
{
if (abs(gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{
/* 20.3.3.5.2.4a
@ -726,7 +652,6 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
* to the effectivity time and ends at six hours after the effectivity time,
* the UTC/GPS-time relationship is given by
*/
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
else
@ -737,13 +662,11 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
* proper accommodation of the leap second event with a possible week number
* transition is provided by the following expression for UTC:
*/
int W = fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400) + 43200;
t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS);
//implement something to handle a leap second event!
}
if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800*(double)(i_GPS_week - i_WN_T));
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
@ -757,11 +680,11 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
* WNLSF and DN values, is in the "past" (relative to the user's current time),
* and the userÕs current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T));
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
double secondsOfWeekBeforeToday= 43200 * floor(gpstime_corrected / 43200);
double secondsOfWeekBeforeToday = 43200 * floor(gpstime_corrected / 43200);
t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc;
}