gps_pvt 0.1.4 → 0.2.0

data/ext/ninja-scan-light/tool/navigation/RINEX.h

@@ -50,8 +50,10 @@
 #include <algorithm>
 #include <cstddef>
 #include <cstring>
+#include <limits>
 
 #include "GPS.h"
+#include "SBAS.h"
 
 template <class U = void>
 class RINEX_Reader {
@@ -180,22 +182,27 @@ class RINEX_Reader {
     const header_t &header() const {return const_cast<self_t *>(this)->header();}
     bool has_next() const {return _has_next;}
 
-    template <class T>
+    template <class T, bool is_integer = std::numeric_limits<T>::is_integer>
     struct conv_t {
-      static void d(
+      static bool d(
           std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
         if(str2val){
-          std::stringstream(buf.substr(offset, length)) >> *(T *)value;
+          std::stringstream ss(buf.substr(offset, length));
+          ss >> *(T *)value;
+          return (ss.rdstate() & std::ios_base::failbit) == 0;
         }else{
           std::stringstream ss;
           ss << std::setfill(opt == 1 ? '0' : ' ') << std::right << std::setw(length) << *(T *)value;
           buf.replace(offset, length, ss.str());
+          return true;
         }
       }
-      static void f(
+      static bool f(
           std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
         if(str2val){
-          std::stringstream(buf.substr(offset, length)) >> *(T *)value;
+          std::stringstream ss(buf.substr(offset, length));
+          ss >> *(T *)value;
+          return (ss.rdstate() & std::ios_base::failbit) == 0;
         }else{
           std::stringstream ss;
           ss << std::setfill(' ') << std::right << std::setw(length)
@@ -215,9 +222,10 @@ class RINEX_Reader {
             }
           }
           buf.replace(offset, length, s);
+          return true;
         }
       }
-      static void e(
+      static bool e(
           std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
         if(str2val){
           std::string s(buf.substr(offset, length));
@@ -225,7 +233,9 @@ class RINEX_Reader {
           if(pos != std::string::npos){
             s.replace(pos, 1, "E");
           }
-          std::stringstream(s) >> *(T *)value;
+          std::stringstream ss(s);
+          ss >> *(T *)value;
+          return (ss.rdstate() & std::ios_base::failbit) == 0;
         }else{
           int w((std::max)(length, precision + 6)); // parentheses of std::max mitigates error C2589 under Windows VC
 
@@ -247,12 +257,36 @@ class RINEX_Reader {
           ss.str("");
           ss << std::setfill(' ') << std::right << std::setw(w) << s;
           buf.replace(offset, length, ss.str());
+          return true;
         }
       }
     };
+    template <class T>
+    struct conv_t<T, true> {
+      static bool d(
+          std::string &buf, const int &offset, const int &length, void *value, const int &opt = 0, const bool &str2val = true){
+        return conv_t<T, false>::d(buf, offset, length, value, opt, str2val);
+      }
+      static bool f(
+          std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
+        double v(*(T *)value);
+        bool res(
+            conv_t<double, false>::f(buf, offset, length, &v, precision, str2val));
+        *(T *)value = static_cast<T>(v);
+        return res;
+      }
+      static bool e(
+          std::string &buf, const int &offset, const int &length, void *value, const int &precision = 0, const bool &str2val = true){
+        double v(*(T *)value);
+        bool res(
+            conv_t<double, false>::e(buf, offset, length, &v, precision, str2val));
+        *(T *)value = static_cast<T>(v);
+        return res;
+      }
+    };
 
     struct convert_item_t {
-      void (*func)(
+      bool (*func)(
           std::string &buf, const int &offset, const int &length, void *value,
           const int &opt, const bool &str2val);
       int offset;
@@ -261,17 +295,30 @@ class RINEX_Reader {
       int opt;
     };
 
-    static void convert(const convert_item_t *items, const int &size, const std::string &buf, void *values){
+    /**
+     * @param recovery if conversion fails, then this functor is invoked.
+     * If recovery is successfully performed, this functor should return true.
+     * The return value of this function reflects it.
+     * @return (bool) if all conversion are successfully performed, true is returned; otherwise false.
+     */
+    static bool convert(
+        const convert_item_t *items, const int &size, const std::string &buf, void *values,
+        bool (*recovery)(const int &, const std::string &, void *) = NULL){
       // str => value
+      bool res(true);
       for(int i(0); i < size; ++i){
-        (*items[i].func)(
+        if((*items[i].func)(
             const_cast<std::string &>(buf), items[i].offset, items[i].length, (char *)values + items[i].value_offset,
-            items[i].opt, true);
+            items[i].opt, true)){continue;}
+        res &= (recovery ? (*recovery)(i, buf, values) : false);
       }
+      return res;
     }
     template <int N>
-    static inline void convert(const convert_item_t (&items)[N], const std::string &buf, void *values){
-      convert(items, N, buf, values);
+    static inline bool convert(
+        const convert_item_t (&items)[N], const std::string &buf, void *values,
+        bool (*recovery)(const int &, const std::string &, void *) = NULL){
+      return convert(items, N, buf, values, recovery);
     }
 };
 
@@ -404,10 +451,8 @@ struct RINEX_NAV {
     std::tm t_oc_tm;
     int t_oc_year4, t_oc_year2, t_oc_mon12;
     FloatT t_oc_sec;
-    FloatT iodc_f, iode_f; // originally int type
     FloatT t_oe_WN;
     FloatT ura_meter;
-    FloatT SV_health_f;
     FloatT t_ot;  ///< Transmitting time [s]
     FloatT fit_interval_hr;
     FloatT dummy;
@@ -420,10 +465,8 @@ struct RINEX_NAV {
         t_oc_year2(t_oc_tm.tm_year % 100),
         t_oc_mon12(t_oc_tm.tm_mon + 1),
         t_oc_sec(std::fmod(eph.t_oc, 60)),
-        iodc_f(eph.iodc), iode_f(eph.iode),
         t_oe_WN(eph.WN),
         ura_meter(ephemeris_t::URA_meter(eph.URA)),
-        SV_health_f(((eph.SV_health & 0x20) && (eph.SV_health & 0x1F == 0)) ? 1 : eph.SV_health),
         t_ot(0), // TODO
         fit_interval_hr(eph.fit_interval / (60 * 60)),
         dummy(0) {
@@ -434,9 +477,6 @@ struct RINEX_NAV {
       eph.WN = t_oc.week;
       eph.t_oc = t_oc.seconds;
 
-      eph.iodc = iodc_f;
-      eph.iode = iode_f;
-
       eph.URA = ephemeris_t::URA_index(ura_meter); // meter to index
 
       /* @see ftp://igs.org/pub/data/format/rinex210.txt
@@ -444,17 +484,69 @@ struct RINEX_NAV {
        * RINEX Value:   0    Health OK
        * RINEX Value:   1    Health not OK (bits 18-22 not stored)
        * RINEX Value: >32    Health not OK (bits 18-22 stored)
+       * eph.SV_health may have a value greater than 32
        */
-      eph.SV_health = (unsigned int)SV_health_f;
-      if(eph.SV_health > 32){
-        eph.SV_health &= 0x3F; // 0b111111
-      }else if(eph.SV_health > 0){
-        eph.SV_health = 0x20; // 0b100000
-      }
 
       // At least 4 hour validity, then, hours => seconds;
       eph.fit_interval = ((fit_interval_hr < 4) ? 4 : fit_interval_hr) * 60 * 60;
     }
+    static message_t from_qzss(const ephemeris_t &eph_){
+      message_t res(eph_);
+      res.eph.svid -= 192;
+      res.fit_interval_hr = (res.fit_interval_hr) > 2 ? 1 : 0;
+      return res;
+    }
+    ephemeris_t eph_qzss() const {
+      ephemeris_t res(eph);
+      res.svid += 192;
+      res.fit_interval = ((fit_interval_hr > 0) ? 4 : 2) * 60 * 60;
+      return res;
+    }
+  };
+  struct message_sbas_t {
+    typedef typename SBAS_SpaceNode<FloatT>
+        ::SatelliteProperties::Ephemeris eph_t;
+    int svid;
+    std::tm date_tm;
+    int t_year4, t_year2, t_mon12;
+    FloatT t_sec;
+    FloatT a_Gf0, a_Gf1;
+    FloatT t_t; // Transmission time of message (start of the message) in GPS seconds of the week
+    FloatT x_km, dx_km_s, ddx_km_s2;
+    FloatT y_km, dy_km_s, ddy_km_s2;
+    FloatT z_km, dz_km_s, ddz_km_s2;
+    unsigned int health;
+    FloatT URA;
+    unsigned int iodn;
+    message_sbas_t() {}
+    message_sbas_t(const eph_t &eph)
+        : svid((int)eph.svid - 100),
+        date_tm(eph.base_time().c_tm()),
+        t_year4(date_tm.tm_year + 1900),
+        t_year2(date_tm.tm_year % 100),
+        t_mon12(date_tm.tm_mon + 1),
+        t_sec(date_tm.tm_sec),
+        a_Gf0(eph.a_Gf0), a_Gf1(eph.a_Gf1),
+        t_t(eph.t_0), // TODO maybe differ from t_e slightly
+        x_km(1E-3 * eph.x), dx_km_s(1E-3 * eph.dx), ddx_km_s2(1E-3 * eph.ddx),
+        y_km(1E-3 * eph.y), dy_km_s(1E-3 * eph.dy), ddy_km_s2(1E-3 * eph.ddy),
+        z_km(1E-3 * eph.z), dz_km_s(1E-3 * eph.dz), ddz_km_s2(1E-3 * eph.ddz),
+        health(0), URA(eph.URA), iodn(0) {
+    }
+    operator eph_t() const {
+      eph_t eph = {0};
+      eph.svid = (unsigned int)svid + 100;
+      typename space_node_t::gps_time_t t(date_tm);
+      t += (t_sec - date_tm.tm_sec);
+      eph.WN = t.week;
+      eph.t_0 = t.seconds;
+      eph.a_Gf0 = a_Gf0; eph.a_Gf1 = a_Gf1;
+      eph.x = 1E3 * x_km; eph.dx = 1E3 * dx_km_s; eph.ddx = 1E3 * ddx_km_s2;
+      eph.y = 1E3 * y_km; eph.dy = 1E3 * dy_km_s; eph.ddy = 1E3 * ddy_km_s2;
+      eph.z = 1E3 * z_km; eph.dz = 1E3 * dz_km_s; eph.ddz = 1E3 * ddz_km_s2;
+      eph.URA = URA;
+      return eph;
+    }
   };
 };
 
@@ -466,6 +558,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
   public:
     typedef typename RINEX_NAV<FloatT>::space_node_t space_node_t;
     typedef typename RINEX_NAV<FloatT>::message_t message_t;
+    typedef typename RINEX_NAV<FloatT>::message_sbas_t message_sbas_t;
     typedef typename space_node_t::Ionospheric_UTC_Parameters iono_utc_t;
 
     static const typename super_t::convert_item_t eph0_v2[10], eph0_v3[10];
@@ -477,14 +570,22 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
     static const typename super_t::convert_item_t eph6_v2[4], eph6_v3[4];
     static const typename super_t::convert_item_t eph7_v2[2], eph7_v3[2];
 
+    static const typename super_t::convert_item_t eph0_sbas_v2[10], eph0_sbas_v3[10];
+    static const typename super_t::convert_item_t eph1_sbas_v2[4], eph1_sbas_v3[4];
+    static const typename super_t::convert_item_t eph2_sbas_v2[4], eph2_sbas_v3[4];
+    static const typename super_t::convert_item_t eph3_sbas_v2[4], eph3_sbas_v3[4];
+
   protected:
+    typename super_t::version_type_t::sat_system_t sys_of_msg;
     message_t msg;
+    message_sbas_t msg_sbas;
     
-    void seek_next_v2() {
+    void seek_next_v2_gps() {
       char buf[256];
 
-      for(int i = 0; (i < 8) && (super_t::src.good()); i++){
-        if(super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      for(int i(0); i < 8; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
         std::string line(buf);
 
         switch(i){
@@ -505,25 +606,69 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
         }
       }
       msg.update();
+      sys_of_msg = super_t::version_type_t::SYS_GPS;
       super_t::_has_next = true;
     }
 
-    void seek_next_v3() {
+    void seek_next_v2_glonass() {
       char buf[256];
 
-      for(int i = 0; (i < 8) && (super_t::src.good()); i++){
-        if(super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      for(int i(0); i < 4; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      }
+      //sys_of_msg = super_t::version_type_t::SYS_GLONASS; // TODO currently not implemented
+      sys_of_msg = super_t::version_type_t::SYS_UNKNOWN;
+      super_t::_has_next = true;
+    }
+
+    void seek_next_v2_sbas() {
+      char buf[256];
+
+      for(int i(0); i < 4; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
         std::string line(buf);
 
         switch(i){
           case 0: {
-            // if(line_data[0] != 'G'){} // TODO check GPS before parsing
-            super_t::convert(eph0_v3, line, &msg);
-            msg.t_oc_tm.tm_year = msg.t_oc_year4 - 1900; // tm_year base is 1900
-            msg.t_oc_tm.tm_mon = msg.t_oc_mon12 - 1; // month [0, 11]
-            msg.t_oc_sec = msg.t_oc_tm.tm_sec;
+            super_t::convert(eph0_sbas_v2, line, &msg_sbas);
+            msg_sbas.date_tm.tm_year = msg_sbas.t_year2 + (msg_sbas.t_year2 < 80 ? 100 : 0); // greater than 1980
+            msg_sbas.date_tm.tm_mon = msg_sbas.t_mon12 - 1; // month [0, 11]
+            msg_sbas.date_tm.tm_sec = (int)msg_sbas.t_sec;
             break;
           }
+          case 1: super_t::convert(eph1_sbas_v2, line, &msg_sbas); break;
+          case 2: super_t::convert(eph2_sbas_v2, line, &msg_sbas); break;
+          case 3: super_t::convert(eph3_sbas_v2, line, &msg_sbas); break;
+        }
+      }
+      sys_of_msg = super_t::version_type_t::SYS_SBAS;
+      super_t::_has_next = true;
+    }
+
+    void seek_next_v2() {
+      switch(super_t::version_type.sat_system){
+        case super_t::version_type_t::SYS_GPS: seek_next_v2_gps(); return;
+        case super_t::version_type_t::SYS_GLONASS: seek_next_v2_glonass(); return;
+        case super_t::version_type_t::SYS_SBAS: seek_next_v2_sbas(); return;
+        default: break;
+      }
+    }
+
+    template <std::size_t N>
+    void seek_next_v3_gps(char (&buf)[N]) {
+      super_t::convert(eph0_v3, std::string(buf), &msg);
+      msg.t_oc_tm.tm_year = msg.t_oc_year4 - 1900; // tm_year base is 1900
+      msg.t_oc_tm.tm_mon = msg.t_oc_mon12 - 1; // month [0, 11]
+      msg.t_oc_sec = msg.t_oc_tm.tm_sec;
+
+      for(int i(1); i < 8; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+        std::string line(buf);
+
+        switch(i){
           case 1: super_t::convert(eph1_v3, line, &msg); break;
           case 2: super_t::convert(eph2_v3, line, &msg); break;
           case 3: super_t::convert(eph3_v3, line, &msg); break;
@@ -534,10 +679,71 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
         }
       }
       msg.update();
+      sys_of_msg = super_t::version_type_t::SYS_GPS;
+      super_t::_has_next = true;
+    }
+
+    template <std::size_t N>
+    void seek_next_v3_sbas(char (&buf)[N]) {
+      super_t::convert(eph0_sbas_v3, std::string(buf), &msg_sbas);
+      msg_sbas.date_tm.tm_year = msg_sbas.t_year4 - 1900; // tm_year base is 1900
+      msg_sbas.date_tm.tm_mon = msg_sbas.t_mon12 - 1; // month [0, 11]
+      msg_sbas.t_sec = msg_sbas.date_tm.tm_sec;
+
+      for(int i(1); i < 4; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+        std::string line(buf);
+
+        switch(i){
+          case 1: super_t::convert(eph1_sbas_v3, line, &msg_sbas); break;
+          case 2: super_t::convert(eph2_sbas_v3, line, &msg_sbas); break;
+          case 3: super_t::convert(eph3_sbas_v3, line, &msg_sbas); break;
+        }
+      }
+      sys_of_msg = super_t::version_type_t::SYS_SBAS;
       super_t::_has_next = true;
     }
 
+    template <std::size_t N>
+    void seek_next_v3_qzss(char (&buf)[N]) {
+      seek_next_v3_gps(buf);
+      if(!super_t::_has_next){return;}
+      sys_of_msg = super_t::version_type_t::SYS_QZSS;
+    }
+
+    template <std::size_t N>
+    void seek_next_v3_not_implemented(char (&buf)[N], const int &lines) {
+      for(int i(1); i < lines; i++){
+        if((!super_t::src.good())
+            || super_t::src.getline(buf, sizeof(buf)).fail()){return;}
+      }
+      sys_of_msg = super_t::version_type_t::SYS_UNKNOWN;
+      super_t::_has_next = true;
+    }
+
+    void seek_next_v3() {
+      char buf[256];
+
+      while(super_t::src.good()
+          && (!super_t::src.getline(buf, sizeof(buf)).fail())){
+
+        switch(buf[0]){
+          case 'G': seek_next_v3_gps(buf); return; // GPS
+          case 'E': seek_next_v3_not_implemented(buf, 8); return; // Galileo
+          case 'R': seek_next_v3_not_implemented(buf, 4); return; // Glonass
+          case 'J': seek_next_v3_qzss(buf); return; // QZSS
+          case 'C': seek_next_v3_not_implemented(buf, 8); return; // Beido
+          case 'S': seek_next_v3_sbas(buf); return; // SBAS
+          case 'T': seek_next_v3_not_implemented(buf, 8); return; // IRNSS
+          default: break;
+        }
+      }
+    }
+
     void seek_next() {
+      super_t::_has_next = false;
+      sys_of_msg = super_t::version_type_t::SYS_UNKNOWN;
       super_t::version_type.version >= 300 ? seek_next_v3() : seek_next_v2();
     }
 
@@ -547,11 +753,8 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
     }
     ~RINEX_NAV_Reader(){}
     
-    typename space_node_t::Satellite::Ephemeris next() {
-      typename space_node_t::Satellite::Ephemeris current(msg.eph);
-      super_t::_has_next = false;
+    void next() {
       seek_next();
-      return current;
     }
     
     static const typename super_t::convert_item_t iono_alpha_v2[4];
@@ -569,19 +772,19 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
       bool alpha, beta, utc, leap;
       super_t::header_t::const_iterator it;
 
-      if(alpha = ((it = _header.find("ION ALPHA")) != _header.end())){
+      if((alpha = ((it = _header.find("ION ALPHA")) != _header.end()))){
         super_t::convert(iono_alpha_v2, it->second.front(), &iono_utc);
       }
       
-      if(beta = ((it = _header.find("ION BETA")) != _header.end())){
+      if((beta = ((it = _header.find("ION BETA")) != _header.end()))){
         super_t::convert(iono_beta_v2, it->second.front(), &iono_utc);
       }
 
-      if(utc = ((it = _header.find("DELTA-UTC: A0,A1,T,W")) != _header.end())){
+      if((utc = ((it = _header.find("DELTA-UTC: A0,A1,T,W")) != _header.end()))){
         super_t::convert(utc_v2, it->second.front(), &iono_utc);
       }
 
-      if(leap = ((it = _header.find("LEAP SECONDS")) != _header.end())){
+      if((leap = ((it = _header.find("LEAP SECONDS")) != _header.end()))){
         super_t::convert(utc_leap_v2, it->second.front(), &iono_utc);
       }
 
@@ -623,6 +826,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
       }
 
       if((it = _header.find("LEAP SECONDS")) != _header.end()){
+        iono_utc.delta_t_LSF = iono_utc.WN_LSF = iono_utc.DN = 0;
         if(version_type.version >= 301){
           super_t::convert(utc_leap_v301, it->second.front(), &iono_utc);
         }else{
@@ -635,19 +839,58 @@ class RINEX_NAV_Reader : public RINEX_Reader<> {
       return alpha && beta && utc && leap;
     }
 
-    static int read_all(std::istream &in, space_node_t &space_node){
+    struct space_node_list_t {
+      space_node_t *gps;
+      SBAS_SpaceNode<FloatT> *sbas;
+      space_node_t *qzss;
+    };
+
+    static int read_all(std::istream &in, space_node_list_t &space_nodes = {0}){
       int res(-1);
       RINEX_NAV_Reader reader(in);
-      (reader.version_type.version >= 300)
-          ? reader.extract_iono_utc_v3(space_node)
-          : reader.extract_iono_utc_v2(space_node);
+      if(space_nodes.gps){
+        (reader.version_type.version >= 300)
+            ? reader.extract_iono_utc_v3(*space_nodes.gps)
+            : reader.extract_iono_utc_v2(*space_nodes.gps);
+      }
+      if(space_nodes.qzss && (space_nodes.gps != space_nodes.qzss)
+          && (reader.version_type.version >= 302)){
+        reader.extract_iono_utc_v3(*space_nodes.qzss);
+      }
       res++;
-      for(; reader.has_next(); ++res){
-        typename space_node_t::Satellite::Ephemeris eph(reader.next());
-        space_node.satellite(eph.svid).register_ephemeris(eph);
+      for(; reader.has_next(); reader.next()){
+        switch(reader.sys_of_msg){
+          case super_t::version_type_t::SYS_GPS:
+            if(!space_nodes.gps){break;}
+            space_nodes.gps->satellite(reader.msg.eph.svid).register_ephemeris(reader.msg.eph);
+            res++;
+            break;
+          case super_t::version_type_t::SYS_SBAS: {
+            if(!space_nodes.sbas){break;}
+            typename message_sbas_t::eph_t eph(reader.msg_sbas);
+            space_nodes.sbas->satellite(eph.svid).register_ephemeris(eph);
+            res++;
+            break;
+          }
+          case super_t::version_type_t::SYS_QZSS: {
+            if(!space_nodes.qzss){break;}
+            typename RINEX_NAV<FloatT>::ephemeris_t eph(reader.msg.eph_qzss());
+            space_nodes.qzss->satellite(eph.svid).register_ephemeris(eph);
+            res++;
+            break;
+          }
+          default: break;
+        }
       }
       return res;
     }
+
+    static int read_all(std::istream &in, space_node_t &space_node){
+      space_node_list_t list = {
+        &space_node,
+      };
+      return read_all(in, list);
+    }
 };
 
 template <class FloatT>
@@ -964,12 +1207,16 @@ class RINEX_OBS_Reader : public RINEX_Reader<> {
 #define GEN_I(offset, length, container_type, container_member, value_type) \
     {super_t::template conv_t<value_type>::d, offset, length, \
       offsetof(container_type, container_member), 1}
-#define GEN_F(offset, length, precision, container_type, container_member) \
-    {super_t::template conv_t<FloatT>::f, offset, length, \
+#define GEN_F2(offset, length, precision, container_type, container_member, value_type) \
+    {super_t::template conv_t<value_type>::f, offset, length, \
       offsetof(container_type, container_member), precision}
-#define GEN_E(offset, length, precision, container_type, container_member) \
-    {super_t::template conv_t<FloatT>::e, offset, length, \
+#define GEN_E2(offset, length, precision, container_type, container_member, value_type) \
+    {super_t::template conv_t<value_type>::e, offset, length, \
       offsetof(container_type, container_member), precision}
+#define GEN_F(offset, length, precision, container_type, container_member) \
+    GEN_F2(offset, length, precision, container_type, container_member, FloatT)
+#define GEN_E(offset, length, precision, container_type, container_member) \
+    GEN_E2(offset, length, precision, container_type, container_member, FloatT)
 
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph0_v2[] = {
@@ -1000,17 +1247,17 @@ const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>
 
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph1_v2[] = {
-  GEN_E( 3, 19, 12, message_t, iode_f),
-  GEN_E(22, 19, 12, message_t, eph.c_rs),
-  GEN_E(41, 19, 12, message_t, eph.delta_n),
-  GEN_E(60, 19, 12, message_t, eph.M0),
+  GEN_E2( 3, 19, 12, message_t, eph.iode, int),
+  GEN_E (22, 19, 12, message_t, eph.c_rs),
+  GEN_E (41, 19, 12, message_t, eph.delta_n),
+  GEN_E (60, 19, 12, message_t, eph.M0),
 };
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph1_v3[] = {
-  GEN_E( 4, 19, 12, message_t, iode_f),
-  GEN_E(23, 19, 12, message_t, eph.c_rs),
-  GEN_E(42, 19, 12, message_t, eph.delta_n),
-  GEN_E(61, 19, 12, message_t, eph.M0),
+  GEN_E2( 4, 19, 12, message_t, eph.iode, int),
+  GEN_E (23, 19, 12, message_t, eph.c_rs),
+  GEN_E (42, 19, 12, message_t, eph.delta_n),
+  GEN_E (61, 19, 12, message_t, eph.M0),
 };
 
 template <class FloatT>
@@ -1075,17 +1322,17 @@ const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>
 
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v2[] = {
-  GEN_E( 3, 19, 12, message_t, ura_meter),
-  GEN_E(22, 19, 12, message_t, SV_health_f),
-  GEN_E(41, 19, 12, message_t, eph.t_GD),
-  GEN_E(60, 19, 12, message_t, iodc_f),
+  GEN_E ( 3, 19, 12, message_t, ura_meter),
+  GEN_E2(22, 19, 12, message_t, eph.SV_health, unsigned int),
+  GEN_E (41, 19, 12, message_t, eph.t_GD),
+  GEN_E2(60, 19, 12, message_t, eph.iodc, int),
 };
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph6_v3[] = {
-  GEN_E( 4, 19, 12, message_t, ura_meter),
-  GEN_E(23, 19, 12, message_t, SV_health_f),
-  GEN_E(42, 19, 12, message_t, eph.t_GD),
-  GEN_E(61, 19, 12, message_t, iodc_f),
+  GEN_E ( 4, 19, 12, message_t, ura_meter),
+  GEN_E2(23, 19, 12, message_t, eph.SV_health, unsigned int),
+  GEN_E (42, 19, 12, message_t, eph.t_GD),
+  GEN_E2(61, 19, 12, message_t, eph.iodc, int),
 };
 
 template <class FloatT>
@@ -1099,6 +1346,80 @@ const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>
   GEN_E(23, 19, 12, message_t, fit_interval_hr),
 };
 
+
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph0_sbas_v2[] = {
+  GEN_D( 0,  2,     message_sbas_t, svid,            int),
+  GEN_D( 3,  2,     message_sbas_t, t_year2,         int),
+  GEN_D( 6,  2,     message_sbas_t, t_mon12,         int),
+  GEN_D( 9,  2,     message_sbas_t, date_tm.tm_mday, int),
+  GEN_D(12,  2,     message_sbas_t, date_tm.tm_hour, int),
+  GEN_D(15,  2,     message_sbas_t, date_tm.tm_min,  int),
+  GEN_F(17,  5,  1, message_sbas_t, t_sec),
+  GEN_E(22, 19, 12, message_sbas_t, a_Gf0),
+  GEN_E(41, 19, 12, message_sbas_t, a_Gf1),
+  GEN_E(60, 19, 12, message_sbas_t, t_t),
+};
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph0_sbas_v3[] = {
+  GEN_I( 1,  2,     message_sbas_t, svid,            int),
+  GEN_I( 4,  4,     message_sbas_t, t_year4,         int),
+  GEN_I( 9,  2,     message_sbas_t, t_mon12,         int),
+  GEN_I(12,  2,     message_sbas_t, date_tm.tm_mday, int),
+  GEN_I(15,  2,     message_sbas_t, date_tm.tm_hour, int),
+  GEN_I(18,  2,     message_sbas_t, date_tm.tm_min,  int),
+  GEN_I(21,  2,     message_sbas_t, date_tm.tm_sec,  int),
+  GEN_E(23, 19, 12, message_sbas_t, a_Gf0),
+  GEN_E(42, 19, 12, message_sbas_t, a_Gf1),
+  GEN_E(61, 19, 12, message_sbas_t, t_t),
+};
+
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph1_sbas_v2[] = {
+  GEN_E( 3, 19, 12, message_sbas_t, x_km),
+  GEN_E(22, 19, 12, message_sbas_t, dx_km_s),
+  GEN_E(41, 19, 12, message_sbas_t, ddx_km_s2),
+  GEN_E2(60, 19, 12, message_sbas_t, health, unsigned int),
+};
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph1_sbas_v3[] = {
+  GEN_E( 4, 19, 12, message_sbas_t, x_km),
+  GEN_E(23, 19, 12, message_sbas_t, dx_km_s),
+  GEN_E(42, 19, 12, message_sbas_t, ddx_km_s2),
+  GEN_E2(61, 19, 12, message_sbas_t, health, unsigned int),
+};
+
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph2_sbas_v2[] = {
+  GEN_E( 3, 19, 12, message_sbas_t, y_km),
+  GEN_E(22, 19, 12, message_sbas_t, dy_km_s),
+  GEN_E(41, 19, 12, message_sbas_t, ddy_km_s2),
+  GEN_E(60, 19, 12, message_sbas_t, URA),
+};
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph2_sbas_v3[] = {
+  GEN_E( 4, 19, 12, message_sbas_t, y_km),
+  GEN_E(23, 19, 12, message_sbas_t, dy_km_s),
+  GEN_E(42, 19, 12, message_sbas_t, ddy_km_s2),
+  GEN_E(61, 19, 12, message_sbas_t, URA),
+};
+
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph3_sbas_v2[] = {
+  GEN_E( 3, 19, 12, message_sbas_t, z_km),
+  GEN_E(22, 19, 12, message_sbas_t, dz_km_s),
+  GEN_E(41, 19, 12, message_sbas_t, ddz_km_s2),
+  GEN_E2(60, 19, 12, message_sbas_t, iodn, unsigned int),
+};
+template <class FloatT>
+const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::eph3_sbas_v3[] = {
+  GEN_E( 4, 19, 12, message_sbas_t, z_km),
+  GEN_E(23, 19, 12, message_sbas_t, dz_km_s),
+  GEN_E(42, 19, 12, message_sbas_t, ddz_km_s2),
+  GEN_E2(61, 19, 12, message_sbas_t, iodn, unsigned int),
+};
+
+
 template <class FloatT>
 const typename RINEX_NAV_Reader<FloatT>::convert_item_t RINEX_NAV_Reader<FloatT>::iono_alpha_v2[] = {
   GEN_E( 2, 12, 4, iono_utc_t, alpha[0]),
@@ -1224,7 +1545,7 @@ class RINEX_Writer {
           for(typename super_t::reverse_iterator it(header.rbegin()), it_end(header.rend());
               it != it_end; ++it){
             if(it->first != key){continue;}
-            it_tail = it.base() - 1;
+            it_tail = it.base(); // it_tail points to the next element of an element having the smae key
             break;
           }
         }
@@ -1235,14 +1556,16 @@ class RINEX_Writer {
             const typename super_t::value_type::second_type &value){
           if(it_head == header.end()){
             header.push_back(typename super_t::value_type(key, value));
-            it_head = it_tail = header.rbegin().base();
+            it_tail = header.end(); // in case of invalidation
+            it_head = it_tail - 1;
             return *this;
           }
           it_head->second = value;
-          for(; it_tail != it_head; --it_tail){
-            if(it_tail->first == key){continue;}
+          for(--it_tail; it_tail != it_head; --it_tail){
+            if(it_tail->first != key){continue;}
             header.erase(it_tail);
           }
+          it_tail = it_head + 1;
           return *this;
         }
         /**
@@ -1250,14 +1573,24 @@ class RINEX_Writer {
          */
         bracket_accessor_t &operator<<(
             const typename super_t::value_type::second_type &value){
-          if(it_tail == header.end()){
-            header.push_back(typename super_t::value_type(key, value));
-            it_head = it_tail = header.rbegin().base();
-            return *this;
-          }
-          header.insert(++it_tail, typename super_t::value_type(key, value));
+          super_t::size_type i_head(it_head - header.begin()), i_tail(it_tail - header.begin());
+          header.insert(it_tail, typename super_t::value_type(key, value));
+          it_head = header.begin() + i_head;  // in case of invalidation
+          it_tail = header.begin() + i_tail + 1;
           return *this;
         }
+        unsigned int entries() const {
+          return it_tail - it_head;
+        }
+        typename super_t::iterator find(
+            const typename super_t::value_type::second_type &value) const {
+          for(typename super_t::iterator it(it_head); it != it_tail; ++it){
+            if(it->second.find(value) != super_t::value_type::second_type::npos){
+              return it;
+            }
+          }
+          return header.end();
+        }
       };
       bracket_accessor_t operator[]( // mimic of std::map::operator[]=
           const typename super_t::value_type::first_type &key){
@@ -1336,20 +1669,25 @@ class RINEX_Writer {
       return s;
     }
     
-    static void convert(
+    /**
+     * @return If all conversion are successfully performed, then true; otherwise false;
+     */
+    static bool convert(
         const typename RINEX_Reader<U>::convert_item_t *items, const int &size,
         std::string &buf, const void *values){
       // value => string
+      bool res(true);
       for(int i(0); i < size; ++i){
-        (*items[i].func)(
+        res &= (*items[i].func)(
             buf, items[i].offset, items[i].length, (char *)(const_cast<void *>(values)) + items[i].value_offset,
             items[i].opt, false);
       }
+      return res;
     }
     template <int N>
-    static inline void convert(
+    static inline bool convert(
         const typename RINEX_Reader<U>::convert_item_t (&items)[N], std::string &buf, const void *values){
-      convert(items, N, buf, values);
+      return convert(items, N, buf, values);
     }
 
     void pgm_runby_date(
@@ -1380,6 +1718,7 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
   public:
     typedef typename RINEX_NAV<FloatT>::space_node_t space_node_t;
     typedef typename RINEX_NAV<FloatT>::message_t message_t;
+    typedef typename RINEX_NAV<FloatT>::message_sbas_t message_sbas_t;
 
     static const typename super_t::header_item_t default_header[];
     static const int default_header_size;
@@ -1426,6 +1765,9 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
       std::string s(60, ' ');
       if(super_t::_version_type.version >= 301){
         super_t::convert(reader_t::utc_leap_v301, s, &space_node.iono_utc());
+        if(space_node.iono_utc().WN_LSF == 0){
+          s.replace(6, 18, 18, ' ');
+        }
       }else{
         super_t::convert(reader_t::utc_leap_v2, s, &space_node.iono_utc());
       }
@@ -1435,7 +1777,7 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
         : super_t(out, default_header, default_header_size) {}
     ~RINEX_NAV_Writer(){}
 
-    self_t &operator<<(const message_t &msg){
+    self_t &dump(const message_t &msg, const bool &is_qzss = false){
       std::stringstream buf;
       switch(super_t::_version_type.version / 100){
         case 2:
@@ -1458,7 +1800,8 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
           for(int i(0); i < 8; ++i){
             std::string s(80, ' ');
             switch(i){
-              case 0: super_t::convert(reader_t::eph0_v3, s, &msg); s[0] = 'G'; break;
+              case 0: super_t::convert(reader_t::eph0_v3, s, &msg);
+                s[0] = is_qzss ? 'J' : 'G'; break;
               case 1: super_t::convert(reader_t::eph1_v3, s, &msg); break;
               case 2: super_t::convert(reader_t::eph2_v3, s, &msg); break;
               case 3: super_t::convert(reader_t::eph3_v3, s, &msg); break;
@@ -1474,16 +1817,40 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
       dist << buf.str();
       return *this;
     }
-
-  protected:
-    struct WriteAllFunctor {
-      RINEX_NAV_Writer &w;
-      int &counter;
-      void operator()(const typename space_node_t::Satellite::Ephemeris &eph) {
-        w << message_t(eph);
-        counter++;
+    self_t &operator<<(const message_t &msg){
+      return dump(msg);
+    }
+    self_t &operator<<(const message_sbas_t &msg){
+      std::stringstream buf;
+      switch(super_t::_version_type.version / 100){
+        case 2:
+          for(int i(0); i < 8; ++i){
+            std::string s(80, ' ');
+            switch(i){
+              case 0: super_t::convert(reader_t::eph0_sbas_v2, s, &msg); break;
+              case 1: super_t::convert(reader_t::eph1_sbas_v2, s, &msg); break;
+              case 2: super_t::convert(reader_t::eph2_sbas_v2, s, &msg); break;
+              case 3: super_t::convert(reader_t::eph3_sbas_v2, s, &msg); break;
+            }
+            buf << s << std::endl;
+          }
+          break;
+        case 3:
+          for(int i(0); i < 8; ++i){
+            std::string s(80, ' ');
+            switch(i){
+              case 0: super_t::convert(reader_t::eph0_sbas_v3, s, &msg); s[0] = 'S'; break;
+              case 1: super_t::convert(reader_t::eph1_sbas_v3, s, &msg); break;
+              case 2: super_t::convert(reader_t::eph2_sbas_v3, s, &msg); break;
+              case 3: super_t::convert(reader_t::eph3_sbas_v3, s, &msg); break;
+            }
+            buf << s << std::endl;
+          }
+          break;
       }
-    };
+      dist << buf.str();
+      return *this;
+    }
 
   public:
     void set_version(
@@ -1493,31 +1860,122 @@ class RINEX_NAV_Writer : public RINEX_Writer<> {
           version, super_t::version_type_t::FTYPE_NAVIGATION, sys));
     }
 
-    int write_all(const space_node_t &space_node, const int &version = 304){
+    int write_all(
+        const typename reader_t::space_node_list_t &space_nodes,
+        const int &version = 304){
       int res(-1);
-      set_version(version);
-      if(space_node.is_valid_iono_utc()){
-        iono_alpha(space_node);
-        iono_beta(space_node);
-        utc_params(space_node);
-        leap_seconds(space_node);
+      int systems(0);
+      set_version(version, super_t::version_type_t::SYS_UNKNOWN);
+      do{
+        if(!space_nodes.gps){break;}
+        ++systems;
+        set_version(version, super_t::version_type_t::SYS_GPS);
+        if(!space_nodes.gps->is_valid_iono_utc()){break;}
+        switch(version / 100){
+          case 2:
+            if(_header["ION ALPHA"].entries() == 0){iono_alpha(*space_nodes.gps);}
+            if(_header["ION BETA"].entries() == 0){iono_beta(*space_nodes.gps);}
+            if(_header["DELTA-UTC: A0,A1,T,W"].entries() == 0){utc_params(*space_nodes.gps);}
+            if(_header["LEAP SECONDS"].entries() == 0){leap_seconds(*space_nodes.gps);}
+            break;
+          case 3:
+            if(_header["IONOSPHERIC CORR"].find("GPSA") == _header.end()){iono_alpha(*space_nodes.gps);}
+            if(_header["IONOSPHERIC CORR"].find("GPSB") == _header.end()){iono_beta(*space_nodes.gps);}
+            if(_header["TIME SYSTEM CORR"].find("GPUT") == _header.end()){utc_params(*space_nodes.gps);}
+            if(_header["LEAP SECONDS"].entries() == 0){leap_seconds(*space_nodes.gps);}
+            break;
+        }
+      }while(false);
+      do{
+        if(!space_nodes.sbas){break;}
+        ++systems;
+        set_version(version, super_t::version_type_t::SYS_SBAS);
+      }while(false);
+      do{
+        if((version < 302) || (!space_nodes.qzss)){break;}
+        ++systems;
+        set_version(version, super_t::version_type_t::SYS_QZSS);
+        if(!space_nodes.qzss->is_valid_iono_utc()){break;}
+        switch(version / 100){
+          case 3:
+            if(_header["IONOSPHERIC CORR"].find("GPSA") == _header.end()){iono_alpha(*space_nodes.qzss);}
+            if(_header["IONOSPHERIC CORR"].find("GPSB") == _header.end()){iono_beta(*space_nodes.qzss);}
+            if(_header["TIME SYSTEM CORR"].find("QZUT") == _header.end()){utc_params(*space_nodes.qzss);}
+            if(_header["LEAP SECONDS"].entries() == 0){leap_seconds(*space_nodes.qzss);}
+            break;
+        }
+      }while(false);
+      if(systems > 1){
+        set_version(version, super_t::version_type_t::SYS_MIXED);
       }
       super_t::dist << header();
       res++;
 
-      WriteAllFunctor functor = {*this, res};
-      for(typename space_node_t::satellites_t::const_iterator
-            it(space_node.satellites().begin()), it_end(space_node.satellites().end());
-          it != it_end; ++it){
-        it->second.each_ephemeris(
-            functor,
-            space_node_t::Satellite::eph_list_t::EACH_ALL_INVERTED);
+      struct {
+        RINEX_NAV_Writer &w;
+        int &counter;
+        bool gps, qzss;
+        void operator()(const typename space_node_t::Satellite::Ephemeris &eph) {
+          if(gps && (eph.svid <= 32)){
+            w << message_t(eph);
+          }else if(qzss && (eph.svid >= 193) && (eph.svid < 202)){
+            w.dump(message_t::from_qzss(eph), true);
+          }else{
+            return;
+          }
+          counter++;
+        }
+        void operator()(const typename message_sbas_t::eph_t &eph) {
+          w << message_sbas_t(eph);
+          counter++;
+        }
+      } functor = {*this, res, false, false};
+      if(space_nodes.gps){
+        functor.gps = true;
+        if(space_nodes.gps == space_nodes.qzss){functor.qzss = true;}
+        for(typename space_node_t::satellites_t::const_iterator
+              it(space_nodes.gps->satellites().begin()), it_end(space_nodes.gps->satellites().end());
+            it != it_end; ++it){
+          it->second.each_ephemeris(
+              functor,
+              space_node_t::Satellite::eph_list_t::EACH_ALL_INVERTED);
+        }
+      }
+      if(space_nodes.sbas){
+        for(typename SBAS_SpaceNode<FloatT>::satellites_t::const_iterator
+              it(space_nodes.sbas->satellites().begin()),
+              it_end(space_nodes.sbas->satellites().end());
+            it != it_end; ++it){
+          it->second.each_ephemeris(
+              functor,
+              SBAS_SpaceNode<FloatT>::Satellite::eph_list_t::EACH_ALL_INVERTED);
+        }
+      }
+      if((version >= 302) && (!functor.qzss) && (space_nodes.qzss)){
+        functor.qzss = true;
+        functor.gps = false;
+        for(typename space_node_t::satellites_t::const_iterator
+              it(space_nodes.qzss->satellites().begin()), it_end(space_nodes.qzss->satellites().end());
+            it != it_end; ++it){
+          it->second.each_ephemeris(
+              functor,
+              space_node_t::Satellite::eph_list_t::EACH_ALL_INVERTED);
+        }
       }
       return res;
     }
+    static int write_all(
+        std::ostream &out,
+        const typename reader_t::space_node_list_t &space_nodes,
+        const int &version = 304){
+      return RINEX_NAV_Writer(out).write_all(space_nodes, version);
+    }
+    int write_all(const space_node_t &space_node, const int &version = 304){
+      const typename reader_t::space_node_list_t list = {&const_cast<space_node_t &>(space_node)};
+      return write_all(list, version);
+    }
     static int write_all(std::ostream &out, const space_node_t &space_node, const int &version = 304){
-      RINEX_NAV_Writer writer(out);
-      return writer.write_all(space_node, version);
+      return RINEX_NAV_Writer(out).write_all(space_node, version);
     }
 };
 template <class FloatT>