Added a method in gps_navigation message that computes UTC time.

Experimental creation of RINEX headers.

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@112 64b25241-fba3-4117-9849-534c7e92360d

src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc

@@ -40,7 +40,7 @@
 #include <glog/logging.h>
 #include "gps_l1_ca_pvt_cc.h"
 #include "control_message_factory.h"
-
+#include "rinex_2_1_printer.h"
 
 using google::LogMessage;
 
@@ -78,11 +78,20 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
     d_ephemeris_clock_s=0.0;
 
     d_sample_counter=0;
+
+    b_rinex_header_writen = false;
+    rp = new rinex_printer();
+
+    //rp->navFile.open(rp->createFilename("RINEX_FILE_TYPE_GPS_NAV"), std::ios::out | std::ios::app);
+    //rp->obsFile.open(rp->createFilename("RINEX_FILE_TYPE_OBS"), std::ios::out | std::ios::app);
+    //Rinex_Nav_File=rp.getNavFileStream();
+    //Rinex_Obs_File=rp.getObsFileStream();
 }
 
 gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc() {
     d_kml_dump.close_file();
     delete d_ls_pvt;
+    delete rp;
 }
 
 bool pseudoranges_pairCompare_min( std::pair<int,gnss_pseudorange> a, std::pair<int,gnss_pseudorange> b)
@@ -146,12 +155,19 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
         {
             //d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,pseudoranges);
             // compute on the fly PVT solution
-            //std::cout<<"diff_clock_ephemerids="<<(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0<<"\r\n";
+            //std::cout<<"diff_clock_ephemeris="<<(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0<<"\r\n";
             if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,
                     d_ephemeris_clock_s+(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,
                     d_flag_averaging)==true)
                 {
                     d_kml_dump.print_position(d_ls_pvt,d_flag_averaging);
+                    if (!b_rinex_header_writen) //  & we have utc data in nav message!
+                        {
+                           // rinex_printer rinex_printer(d_last_nav_msg);
+                            rp->Rinex2NavHeader(rp->navFile, d_last_nav_msg);
+                            rp->Rinex2ObsHeader(rp->obsFile, d_last_nav_msg);
+                            b_rinex_header_writen=true; // do not write header anymore
+                        }
                 }
         }
 

src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h

@@ -63,6 +63,10 @@ private:
   // class private vars
   gr_msg_queue_sptr d_queue;
   bool d_dump;
+  bool b_rinex_header_writen;
+  //std::ofstream Rinex_Nav_File;
+  //std::ofstream Rinex_Obs_File;
+  rinex_printer *rp;
 
   unsigned int d_nchannels;
 

src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc

@@ -33,7 +33,7 @@
 #include "GPS_L1_CA.h"
 #include <glog/log_severity.h>
 #include <glog/logging.h>
-
+#include "boost/date_time/posix_time/posix_time.hpp"
 
 using google::LogMessage;
 
@@ -211,6 +211,10 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
     arma::vec obs=arma::zeros(d_nchannels); // pseudoranges observation vector
     arma::mat satpos=arma::zeros(3,d_nchannels); //satellite positions matrix
 
+    int GPS_week;
+    double GPS_corrected_time;
+    double utc;
+
     int valid_obs=0; //valid observations counter
     for (int i=0; i<d_nchannels; i++)
         {
@@ -227,9 +231,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
                             // d_ephemeris[i].master_clock(GPS_current_time); ?????
 
                              // compute the clock error including relativistic effects
-                            d_ephemeris[i].sv_clock_correction(GPS_current_time);
+                            GPS_corrected_time = d_ephemeris[i].sv_clock_correction(GPS_current_time);
+                            GPS_week = d_ephemeris[i].i_GPS_week;
+
+                            utc =d_ephemeris[i].utc_time(GPS_corrected_time);
+
                             // compute the satellite current ECEF position
-                            d_ephemeris[i].satellitePosition(GPS_current_time);
+                            d_ephemeris[i].satellitePosition(GPS_corrected_time);
 
                             satpos(0,i)=d_ephemeris[i].d_satpos_X;
                             satpos(1,i)=d_ephemeris[i].d_satpos_Y;
@@ -256,7 +264,11 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
             mypos=leastSquarePos(satpos,obs,W);
             LOG_AT_LEVEL(INFO) << "Position at TOW="<<GPS_current_time<<" in ECEF (X,Y,Z) = " << mypos << std::endl;
             cart2geo(mypos(0), mypos(1), mypos(2), 4);
-            std::cout << "Position at TOW="<<GPS_current_time<<" is Lat = " << d_latitude_d << " [deg] Long = "<< d_longitude_d <<" [deg] Height= "<<d_height_m<<" [m]" <<std::endl;
+
+            // Compute UTC time and print PVT solution
+            boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800*(double)GPS_week);
+            boost::posix_time::ptime p_time(boost::gregorian::date(1999,8,22),t);
+            std::cout << "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]" <<std::endl;
             // ######## LOG FILE #########
             if(d_flag_dump_enabled==true) {
                     // MULTIPLEXED FILE RECORDING - Record results to file

src/algorithms/PVT/libs/kml_printer.h

@@ -36,6 +36,7 @@
 #include <iostream>
 #include <fstream>
 #include "gps_l1_ca_ls_pvt.h"
+#include "gps_navigation_message.h"
 
 /*!
  * \brief Prints PVT information to OGC KML format file (can be viewed with Google Earth)

src/algorithms/PVT/libs/rinex_2_1_printer.cc

@@ -46,12 +46,23 @@
 using google::LogMessage;
 
 
+//std::ofstream getNavFileStream() {
+//    return navFile;
+//}
+
+//std::ofstream getObsFileStream() {
+ //   return obsFile;
+//}
+
+std::ofstream getObsFileStream() ;
+
 rinex_printer::rinex_printer()
 {
 
-    rinex_printer::navFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_GPS_NAV"));
+    rinex_printer::navFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_GPS_NAV"), std::ios::out | std::ios::app);
-    rinex_printer::obsFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_OBS"));
+    rinex_printer::obsFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_OBS"), std::ios::out | std::ios::app);
-    //rinex_printer::Rinex2NavHeader(rinex_printer::navFile, gps_navigation_message nav);
+    //rinex_printer::Rinex2NavHeader(rinex_printer::navFile, nav);
+    //rinex_printer::Rinex2ObsHeader(rinex_printer::navFile, nav);
 
     satelliteSystem["GPS"]="G";
     satelliteSystem["GLONASS"]="R";
@@ -62,7 +73,7 @@ rinex_printer::rinex_printer()
     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_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
@@ -529,7 +540,7 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out, gps_navigation_message n
     out << line << std::endl;
 
 
-    // -------- Line 6
+    // -------- Line OBSERVER / AGENCY
     line.clear();
     std::string username=getenv("USER");
     line += leftJustify(username,20);
@@ -540,7 +551,7 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out, gps_navigation_message n
 
 
 
-    // -------- Line 6 REC / TYPE VERS
+    // -------- Line  REC / TYPE VERS
     line.clear();
     line += rinex_printer::leftJustify("GNSS-SDR",20); // add flag and property
     line += rinex_printer::leftJustify("Software Receiver",20); // add flag and property
@@ -608,23 +619,22 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out, gps_navigation_message n
 
     // -------- 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;
+    boost::posix_time::ptime p_utc_time = rinex_printer::computeTime(nav_msg);
-    double month=1;
+
-    double day=4;
+    tm pt_utc_tm=boost::posix_time::to_tm(p_utc_time);
-    double hour=8;
+
-    double minute =43;
+    double seconds =(double)(pt_utc_tm.tm_sec);
-    double second = GPS_PI;
+
-    line += rightJustify(asString<short>(year),  6);
+    line += rightJustify(asString<short>(pt_utc_tm.tm_year+1900),  6);
-    line += rightJustify(asString<short>(month),  6);
+    line += rightJustify(asString<short>(pt_utc_tm.tm_mon),  6);
-    line += rightJustify(asString<short>(day),  6);
+    line += rightJustify(asString<short>(pt_utc_tm.tm_mday),  6);
-    line += rightJustify(asString<short>(hour),  6);
+    line += rightJustify(asString<short>(pt_utc_tm.tm_hour),  6);
-    line += rightJustify(asString<short>(minute),  6);
+    line += rightJustify(asString<short>(pt_utc_tm.tm_min),  6);
-    line += rightJustify(asString(second,7), 13);
+    line += rightJustify(asString(seconds,7), 13);
     line += rightJustify(std::string("GPS"),  8);
 
     line += rinex_printer::leftJustify("TIME OF FIRST OBS",20);
@@ -770,7 +780,15 @@ int rinex_printer::signalStrength(double snr)
 }
 
 
-
+boost::posix_time::ptime rinex_printer::computeTime(gps_navigation_message nav_msg)
+{
+    // if we are processing a file -> wait to leap second to resolve the ambiguity else take the week from the local system time
+    //: idea resolve the ambiguity with the leap second  http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm
+    double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW));
+    boost::posix_time::time_duration t = boost::posix_time::seconds(utc_t+ 604800*(double)(nav_msg.i_GPS_week));// should be i_WN_T?
+    boost::posix_time::ptime p_time(boost::gregorian::date(1999,8,22),t);
+    return p_time;
+}
 
 /*
 

src/algorithms/PVT/libs/rinex_2_1_printer.h

@@ -37,6 +37,7 @@
 #include <sstream>  // for stringstream
 #include <iomanip> // for setprecision
 #include "gps_navigation_message.h"
+#include "boost/date_time/posix_time/posix_time.hpp"
 
 /*!
  * \brief Class that handles the generation of Receiver
@@ -45,18 +46,9 @@
 class rinex_printer
 {
 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
@@ -262,13 +254,28 @@ private:
 
 
 
-
 public:
     /*!
      * \brief Default constructor. Creates GPS Navigation and Observables RINEX files and their headers
      */
     rinex_printer();
 
+    std::ofstream obsFile ;
+    std::ofstream navFile ;
+
+
+    /*!
+     *  \brief Generates the Navigation Data header
+     */
+    void Rinex2NavHeader(std::ofstream& out, gps_navigation_message nav);
+
+    /*!
+     *  \brief Generates the Observation data header
+     */
+    void Rinex2ObsHeader(std::ofstream& out, gps_navigation_message nav);
+
+    boost::posix_time::ptime computeTime(gps_navigation_message nav_msg);
+
     /*!
      * \brief Default destructor. Closes GPS Navigation and Observables RINEX files
      */

src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc

@@ -230,7 +230,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
 
 		  for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
 			{
-			// #### compute the pseudorrange for this satellite ###
+			// #### compute the pseudorange for this satellite ###
 
 		    current_prn_delay_ms=current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID);
 			traveltime_ms=current_prn_delay_ms-actual_min_prn_delay_ms+GPS_STARTOFFSET_ms; //[ms]
@@ -271,6 +271,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
 		  }
 	}
   consume_each(1); //one by one
+
   if ((d_sample_counter%d_output_rate_ms)==0)
     {
 	  return 1; //Output the observables

src/core/system_parameters/gps_navigation_message.cc

@@ -1,6 +1,6 @@
 /*!
  * \file gps_navigation_message.cc
- * \brief  Implementation of a GPS NAV Data message decoder
+ * \brief  Implementation of a GPS NAV Data message decoder as described in IS-GPS-200E
  *
  * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
  * \author Javier Arribas, 2011. jarribas(at)cttc.es
@@ -31,6 +31,8 @@
  */
 
 #include "gps_navigation_message.h"
+#include <math.h>
+#include "boost/date_time/posix_time/posix_time.hpp"
 
 #define num_of_slices(x) sizeof(x)/sizeof(bits_slice)
 
@@ -252,11 +254,13 @@ void gps_navigation_message::master_clock(double transmitTime)
 */
 
 // 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
-void gps_navigation_message::sv_clock_correction(double transmitTime)
+double 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;
+    double correctedTime = transmitTime - d_satClkCorr;
+    return correctedTime;
 }
 
 
@@ -285,18 +289,19 @@ void gps_navigation_message::satellitePosition(double transmitTime)
     // Restore semi-major axis
     a   = d_sqrt_A*d_sqrt_A;
 
-    // Time correction
+    // Time from ephemeris reference epoch
     tk  = check_t(transmitTime - d_Toe);
 
-    // Initial mean motion
+    // Computed mean motion
     n0  = sqrt(GM / (a*a*a));
-    // Mean motion
+
+    // Corrected mean motion
     n   = n0 + d_Delta_n;
 
     // Mean anomaly
     M   = d_M_0 + n * tk;
-    // Reduce mean anomaly to between 0 and 360 deg
 
+    // Reduce mean anomaly to between 0 and 2pi
     M   = fmod((M + 2*GPS_PI),(2*GPS_PI));
 
     // Initial guess of eccentric anomaly
@@ -318,12 +323,12 @@ void gps_navigation_message::satellitePosition(double transmitTime)
     // Compute relativistic correction term
     d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
 
-    // Calculate the true anomaly
+    // 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);
 
-    // Compute angle phi
+    // Compute angle phi (argument of Latitude)
     phi = nu + d_OMEGA;
 
     // Reduce phi to between 0 and 2*pi rad
@@ -341,6 +346,7 @@ void gps_navigation_message::satellitePosition(double transmitTime)
 
     // 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));
 
@@ -359,7 +365,7 @@ void gps_navigation_message::satellitePosition(double transmitTime)
      }
      */
 
-    // --- Compute satellite coordinates ------------------------------------
+    // --- 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);
@@ -654,6 +660,79 @@ 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));
+
+    // Determine if the effectivity time of the leap second event is in the past
+    int  weeksToLeapSecondEvent = i_WN_LSF-i_GPS_week;
+
+    if ((weeksToLeapSecondEvent) >= 0) // is not in the past
+        {
+            //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
+                             * Whenever the effectivity time indicated by the WN_LSF and the DN values
+                             * is not in the past (relative to the user's present time), and the user's
+                             * present time does not fall in the time span which starts at six hours prior
+                             * 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
+                        {
+                            /* 20.3.3.5.2.4b
+                             * Whenever the user's current time falls within the time span of six hours
+                             * prior to the effectivity time to six hours after the effectivity time,
+                             * 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)
+                        {
+                            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);
+                        }
+                }
+        }
+    else // the effectivity time is in the past
+        {
+            /* 20.3.3.5.2.4c
+             * Whenever the effectivity time of the leap second event, as indicated by the
+             * 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));
+            t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400);
+        }
+
+    double secondsOfWeekBeforeToday= 43200*floor(gpstime_corrected/43200);
+    t_utc = secondsOfWeekBeforeToday+t_utc_daytime;
+    return t_utc;
+
+}
+
+
 
 
 

src/core/system_parameters/gps_navigation_message.h

@@ -118,7 +118,7 @@ public:
     // Almanac
     double d_Toa;           //!< Almanac reference time [s]
     int i_WN_A;             //!< Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced
-    std::map<int,int> almanacHealth;
+    std::map<int,int> almanacHealth; //!< Map that stores the health information stored in the almanac
 
     // Flags
 
@@ -187,12 +187,6 @@ public:
      */
     int subframe_decoder(char *subframe);
 
-    /*
-     *  User Algorithm for SV Clock Correction
-     *
-     * Implementation of paragraph 20.3.3.3.3.1 (IS-GPS-200E)
-     */
-    //void master_clock(double transmitTime);
 
     /*!
      * \brief Computes the position of the satellite
@@ -202,10 +196,16 @@ public:
     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)
+     * \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction
+     * and returns the corrected clock (IS-GPS-200E,  20.3.3.3.3.1)
      */
-    void sv_clock_correction(double transmitTime);
+    double sv_clock_correction(double transmitTime);
 
+    /*!
+     * \brief Computes the Coordinated Universal Time (UTC) and
+     * returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4)
+     */
+    double utc_time(double gpstime_corrected);
 
     bool satellite_validation();
 

src/main/main.cc

@@ -62,7 +62,7 @@ int main(int argc, char** argv)
     const std::string intro_help(
             std::string("\nGNSS-SDR is an Open Source GNSS Software Defined Receiver\n")
     +
-    "Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)\n"
+    "Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)\n"
     +
     "This program comes with ABSOLUTELY NO WARRANTY;\n"
     +