Including SBAS output in rinex printer

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

src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc

@@ -42,6 +42,8 @@
 #include "boost/date_time/posix_time/posix_time.hpp"
 #include "gnss_synchro.h"
 #include "concurrent_map.h"
+#include "sbas_telemetry_data.h"
+#include "sbas_ionospheric_correction.h"
 
 using google::LogMessage;
 
@@ -49,6 +51,11 @@ extern concurrent_map<Gps_Ephemeris> global_gps_ephemeris_map;
 extern concurrent_map<Gps_Iono> global_gps_iono_map;
 extern concurrent_map<Gps_Utc_Model> global_gps_utc_model_map;
 
+extern concurrent_queue<Sbas_Raw_Msg> global_sbas_raw_msg_queue;
+extern concurrent_map<Sbas_Ionosphere_Correction> global_sbas_iono_map;
+extern concurrent_map<Sbas_Satellite_Correction> global_sbas_sat_corr_map;
+extern concurrent_map<Sbas_Ephemeris> global_sbas_ephemeris_map;
+
 gps_l1_ca_pvt_cc_sptr
 gps_l1_ca_make_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname)
 {
@@ -99,6 +106,7 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels,
     d_rx_time = 0.0;
 
     b_rinex_header_writen = false;
+    b_rinex_sbs_header_writen = false;
     rp = new Rinex_Printer();
 
     // ############# ENABLE DATA FILE LOG #################
@@ -173,6 +181,52 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
             global_gps_iono_map.read(0,d_ls_pvt->gps_iono);
         }
 
+    // update SBAS data collections
+    if (global_sbas_iono_map.size() > 0)
+        {
+            // SBAS ionosperic correction is shared for all the GPS satellites. Read always at ID=0
+            global_sbas_iono_map.read(0, d_ls_pvt->sbas_iono);
+        }
+    d_ls_pvt->sbas_sat_corr_map = global_sbas_sat_corr_map.get_map_copy();
+    d_ls_pvt->sbas_ephemeris_map = global_sbas_ephemeris_map.get_map_copy();
+
+    // read SBAS raw messages directly from queue and write them into rinex file
+    Sbas_Raw_Msg sbas_raw_msg;
+    while (global_sbas_raw_msg_queue.try_pop(sbas_raw_msg))
+        {
+            // create the header of not yet done
+            if(!b_rinex_sbs_header_writen)
+                {
+                    rp->rinex_sbs_header(rp->sbsFile);
+                    b_rinex_sbs_header_writen = true;
+                }
+
+            // Define the RX time of the SBAS message by using the GPS time.
+            // It has only an effect if there has not been yet a SBAS MT12 available
+            // when the message was received.
+            if(sbas_raw_msg.get_rx_time_obj().is_related() == false
+                    && gnss_pseudoranges_map.size() > 0
+                    && d_ls_pvt->gps_ephemeris_map.size() > 0)
+                {
+                    // doesn't matter which channel/satellite we choose
+                    Gnss_Synchro gs = gnss_pseudoranges_map.begin()->second;
+                    Gps_Ephemeris eph = d_ls_pvt->gps_ephemeris_map.begin()->second;
+
+                    double relative_rx_time = gs.Tracking_timestamp_secs;
+                    int gps_week = eph.i_GPS_week;
+                    double gps_sec = gs.d_TOW_at_current_symbol;
+
+                    Sbas_Time_Relation time_rel(relative_rx_time,gps_week,gps_sec);
+                    sbas_raw_msg.relate(time_rel);
+                }
+
+            // send the message to the rinex logger if it has a valid GPS time stamp
+            if(sbas_raw_msg.get_rx_time_obj().is_related())
+                {
+                    rp->log_rinex_sbs(rp->sbsFile, sbas_raw_msg);
+                }
+        }
+
     // ############ 2 COMPUTE THE PVT ################################
     if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() >0)
         {

src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h

@@ -65,6 +65,7 @@ private:
     boost::shared_ptr<gr::msg_queue> d_queue;
     bool d_dump;
     bool b_rinex_header_writen;
+    bool b_rinex_sbs_header_writen;
     Rinex_Printer *rp;
     unsigned int d_nchannels;
     std::string d_dump_filename;

src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h

@@ -38,7 +38,6 @@
 #include <stdio.h>
 #include <sys/time.h>
 #include <time.h>
-//#include <math.h>
 #include <cmath>
 #include <map>
 #include <algorithm>
@@ -51,6 +50,11 @@
 #include "gps_ephemeris.h"
 #include "gps_utc_model.h"
 
+#include "sbas_telemetry_data.h"
+#include "sbas_ionospheric_correction.h"
+#include "sbas_satellite_correction.h"
+#include "sbas_ephemeris.h"
+
 #define PVT_MAX_CHANNELS 24
 
 /*!
@@ -80,6 +84,10 @@ public:
     Gps_Utc_Model gps_utc_model;
     // new iono storage
     Gps_Iono gps_iono;
+    // new SBAS storage
+    Sbas_Ionosphere_Correction sbas_iono;
+    std::map<int,Sbas_Satellite_Correction> sbas_sat_corr_map;
+    std::map<int,Sbas_Ephemeris> sbas_ephemeris_map;
 
     double d_GPS_current_time;
     boost::posix_time::ptime d_position_UTC_time;

src/algorithms/PVT/libs/rinex_printer.cc

@@ -34,6 +34,7 @@
 #include "gps_iono.h"
 #include "gps_utc_model.h"
 #include "GPS_L1_CA.h"
+#include "sbas_telemetry_data.h"
 #include <ostream>
 #include <fstream>
 #include <stdlib.h>  // for getenv()
@@ -60,9 +61,11 @@ Rinex_Printer::Rinex_Printer()
 {
     navfilename = Rinex_Printer::createFilename("RINEX_FILE_TYPE_GPS_NAV");
     obsfilename = Rinex_Printer::createFilename("RINEX_FILE_TYPE_OBS");
+    sbsfilename = Rinex_Printer::createFilename("RINEX_FILE_TYPE_SBAS");
 
     Rinex_Printer::navFile.open(navfilename, std::ios::out | std::ios::app);
     Rinex_Printer::obsFile.open(obsfilename, std::ios::out | std::ios::app);
+    Rinex_Printer::sbsFile.open(sbsfilename, std::ios::out | std::ios::app);
 
     // RINEX v3.00 codes
     satelliteSystem["GPS"] = "G";
@@ -140,8 +143,6 @@ Rinex_Printer::Rinex_Printer()
     observationType["SIGNAL_STRENGTH_v2"] = "S";
     observationCode["GPS_L1_CA_v2"] = "1";
 
-
-
     if ( FLAGS_RINEX_version.compare("3.01") == 0 )
         {
             version = 3;
@@ -170,11 +171,13 @@ Rinex_Printer::Rinex_Printer()
 Rinex_Printer::~Rinex_Printer()
 {
     // close RINEX files
-    long posn, poso;
+    long posn, poso, poss;
     posn = navFile.tellp();
     poso = obsFile.tellp();
+    poss = obsFile.tellp();
     Rinex_Printer::navFile.close();
     Rinex_Printer::obsFile.close();
+    Rinex_Printer::sbsFile.close();
     // If nothing written, erase the files.
     if (posn == 0)
         {
@@ -184,6 +187,10 @@ Rinex_Printer::~Rinex_Printer()
         {
             remove(obsfilename.c_str());
         }
+    if (poss == 0)
+        {
+            remove(sbsfilename.c_str());
+        }
 }
 
 
@@ -522,6 +529,94 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Iono iono, Gps_Utc_
 
 
 
+void Rinex_Printer::rinex_sbs_header(std::ofstream& out)
+{
+    std::string line;
+
+    // -------- Line 1
+    line.clear();
+    line = std::string(5, ' ');
+    line += std::string("2.10");
+    line += std::string(11, ' ');
+    line += Rinex_Printer::leftJustify("B SBAS DATA",20);
+    line += std::string(20, ' ');
+    line += std::string("RINEX VERSION / TYPE");
+
+    Rinex_Printer::lengthCheck(line);
+    out << line << std::endl;
+
+    // -------- Line 2
+    line.clear();
+    line += Rinex_Printer::leftJustify("GNSS-SDR", 20);
+    line += Rinex_Printer::leftJustify("CTTC", 20);
+    // Date of file creation (dd-mmm-yy hhmm)
+    boost::local_time::time_zone_ptr zone(new boost::local_time::posix_time_zone("UTC"));
+    boost::local_time::local_date_time pt = boost::local_time::local_sec_clock::local_time(zone);
+    tm pt_tm = boost::local_time::to_tm(pt);
+    std::stringstream strYear;
+    int utc_year = pt.date().year();
+    utc_year -= 2000; //  two digits for year
+    strYear << utc_year;
+    std::stringstream strMonth;
+    int utc_month = pt.date().month().as_number();
+    if (utc_month < 10) strMonth << "0"; //  two digits for months
+    strMonth << utc_month;
+    std::stringstream strmDay;
+    int utc_day = pt.date().day().as_number();
+    if (utc_day < 10) strmDay << "0"; //  two digits for days
+    strmDay << utc_day;
+    std::stringstream strmHour;
+    int utc_hour = pt_tm.tm_hour;
+    if (utc_hour < 10) strmHour << "0"; //  two digits for hours
+    strmHour << utc_hour;
+    std::stringstream strmMin;
+    int utc_minute = pt_tm.tm_min;
+    if (utc_minute < 10) strmMin << "0"; //  two digits for minutes
+    strmMin << utc_minute;
+    std::string time_str;
+    time_str += strmDay.str();
+    time_str += "-";
+    time_str += strMonth.str();
+    time_str += "-";
+    time_str += strYear.str();
+    time_str += " ";
+    time_str += strmHour.str();
+    time_str += strmMin.str();
+    line += Rinex_Printer::leftJustify(time_str, 20);
+    line += Rinex_Printer::leftJustify("PGM / RUN BY / DATE", 20);
+    Rinex_Printer::lengthCheck(line);
+    out << line << std::endl;
+
+    // -------- Line 3
+    line.clear();
+    line += std::string(60, ' ');
+    line += Rinex_Printer::leftJustify("REC INDEX/TYPE/VERS", 20);
+    Rinex_Printer::lengthCheck(line);
+    out << line << std::endl;
+
+    // -------- Line COMMENT 1
+    line.clear();
+    line += Rinex_Printer::leftJustify("BROADCAST DATA FILE FOR GEO SV, GENERATED BY GNSS-SDR", 60);
+    line += Rinex_Printer::leftJustify("COMMENT", 20);
+    Rinex_Printer::lengthCheck(line);
+    out << line << std::endl;
+
+    // -------- Line COMMENT 2
+    line.clear();
+    line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60);
+    line += Rinex_Printer::leftJustify("COMMENT", 20);
+    Rinex_Printer::lengthCheck(line);
+    out << line << std::endl;
+
+    // -------- End of Header
+    line.clear();
+    line += std::string(60, ' ');
+    line += Rinex_Printer::leftJustify("END OF HEADER", 20);
+    Rinex_Printer::lengthCheck(line);
+    out << line << std::endl;
+}
+
+
 
 
 void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris> eph_map)
@@ -601,8 +696,6 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris
             out << line << std::endl;
 
 
-
-
             // -------- BROADCAST ORBIT - 1
             line.clear();
 
@@ -1252,6 +1345,163 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
 
 
 
+// represents GPS time (week, TOW) in the date time format of the Gregorian calendar.
+// -> Leap years are considered, but leap seconds not.
+void Rinex_Printer::to_date_time(int gps_week, int gps_tow, int &year, int &month, int &day, int &hour, int &minute, int &second)
+{
+    //std::cout << "to_date_time(): gps_week=" << gps_week << " gps_tow=" << gps_tow << std::endl;
+
+    int days_per_month[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
+
+    // seconds in a not leap year
+    const int secs_per_day = 24*60*60;
+    const int secs_per_week = 7*secs_per_day;
+    const int secs_per_normal_year = 365*secs_per_day;
+    const int secs_per_leap_year = secs_per_normal_year + secs_per_day;
+
+    // the GPS epoch is 06.01.1980 00:00, i.e. midnight 5. / 6. January 1980
+    // -> seconds since then
+    int secs_since_gps_epoch = gps_week*secs_per_week + gps_tow;
+
+    // find year, consider leap years
+    bool is_leap_year;
+    int remaining_secs = secs_since_gps_epoch + 5*secs_per_day;
+    for (int y = 1980; true ; y++)
+        {
+            is_leap_year = y%4 == 0 && (y%100 != 0 || y%400 == 0);
+            int secs_in_year_y = is_leap_year ? secs_per_leap_year : secs_per_normal_year;
+
+            if (secs_in_year_y <= remaining_secs)
+                {
+                    remaining_secs -= secs_in_year_y;
+                }
+            else
+                {
+                    year = y;
+                    //std::cout << "year: year=" << year << " secs_in_year_y="<< secs_in_year_y << " remaining_secs="<< remaining_secs << std::endl;
+                    break;
+                }
+            //std::cout << "year: y=" << y << " secs_in_year_y="<< secs_in_year_y << " remaining_secs="<< remaining_secs << std::endl;
+        }
+
+    // find month
+    for (int m = 1; true ; m++)
+        {
+            int secs_in_month_m = days_per_month[m-1]*secs_per_day;
+            if (is_leap_year && m == 2 ) // consider February of leap year
+                {
+                    secs_in_month_m += secs_per_day;
+                }
+
+            if (secs_in_month_m <= remaining_secs)
+                {
+                    remaining_secs -= secs_in_month_m;
+                }
+            else
+                {
+                    month = m;
+                    //std::cout << "month: month=" << month << " secs_in_month_m="<< secs_in_month_m << " remaining_secs="<< remaining_secs << std::endl;
+                    break;
+                }
+            //std::cout << "month: m=" << m << " secs_in_month_m="<< secs_in_month_m << " remaining_secs="<< remaining_secs << std::endl;
+        }
+
+    day = remaining_secs/secs_per_day+1;
+    remaining_secs = remaining_secs%secs_per_day;
+
+    hour = remaining_secs/(60*60);
+    remaining_secs = remaining_secs%(60*60);
+
+    minute = remaining_secs/60;
+    second = remaining_secs%60;
+}
+
+
+
+void Rinex_Printer::log_rinex_sbs(std::ofstream& out, Sbas_Raw_Msg sbs_message)
+{
+    // line 1: PRN / EPOCH / RCVR
+    std::stringstream line1;
+
+    // SBAS PRN
+    line1 << sbs_message.get_prn();
+    line1 << " ";
+
+    // gps time of reception
+    int gps_week;
+    double gps_sec;
+    if(sbs_message.get_rx_time_obj().get_gps_time(gps_week, gps_sec))
+    {
+        int year;
+        int month;
+        int day;
+        int hour;
+        int minute;
+        int second;
+
+        double gps_sec_one_digit_precicion = round(gps_sec *10)/10; // to prevent rounding towards 60.0sec in the stream output
+        int gps_tow = trunc(gps_sec_one_digit_precicion);
+        double sub_sec = gps_sec_one_digit_precicion - double(gps_tow);
+
+        to_date_time(gps_week, gps_tow, year, month, day, hour, minute, second);
+        line1 << asFixWidthString(year, 2, '0') <<  " " << asFixWidthString(month, 2, '0') <<  " " << asFixWidthString(day, 2, '0') <<  " " << asFixWidthString(hour, 2, '0') <<  " " << asFixWidthString(minute, 2, '0') <<  " " << rightJustify(asString(double(second)+sub_sec,1),4,' ');
+    }
+    else
+    {
+        line1 << std::string(19, ' ');
+    }
+    line1 << "  ";
+
+    // band
+    line1 << "L1";
+    line1 << "   ";
+
+    // Length of data message (bytes)
+    line1 <<  asFixWidthString(sbs_message.get_msg().size(), 3, ' ');
+    line1 << "   ";
+    // File-internal receiver index
+    line1 << "  0";
+    line1 << "   ";
+    // Transmission System Identifier
+    line1 << "SBA";
+    line1 << std::string(35, ' ');
+    lengthCheck(line1.str());
+    out << line1.str() << std::endl;
+
+    // DATA RECORD - 1
+    std::stringstream line2;
+    line2 << " ";
+    // Message frame identifier
+    if (sbs_message.get_msg_type() < 10) line2 << " ";
+    line2 << sbs_message.get_msg_type();
+    line2 << std::string(4, ' ');
+    // First 18 bytes of message (hex)
+    std::vector<unsigned char> msg = sbs_message.get_msg();
+    for (size_t i = 0; i < 18 && i <  msg.size(); ++i)
+    {
+        line2 << std::hex << std::setfill('0') << std::setw(2);
+        line2 << int(msg[i]) << " ";
+    }
+    line2 << std::string(19, ' ');
+    lengthCheck(line2.str());
+    out << line2.str() << std::endl;
+
+    // DATA RECORD - 2
+    std::stringstream line3;
+    line3 << std::string(7, ' ');
+    // Remaining bytes of message (hex)
+    for (size_t i = 18; i < 36 && i <  msg.size(); ++i)
+    {
+        line3 << std::hex << std::setfill('0') << std::setw(2);
+        line3 << int(msg[i]) << " ";
+    }
+    line3 << std::string(31, ' ');
+    lengthCheck(line3.str());
+    out << line3.str() << std::endl;
+}
+
+
+
 
 int Rinex_Printer::signalStrength(double snr)
 {
@@ -1303,3 +1553,6 @@ enum RINEX_enumMarkerType {
 
  */
 
+inline std::string Rinex_Printer::asString(const long double x,
+                const std::string::size_type precision) {
+}

src/algorithms/PVT/libs/rinex_printer.h

@@ -61,6 +61,8 @@
 #include "GPS_L1_CA.h"
 #include "gnss_synchro.h"
 
+class Sbas_Raw_Msg;
+
 /*!
  * \brief Class that handles the generation of Receiver
  * INdependent EXchange format (RINEX) files
@@ -80,6 +82,7 @@ public:
 
     std::ofstream obsFile ; //<! Output file stream for RINEX observation file
     std::ofstream navFile ; //<! Output file stream for RINEX navigation data file
+    std::ofstream sbsFile ; //<! Output file stream for RINEX SBAS raw data file
 
     /*!
      *  \brief Generates the Navigation Data header
@@ -91,6 +94,11 @@ public:
      */
     void rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, double d_TOW_first_observation);
 
+    /*!
+     *  \brief Generates the SBAS raw data header
+     */
+    void rinex_sbs_header(std::ofstream& out);
+
     /*!
      *  \brief Computes the UTC time and returns a boost::posix_time::ptime object
      */
@@ -111,6 +119,17 @@ public:
      *  \brief Writes observables into the RINEX file
      */
     void log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double obs_time, std::map<int,Gnss_Synchro> pseudoranges);
+
+    /*!
+     * \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not.
+     */
+    void to_date_time(int gps_week, int gps_tow, int &year, int &month, int &day, int &hour, int &minute, int &second);
+
+    /*!
+     *  \brief Writes raw SBAS messages into the RINEX file
+     */
+    void log_rinex_sbs(std::ofstream& out, Sbas_Raw_Msg sbs_message);
+
     std::map<std::string,std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass
     std::map<std::string,std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH
     std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors
@@ -144,6 +163,7 @@ private:
 
     std::string navfilename;
     std::string obsfilename;
+    std::string sbsfilename;
 
     /*
      * Generates the data for the PGM / RUN BY / DATE line
@@ -320,6 +340,7 @@ private:
      */
     template <class X>
     inline std::string asString(const X x);
+    inline std::string asFixWidthString(const int x, const int width, char fill_digit);
 };
 
 
@@ -521,6 +542,15 @@ inline std::string Rinex_Printer::asString(const double x, const std::string::si
 }
 
 
+inline std::string Rinex_Printer::asFixWidthString(const int x, const int width, char fill_digit)
+{
+    std::ostringstream ss;
+    ss << std::setfill(fill_digit) << std::setw(width) << x;
+    //std::cout << "asFixWidthString(): x=" << x << " width=" << width << " fill_digit=" << fill_digit << " ss=" << ss.str() << std::endl;
+    return ss.str().substr(ss.str().size() - width);
+}
+
+
 template<class X>
 inline std::string Rinex_Printer::asString(const X x)
 {