Refactor QZSS ephemeris and almanac to have the same structure of GPS

fenrir-naru · Mar 28, 2024 · 97592fa · 97592fa
1 parent 76d6091
commit 97592fa
Show file tree

Hide file tree

Showing 2 changed files with 51 additions and 16 deletions.
diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h
@@ -71,7 +71,9 @@ struct GNSS_Data {
           : super_t(), set_iodc(false), iode_subframe2(-1), iode_subframe3(-1), is_qzss(false) {}
       operator gps_ephemeris_t() const {
         return is_qzss
-            ? (gps_ephemeris_t)(reinterpret_cast<const QZSS_LNAV_Ephemeris_Raw<FloatT> &>(*this))
+            ? (gps_ephemeris_t)(
+              reinterpret_cast<
+                const typename QZSS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t &>(*this))
             : super_t::operator gps_ephemeris_t();
       }
     } gps_ephemeris[32], qzss_ephemeris[10];

diff --git a/tool/navigation/QZSS.h b/tool/navigation/QZSS.h
@@ -40,23 +40,56 @@
 #include <limits>
 
 template <class FloatT>
-struct QZSS_LNAV_Ephemeris_Raw : public GPS_SpaceNode<FloatT>::Satellite::Ephemeris::raw_t {
-  typedef typename GPS_SpaceNode<FloatT>::Satellite::Ephemeris eph_t;
-  typedef typename eph_t::raw_t super_t;
+struct QZSS_SpaceNode {
+  struct SatelliteProperties {
+    struct Ephemeris {
+      typedef typename GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris eph_gps_t;
+      struct raw_t : public eph_gps_t::raw_t {
+        typedef typename eph_gps_t::raw_t super_t;
 
-  operator eph_t() const {
-    eph_t res(super_t::operator eph_t());
-    res.fit_interval = super_t::fit_interval_flag
-        ? (std::numeric_limits<FloatT>::max)()
-        : (2 * 60 * 60); // Fit interval (ICD:4.1.2.4 Subframe 2); should be zero
-    return res;
-  };
+        operator eph_gps_t() const {
+          eph_gps_t res(super_t::operator eph_gps_t());
+          res.fit_interval = super_t::fit_interval_flag
+              ? (std::numeric_limits<FloatT>::max)()
+              : (2 * 60 * 60); // Fit interval (ICD:4.1.2.4 Subframe 2); should be zero
+          return res;
+        };
 
-  QZSS_LNAV_Ephemeris_Raw &operator=(const eph_t &eph){
-    super_t::operator=(eph);
-    super_t::fit_interval_flag = (eph.fit_interval > 2 * 60 * 60);
-    return *this;
-  }
+        raw_t &operator=(const eph_gps_t &eph){
+          super_t::operator=(eph);
+          super_t::fit_interval_flag = (eph.fit_interval > 2 * 60 * 60);
+          return *this;
+        }
+      };
+    };
+    struct Almanac {
+      typedef typename GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac alm_gps_t;
+      struct raw_t : public alm_gps_t::raw_t {
+        typedef typename alm_gps_t::raw_t super_t;
+        template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
+        void update(const InputT *src){
+          super_t::template update<PaddingBits_MSB, PaddingBits_LSB, InputT>(src);
+          if((super_t::svid >= 1) && (super_t::svid <= 10)){
+            super_t::svid += 192;
+          }
+        }
+        template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
+        void dump(BufferT *dst, const typename GPS_SpaceNode<FloatT>::u8_t &sf = 4){
+          super_t::template dump<PaddingBits_MSB, PaddingBits_LSB, BufferT>(dst);
+          typedef typename GPS_SpaceNode<FloatT>::template BroadcastedMessage<
+              BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
+              deparse_t;
+          if((super_t::svid >= 193) && (super_t::svid <= 202)){
+            deparse_t::subframe_id_set(dst, sf); // sf = 4 or 5
+            deparse_t::sv_page_id_set(dst, super_t::svid - 192); // [1, 10]
+          }else{
+            return;
+          }
+          deparse_t::data_id_set(dst, 3); // always 3 @see 4.1.2.6.2. QZS Almanac
+        }
+      };
+    };
+  };
 };
 
 #endif /* __QZSS_H__ */