diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 03c4472f3..3da7a332f 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -109,13 +109,14 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite } //debug print - std::cout << std::setprecision(16); + /*std::cout << std::setprecision(16); for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin(); gnss_pseudoranges_iter != gnss_pseudoranges_map.end(); gnss_pseudoranges_iter++) { - std::cout<<"Pseudoranges(SV ID,pseudorange [m]) =("<first<<","<second.pseudorange_m<<")"<first << "," + << gnss_pseudoranges_iter->second.pseudorange_m << ")" < +#include +using google::LogMessage; + gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file) { - // init empty ephemerids for all the available GNSS channels + // init empty ephemeris for all the available GNSS channels d_nchannels=nchannels; d_ephemeris=new gps_navigation_message[nchannels]; d_dump_filename=dump_filename; @@ -220,15 +224,17 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_ */ W(i,i)=1; // compute the GPS master clock - d_ephemeris[i].master_clock(GPS_current_time); + // d_ephemeris[i].master_clock(GPS_current_time); ????? + + // compute the clock error including relativistic effects + d_ephemeris[i].sv_clock_correction(GPS_current_time); // compute the satellite current ECEF position - d_ephemeris[i].satpos(); - // compute the clock error including relativistic effects - d_ephemeris[i].relativistic_clock_correction(GPS_current_time); + d_ephemeris[i].satellitePosition(GPS_current_time); + satpos(0,i)=d_ephemeris[i].d_satpos_X; satpos(1,i)=d_ephemeris[i].d_satpos_Y; satpos(2,i)=d_ephemeris[i].d_satpos_Z; - std::cout<<"ECEF satellite SV ID="<second.pseudorange_m+d_ephemeris[i].d_satClkCorr*GPS_C_m_s; valid_obs++; @@ -248,9 +254,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_ { arma::vec mypos; mypos=leastSquarePos(satpos,obs,W); - std::cout << "Position at TOW="< +#include "gps_navigation_message.h" #include #include #include // for getenv() @@ -51,7 +51,74 @@ rinex_printer::rinex_printer() rinex_printer::navFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_GPS_NAV")); rinex_printer::obsFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_OBS")); - rinex_printer::Rinex2NavHeader(rinex_printer::navFile); + //rinex_printer::Rinex2NavHeader(rinex_printer::navFile, gps_navigation_message nav); + + satelliteSystem["GPS"]="G"; + satelliteSystem["GLONASS"]="R"; + satelliteSystem["SBAS payload"]="S"; + satelliteSystem["Galileo"]="E"; + satelliteSystem["Compass"]="C"; + + observationCode["GPS_L1_CA"] = "1C"; //!< "1C" GPS L1 C/A + observationCode["GPS_L1_P"] = "1P"; //!< "1P" GPS L1 P + observationCode["GPS_L1_Z_TRACKING"] = "1W"; //!< "1W" GPS L1 Z-tracking and similar (AS on) + observationCode["RINEX_GPS_L1_Y"] = "1Y"; //!< "1Y" GPS L1 Y + observationCode["GPS_L1_M "]= "1M"; //!< "1M" GPS L1 M + observationCode["GPS_L1_CODELESS"] = "1N"; //!< "1N" GPS L1 codeless + observationCode["GPS_L2_CA"]= "2C"; //!< "2C" GPS L2 C/A + observationCode["L2_SEMI_CODELESS"] = "2D"; //!< "2D" GPS L2 L1(C/A)+(P2-P1) semi-codeless + observationCode["GPS_L2_L2CM"] = "2S"; //!< "2S" GPS L2 L2C (M) + observationCode["GPS_L2_L2CL"] = "2L"; //!< "2L" GPS L2 L2C (L) + observationCode["GPS_L2_L2CML"] = "2X"; //!< "2X" GPS L2 L2C (M+L) + observationCode["GPS_L2_P"] = "2P"; //!< "2P" GPS L2 P + observationCode["GPS_L2_Z_TRACKING"] = "2W"; //!< "2W" GPS L2 Z-tracking and similar (AS on) + observationCode["GPS_L2_Y"] = "2Y"; //!< "2Y" GPS L2 Y + observationCode["GPS_L2_M"] = "2M"; //!< "2M" GPS GPS L2 M + observationCode["GPS_L2_codeless"] = "2N"; //!< "2N" GPS L2 codeless + observationCode["GPS_L5_I"] = "5I"; //!< "5I" GPS L5 I + observationCode["GPS_L5_Q"] = "5Q"; //!< "5Q" GPS L5 Q + observationCode["GPS_L5_IQ"] = "5X"; //!< "5X" GPS L5 I+Q + observationCode["GLONASS_G1_CA"] = "1C"; //!< "1C" GLONASS G1 C/A + observationCode["GLONASS_G1_P"]= "1P"; //!< "1P" GLONASS G1 P + observationCode["GLONASS_G2_CA"]= "2C"; //!< "2C" GLONASS G2 C/A (Glonass M) + observationCode["GLONASS_G2_P"]= "2P"; //!< "2P" GLONASS G2 P + observationCode["GALILEO_E1_A"]= "1A"; //!< "1A" GALILEO E1 A (PRS) + observationCode["GALILEO_E1_B"]= "1B"; //!< "1B" GALILEO E1 B (I/NAV OS/CS/SoL) + observationCode["GALILEO_E1_C"]= "1C"; //!< "1C" GALILEO E1 C (no data) + observationCode["GALILEO_E1_BC"]= "1X"; //!< "1X" GALILEO E1 B+C + observationCode["GALILEO_E1_ABC"]= "1Z"; //!< "1Z" GALILEO E1 A+B+C + observationCode["GALILEO_E5a_I"]= "5I"; //!< "5I" GALILEO E5a I (F/NAV OS) + observationCode["GALILEO_E5a_Q"]= "5Q"; //!< "5Q" GALILEO E5a Q (no data) + observationCode["GALILEO_E5aIQ"]= "5X"; //!< "5X" GALILEO E5a I+Q + observationCode["GALILEO_E5b_I"]= "7I"; //!< "7I" GALILEO E5b I + observationCode["GALILEO_E5b_Q"]= "7Q"; //!< "7Q" GALILEO E5b Q + observationCode["GALILEO_E5b_IQ"]= "7X"; //!< "7X" GALILEO E5b I+Q + observationCode["GALILEO_E5_I"]= "8I"; //!< "8I" GALILEO E5 I + observationCode["GALILEO_E5_Q"]= "8Q"; //!< "8Q" GALILEO E5 Q + observationCode["GALILEO_E5_IQ"]= "8X"; //!< "8X" GALILEO E5 I+Q + observationCode["GALILEO_E56_A"]= "6A"; //!< "6A" GALILEO E6 A + observationCode["GALILEO_E56_B"] = "6B"; //!< "6B" GALILEO E6 B + observationCode["GALILEO_E56_B"] = "6C"; //!< "6C" GALILEO E6 C + observationCode["GALILEO_E56_BC"] = "6X"; //!< "6X" GALILEO E6 B+C + observationCode["GALILEO_E56_ABC"] = "6Z"; //!< "6Z" GALILEO E6 A+B+C + observationCode["SBAS_L1_CA"] = "1C"; //!< "1C" SBAS L1 C/A + observationCode["SBAS_L5_I"] = "5I"; //!< "5I" SBAS L5 I + observationCode["SBAS_L5_Q"] = "5Q"; //!< "5Q" SBAS L5 Q + observationCode["SBAS_L5_IQ"] = "5X"; //!< "5X" SBAS L5 I+Q + observationCode["COMPASS_E2_I"] = "2I"; + observationCode["COMPASS_E2_Q"] = "2Q"; + observationCode["COMPASS_E2_IQ"] = "2X"; + observationCode["COMPASS_E5b_I"] = "7I"; + observationCode["COMPASS_E5b_Q"] = "7Q"; + observationCode["COMPASS_E5b_IQ"] = "7X"; + observationCode["COMPASS_E6_I"] = "6I"; + observationCode["COMPASS_E6_Q"] = "6Q"; + observationCode["COMPASS_E6_IQ"] = "6X"; + + observationType["PSEUDORANGE"]="C"; + observationType["CARRIER_PHASE"]="L"; + observationType["DOPPLER"]="D"; + observationType["SIGNAL_STRENGTH"]="S"; } @@ -195,7 +262,7 @@ std::string rinex_printer::getLocalTime() } -void rinex_printer::Rinex2NavHeader(std::ofstream& out) +void rinex_printer::Rinex2NavHeader(std::ofstream& out, gps_navigation_message nav_msg) { std::string line; @@ -204,22 +271,22 @@ void rinex_printer::Rinex2NavHeader(std::ofstream& out) std::string version="3.01"; line = std::string(5,' '); line += version; - line +=std::string(11,' '); - line +=std::string("N: GNSS NAV DATA"); - line +=std::string(4,' '); + line += std::string(11,' '); + line += std::string("N: GNSS NAV DATA"); + line += std::string(4,' '); //! \todo Add here other systems... - line +=std::string("G: GPS"); - line +=std::string(14,' '); + line += std::string("G: GPS"); + line += std::string(14,' '); // ... - line +=std::string("RINEX VERSION / TYPE"); + line += std::string("RINEX VERSION / TYPE"); rinex_printer::lengthCheck(line); out << line << std::endl; // -------- Line 2 line.clear(); - line +=rinex_printer::getLocalTime(); - line +=std::string("PGM / RUN BY / DATE"); - line +=std::string(1,' '); + line += rinex_printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1,' '); rinex_printer::lengthCheck(line); out << line << std::endl; // -------- Line 3 @@ -230,18 +297,63 @@ void rinex_printer::Rinex2NavHeader(std::ofstream& out) out << line << std::endl; // -------- Line 4 ionospheric info line.clear(); - line +=std::string("GPSA"); - line +=std::string(4,' '); + line += std::string("GPSA"); + line += std::string(1,' '); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_alpha0, 12, 2),12); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_alpha1, 12, 2),12); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_alpha2, 12, 2),12); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_alpha3, 12, 2),12); + line += std::string(7,' '); + line += rinex_printer::leftJustify("IONOSPHERIC CORR",20); + rinex_printer::lengthCheck(line); + out << line << std::endl; - // IONOSPHERIC INFORMATION + // -------- Line 5 ionospheric info + line.clear(); + line += std::string("GPSB"); + line += std::string(1,' '); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_beta0, 12, 2),12); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_beta1, 12, 2),12); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_beta2, 12, 2),12); + line += rinex_printer::rightJustify(rinex_printer::doub2for(nav_msg.d_beta3, 12, 2),12); + line += std::string(7,' '); + line += rinex_printer::leftJustify("IONOSPHERIC CORR",20); + rinex_printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 5 system time correction + line.clear(); + line += std::string("GPUT"); + line += std::string(1,' '); + line += rinex_printer::doub2for(nav_msg.d_A0, 17, 2); + line += rinex_printer::doub2for(nav_msg.d_A0, 16, 2); + line += rinex_printer::rightJustify(rinex_printer::asString(nav_msg.d_t_OT),7); + line += rinex_printer::rightJustify(rinex_printer::asString(nav_msg.i_WN_T),5); + /* if ( SBAS ) + { + line += string(1, ' '); + line += leftJustify(asString(d_t_OT_SBAS),5); + line += string(1, ' '); + line += leftJustify(asString(d_WN_T_SBAS),2); + line += string(1, ' '); + } + else + */ + line += std::string(10, ' '); + line += rinex_printer::leftJustify("TIME SYSTEM CORR",20); + rinex_printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 6 leap seconds // For leap second information, see See http://www.endruntechnologies.com/leap.htm line.clear(); - line +=std::string(4,' '); - line +=std::string("15"); // Page 18 of subframe 4 Delta_T_LSF - line +=std::string(54,' '); - line +=std::string("LEAP SECONDS"); - line +=std::string(8,' '); + line = rinex_printer::rightJustify(rinex_printer::asString(nav_msg.d_DeltaT_LS),6); + line = rinex_printer::rightJustify(rinex_printer::asString(nav_msg.d_DeltaT_LSF),6); + line = rinex_printer::rightJustify(rinex_printer::asString(nav_msg.i_WN_LSF),6); + line = rinex_printer::rightJustify(rinex_printer::asString(nav_msg.i_DN),6); + line += std::string(36, ' '); + line += rinex_printer::leftJustify("LEAP SECONDS",20); rinex_printer::lengthCheck(line); out << line << std::endl; @@ -364,15 +476,8 @@ void rinex_printer::LogRinex2Nav(gps_navigation_message nav_msg){ } -void rinex_printer::Rinex2ObsHeader(std::ofstream& out) +void rinex_printer::Rinex2ObsHeader(std::ofstream& out, gps_navigation_message nav_msg) { - std::map satelliteSystem; - satelliteSystem.insert(std::pair("GPS","G")); - satelliteSystem.insert(std::pair("GLONASS","R")); - satelliteSystem.insert(std::pair("Galileo","E")); - satelliteSystem.insert(std::pair("SBAS payload","S")); - satelliteSystem.insert(std::pair("Mixed","M")); - satelliteSystem.insert(std::pair("Compass","C")); std::string line; @@ -457,10 +562,42 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out) // -------- ANTENNA: DELTA H/E/N + // put here real data! + double antena_h=0.0; + double antena_e=0.0; + double antena_n=0.0; + line.clear(); + line = rinex_printer::rightJustify(rinex_printer::asString(antena_h, 4), 14); + line += rinex_printer::rightJustify(rinex_printer::asString(antena_e, 4), 14); + line += rinex_printer::rightJustify(rinex_printer::asString(antena_n, 4), 14); + line += std::string(18, ' '); + line += rinex_printer::leftJustify("ANTENNA: DELTA H/E/N",20); + rinex_printer::lengthCheck(line); + out << line << std::endl; // -------- SYS / OBS TYPES + // one line per available system + line.clear(); + line += satelliteSystem["GPS"]; + line +=std::string(2,' '); + int numberObservations=2; // Count the number of available types of observable in the system + std::stringstream strm; + strm << numberObservations; + line += rinex_printer::rightJustify(strm.str(),3); + // per type of observation + line += std::string(1,' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L1_CA"]; + line += std::string(1,' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L1_CA"]; + + line +=std::string(60-line.size(),' '); + line += rinex_printer::leftJustify("SYS / # / OBS TYPES",20); + rinex_printer::lengthCheck(line); + out << line << std::endl; // -------- Signal Strength units line.clear(); line += rinex_printer::leftJustify("DBHZ",20); @@ -470,8 +607,29 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out) out << line << std::endl; // -------- TIME OF FIRST OBS + line.clear(); + line += std::string("GPS"); + line += std::string(5,' '); + /////////////////////////////////////////// + // 4-digit-year, month,day,hour,min,sec + double year=2012; + double month=1; + double day=4; + double hour=8; + double minute =43; + double second = GPS_PI; + line += rightJustify(asString(year), 6); + line += rightJustify(asString(month), 6); + line += rightJustify(asString(day), 6); + line += rightJustify(asString(hour), 6); + line += rightJustify(asString(minute), 6); + line += rightJustify(asString(second,7), 13); + line += rightJustify(std::string("GPS"), 8); + line += rinex_printer::leftJustify("TIME OF FIRST OBS",20); + rinex_printer::lengthCheck(line); + out << line << std::endl; // -------- SYS /PHASE SHIFTS diff --git a/src/algorithms/PVT/libs/rinex_2_1_printer.h b/src/algorithms/PVT/libs/rinex_2_1_printer.h index 313816bab..570182a15 100644 --- a/src/algorithms/PVT/libs/rinex_2_1_printer.h +++ b/src/algorithms/PVT/libs/rinex_2_1_printer.h @@ -33,6 +33,9 @@ #include #include +#include +#include // for stringstream +#include // for setprecision #include "gps_navigation_message.h" /*! @@ -45,6 +48,22 @@ private: std::ofstream navFile ; std::ofstream obsFile ; + /* + * Generates the Navigation Data header + */ + void Rinex2NavHeader(std::ofstream& out, gps_navigation_message nav); + + /* + * Generates the Observation data header + */ + void Rinex2ObsHeader(std::ofstream& out, gps_navigation_message nav); + + /* + * Generation of RINEX signal strength indicators + */ + int signalStrength(double snr); + + /* Creates RINEX file names according to the naming convention * * See http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf @@ -109,21 +128,137 @@ private: { std::string t(s); return leftJustify(t, length, pad); } + /* + * Right-justifies the receiver in a string of the specified + * length. If the receiver's data is shorter than the + * requested length (\a length), it is padded on the left with + * the pad character (\a pad). The default pad + * character is a blank. */ + inline std::string& rightJustify(std::string& s, + const std::string::size_type length, + const char pad = ' '); /* - * Generates the Navigation Data header - */ - void Rinex2NavHeader(std::ofstream& out); + * Right-justifies the receiver in a string of the specified + * length (const version). If the receiver's data is shorter than the + * requested length (\a length), it is padded on the left with + * the pad character (\a pad). The default pad + * character is a blank.*/ + inline std::string rightJustify(const std::string& s, + const std::string::size_type length, + const char pad = ' ') + { std::string t(s); return rightJustify(t, length, pad); } + + /* - * Generates the Observation data header + * Convert a double to a scientific notation number. + * @param d the double to convert + * @param length length (in characters) of output, including exponent + * @param expLen length (in characters) of the exponent, with sign + * @param showSign if true, reserves 1 character for +/- sign + * @param checkSwitch if true, keeps the exponential sanity check for + * exponentials above three characters in length. If false, it removes + * that check. */ - void Rinex2ObsHeader(std::ofstream& out); + inline std::string doub2sci(const double& d, + const std::string::size_type length, + const std::string::size_type expLen, + const bool showSign = true, + const bool checkSwitch = true); + + + /* - * Generation of RINEX signal strength indicators + * Convert scientific notation to FORTRAN notation. + * As an example, the string "1.5636E5" becomes " .15636D6". + * Note that the first character of the string will be '-' if + * the number is negative or ' ' if the first character is positive. + * @param aStr string with number to convert + * @param startPos start position of number in string + * @param length length (in characters) of number, including exponent. + * @param expLen length (in characters of exponent, not including sign. + * @param checkSwitch will keep the method running as originally programmed + * when set to true. If false, the method will always resize exponentials, + * produce an exponential with an E instead of a D, and always have a leading + * zero. For example -> 0.87654E-0004 or -0.1234E00005. */ - int signalStrength(double snr); + inline std::string& sci2for(std::string& aStr, + const std::string::size_type startPos = 0, + const std::string::size_type length = std::string::npos, + const std::string::size_type expLen = 3, + const bool checkSwitch = true); + + + + + + /* + * Convert double precision floating point to a string + * containing the number in FORTRAN notation. + * As an example, the number 156360 becomes ".15636D6". + * @param d number to convert. + * @param length length (in characters) of number, including exponent. + * @param expLen length (in characters of exponent, including sign. + * @param checkSwitch if true, keeps the exponential sanity check for + * exponentials above three characters in length. If false, it removes + * that check. + * @return a string containing \a d in FORTRAN notation. + */ + inline std::string doub2for(const double& d, + const std::string::size_type length, + const std::string::size_type expLen, + const bool checkSwitch = true); + + + /* + * Convert a string to a double precision floating point number. + * @param s string containing a number. + * @return double representation of string. + */ + inline double asDouble(const std::string& s) + { return strtod(s.c_str(), 0); } + + /* + * Convert a string to an integer. + * @param s string containing a number. + * @return long integer representation of string. + */ + inline long asInt(const std::string& s) + { return strtol(s.c_str(), 0, 10); } + + + + /* + * Convert a double to a string in fixed notation. + * @param x double. + * @param precision the number of decimal places you want displayed. + * @return string representation of \a x. + */ + inline std::string asString(const double x, + const std::string::size_type precision = 17); + + + /* + * Convert a long double to a string in fixed notation. + * @param x long double. + * @param precision the number of decimal places you want displayed. + * @return string representation of \a x. + */ + inline std::string asString(const long double x, + const std::string::size_type precision = 21); + + /* + * Convert any old object to a string. + * The class must have stream operators defined. + * @param x object to turn into a string. + * @return string representation of \a x. + */ + template + inline std::string asString(const X x); + + @@ -139,13 +274,17 @@ public: */ ~rinex_printer(); void LogRinex2Nav(gps_navigation_message nav_msg); - void LogRinex2Obs(gps_navigation_message nav_msg,double interframe_seconds, std::map pseudoranges); + void LogRinex2Obs(gps_navigation_message nav_msg, double interframe_seconds, std::map pseudoranges); + + std::map satelliteSystem; + std::map observationType; + std::map observationCode; }; -// Implementation of inline function +// Implementation of inline functions inline std::string& rinex_printer::leftJustify(std::string& s, const std::string::size_type length, @@ -164,4 +303,194 @@ inline std::string& rinex_printer::leftJustify(std::string& s, } +// if the string is bigger than length, truncate it from the left. +// otherwise, add pad characters to its left. +inline std::string& rinex_printer::rightJustify(std::string& s, + const std::string::size_type length, + const char pad) +{ + if(length < s.length()) + { + s = s.substr(s.length()-length, std::string::npos); + } + else + { + s.insert((std::string::size_type)0, length-s.length(), pad); + } + return s; +} + + + + + +inline std::string rinex_printer::doub2for(const double& d, + const std::string::size_type length, + const std::string::size_type expLen, + const bool checkSwitch) +{ + + short exponentLength = expLen; + + /* Validate the assumptions regarding the input arguments */ + if (exponentLength < 0) exponentLength = 1; + if (exponentLength > 3 && checkSwitch) exponentLength = 3; + + std::string toReturn = doub2sci(d, length, exponentLength, true, checkSwitch); + sci2for(toReturn, 0, length, exponentLength, checkSwitch); + + return toReturn; + +} + + +inline std::string rinex_printer::doub2sci(const double& d, + const std::string::size_type length, + const std::string::size_type expLen, + const bool showSign, + const bool checkSwitch) +{ + std::string toReturn; + short exponentLength = expLen; + + /* Validate the assumptions regarding the input arguments */ + if (exponentLength < 0) exponentLength = 1; + if (exponentLength > 3 && checkSwitch) exponentLength = 3; + + std::stringstream c; + c.setf(std::ios::scientific, std::ios::floatfield); + + // length - 3 for special characters ('.', 'e', '+' or '-') + // - exponentlength (e04) + // - 1 for the digit before the decimal (2.) + // and if showSign == true, + // an extra -1 for '-' or ' ' if it's positive or negative + int expSize = 0; + if (showSign) + expSize = 1; + c.precision(length - 3 - exponentLength - 1 - expSize); + c << d; + c >> toReturn; + return toReturn; +} + +inline std::string& rinex_printer::sci2for(std::string& aStr, + const std::string::size_type startPos, + const std::string::size_type length, + const std::string::size_type expLen, + const bool checkSwitch) +{ + + std::string::size_type idx = aStr.find('.', startPos); + int expAdd = 0; + std::string exp; + long iexp; + //If checkSwitch is false, always redo the exponential. Otherwise, + //set it to false. + bool redoexp=!checkSwitch; + + // Check for decimal place within specified boundaries + if ((idx == 0) || (idx >= (startPos + length - expLen - 1))) + { + //StringException e("sci2for: no decimal point in string"); + //GPSTK_THROW(e); + } + + // Here, account for the possibility that there are + // no numbers to the left of the decimal, but do not + // account for the possibility of non-scientific + // notation (more than one digit to the left of the + // decimal) + if (idx > startPos) + { + redoexp = true; + // Swap digit and decimal. + aStr[idx] = aStr[idx-1]; + aStr[idx-1] = '.'; + // Only add one to the exponent if the number is non-zero + if (asDouble(aStr.substr(startPos, length)) != 0.0) + expAdd = 1; + } + + idx = aStr.find('e', startPos); + if (idx == std::string::npos) + { + idx = aStr.find('E', startPos); + if (idx == std::string::npos) + { + //StringException e("sci2for:no 'e' or 'E' in string"); + //GPSTK_THROW(e); + } + } + // Change the exponent character to D normally, or E of checkSwitch is false. + if (checkSwitch) + aStr[idx] = 'D'; + else + aStr[idx] = 'E'; + + // Change the exponent itself + if (redoexp) + { + exp = aStr.substr(idx + 1, std::string::npos); + iexp = asInt(exp); + iexp += expAdd; + + aStr.erase(idx + 1); + if (iexp < 0) + { + aStr += "-"; + iexp -= iexp*2; + } + else + aStr += "+"; + aStr += rinex_printer::rightJustify(asString(iexp),expLen,'0'); + + } + + // if the number is positive, append a space + // (if it's negative, there's a leading '-' + if (aStr[0] == '.') + { + aStr.insert((std::string::size_type)0, 1, ' '); + } + + //If checkSwitch is false, add on one leading zero to the string + if (!checkSwitch) + { + aStr.insert((std::string::size_type)1, 1, '0'); + } + + + return aStr; + +} // end sci2for + + + +inline std::string asString(const long double x, const std::string::size_type precision) + { + std::ostringstream ss; + ss << std::fixed << std::setprecision(precision) << x ; + return ss.str(); + } + + + + +inline std::string rinex_printer::asString(const double x, const std::string::size_type precision) +{ + std::ostringstream ss; + ss << std::fixed << std::setprecision(precision) << x; + return ss.str(); +} + +template +inline std::string rinex_printer::asString(const X x) +{ + std::ostringstream ss; + ss << x; + return ss.str(); +} + + #endif diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index ac3f86059..f4e5cefc6 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -75,7 +75,7 @@ void gps_navigation_message::reset() d_A_f2=0; //clock terms - d_master_clock=0; + //d_master_clock=0; d_dtr=0; d_satClkCorr=0; @@ -233,7 +233,7 @@ double gps_navigation_message::check_t(double time) } - +/* void gps_navigation_message::master_clock(double transmitTime) { double dt; @@ -249,9 +249,21 @@ void gps_navigation_message::master_clock(double transmitTime) d_master_clock = transmitTime - satClkCorr; } +*/ + +// 20.3.3.3.3.1 User Algorithm for SV Clock Correction. +void gps_navigation_message::sv_clock_correction(double transmitTime) +{ + double dt; + dt = check_t(transmitTime - d_Toc); + d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr; +} -void gps_navigation_message::satpos() + + + +void gps_navigation_message::satellitePosition(double transmitTime) { double tk; double a; @@ -274,7 +286,7 @@ void gps_navigation_message::satpos() a = d_sqrt_A*d_sqrt_A; // Time correction - tk = check_t(d_master_clock - d_Toe); + tk = check_t(transmitTime - d_Toe); // Initial mean motion n0 = sqrt(GM / (a*a*a)); @@ -359,20 +371,6 @@ void gps_navigation_message::satpos() -void gps_navigation_message::relativistic_clock_correction(double transmitTime) -{ - double dt; - // Find final satellite clock correction -------------------------------- - - // --- Find time difference --------------------------------------------- - dt = check_t(transmitTime - d_Toc); - - //Include relativistic correction in clock correction -------------------- - d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 -d_TGD + d_dtr; -} - - - diff --git a/src/core/system_parameters/gps_navigation_message.h b/src/core/system_parameters/gps_navigation_message.h index 2169ab097..09bea6c50 100644 --- a/src/core/system_parameters/gps_navigation_message.h +++ b/src/core/system_parameters/gps_navigation_message.h @@ -101,7 +101,7 @@ public: //broadcast orbit 6 int i_SV_accuracy; //!< User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200E) int i_SV_health; - double d_TGD; //!< Estimated Group Delay Differential: L1-L2 correction term for the benefit of "L1 only" or "L2 only" users [s] + double d_TGD; //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] double d_IODC; //!< Issue of Data, Clock //broadcast orbit 7 int i_AODO; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s] @@ -139,7 +139,7 @@ public: // clock terms - double d_master_clock; // GPS transmission time + //double d_master_clock; // GPS transmission time double d_satClkCorr; // GPS clock error double d_dtr; // relativistic clock correction term @@ -173,7 +173,7 @@ public: double d_A0; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200E) [s] double d_t_OT; //!< Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200E) [s] int i_WN_T; //!< UTC reference week number [weeks] - double d_DeltaT_LS; //!< delta time due to leap seconds [s] + double d_DeltaT_LS; //!< delta time due to leap seconds [s]. Number of leap seconds since 6-Jan-1980 as transmitted by the GPS almanac. int i_WN_LSF; //!< Week number at the end of which the leap second becomes effective [weeks] int i_DN; //!< Day number (DN) at the end of which the leap second becomes effective [days] double d_DeltaT_LSF; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s] @@ -187,20 +187,26 @@ public: */ int subframe_decoder(char *subframe); - /*! - * \brief User Algorithm for SV Clock Correction + /* + * User Algorithm for SV Clock Correction * * Implementation of paragraph 20.3.3.3.3.1 (IS-GPS-200E) */ - void master_clock(double transmitTime); + //void master_clock(double transmitTime); /*! * \brief Computes the position of the satellite * * Implementation of Table 20-IV (IS-GPS-200E) */ - void satpos(); - void relativistic_clock_correction(double transmitTime); + void satellitePosition(double transmitTime); + + /*! + * \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction (IS-GPS-200E, 20.3.3.3.3.1) + */ + void sv_clock_correction(double transmitTime); + + bool satellite_validation(); /*!