[utils] added methods to compare navigation

include/gnss_utils/utils/utils.h

@@ -116,7 +116,7 @@ bool equalArray2d(const T*   array_1,
   {
     for (int j = 0; j < ncol_1; ++j)
     {
-      if (*((array_1 + i * ncol_1) + j) != *((array_2 + i * ncol_1) + j))
+      if (*(array_1 + i * ncol_1 + j) != *(array_2 + i * ncol_1 + j))
         return false;
     }
   }
@@ -124,11 +124,53 @@ bool equalArray2d(const T*   array_1,
   return true;
 }
 
+template <typename T>
+bool equalArray3d(const T*   array_1,
+                  const T*   array_2,
+                  const int& nmat_1,
+                  const int& nrow_1,
+                  const int& ncol_1,
+                  const int& nmat_2,
+                  const int& nrow_2,
+                  const int& ncol_2)
+{
+  if (nmat_1 != nmat_2)
+    return false;
+  if (nrow_1 != nrow_2)
+    return false;
+  if (ncol_1 != ncol_2)
+    return false;
+
+  for (int i = 0; i < nmat_1; ++i)
+  {
+    for (int j = 0; j < nrow_1; ++j)
+    {
+      for (int k = 0; k < ncol_1; ++k)
+      {
+        if (*(array_1 + i * ncol_1 * nrow_1 + j * ncol_1 + k) != *(array_2 + i * ncol_1 * nrow_1 + j * ncol_1 + k))
+          return false;
+      }
+    }
+  }
+
+  return true;
+}
+
 bool equalTime(const gtime_t& time1, const gtime_t& time2);
+
 bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
 bool equalObservations(const obs_t& obs1, const obs_t& obs2);
 
-bool equalNavigations(const nav_t& nav_1, const nav_t& nav_2);
+bool equalEphemeris(const eph_t& eph1, const eph_t& eph2);
+bool equalGlonassEphemeris(const geph_t& geph1, const geph_t& geph2);
+bool equalSbasEphemeris(const seph_t& seph1, const seph_t& seph2);
+bool equalPreciseEphemeris(const peph_t& peph1, const peph_t& peph2);
+bool equalPreciseClock(const pclk_t& pclk1, const pclk_t& pclk2);
+bool equalAlmanac(const alm_t& alm1, const alm_t& alm2);
+bool equalTecGrid(const tec_t& tec1, const tec_t& tec2);
+bool equalFcb(const fcbd_t& fcbd1, const fcbd_t& fcbd2);
+bool equalEarthRotationParameters(const erp_t& erp1, const erp_t& erp2);
+bool equalNavigations(const nav_t& nav1, const nav_t& nav2);
 
 }  // namespace GnssUtils
 

src/utils/utils.cpp

@@ -118,6 +118,322 @@ bool equalObservations(const obs_t& obs1, const obs_t& obs2)
   return true;
 }
 
+bool equalEphemeris(const eph_t& eph1, const eph_t& eph2)
+{
+  if (eph1.sat != eph2.sat)
+    return false;
+  if (eph1.iode != eph2.iode)
+    return false;
+  if (eph1.iodc != eph2.iodc)
+    return false;
+  if (eph1.sva != eph2.sva)
+    return false;
+  if (eph1.svh != eph2.svh)
+    return false;
+  if (eph1.week != eph2.week)
+    return false;
+  if (eph1.code != eph2.code)
+    return false;
+  if (eph1.flag != eph2.flag)
+    return false;
+
+  if (!equalTime(eph1.toe, eph2.toe))
+    return false;
+  if (!equalTime(eph1.toc, eph2.toc))
+    return false;
+  if (!equalTime(eph1.ttr, eph2.ttr))
+    return false;
+
+  if (eph1.A != eph2.A)
+    return false;
+  if (eph1.e != eph2.e)
+    return false;
+  if (eph1.i0 != eph2.i0)
+    return false;
+  if (eph1.OMG0 != eph2.OMG0)
+    return false;
+  if (eph1.omg != eph2.omg)
+    return false;
+  if (eph1.M0 != eph2.M0)
+    return false;
+  if (eph1.deln != eph2.deln)
+    return false;
+  if (eph1.OMGd != eph2.OMGd)
+    return false;
+  if (eph1.idot != eph2.idot)
+    return false;
+  if (eph1.crc != eph2.crc)
+    return false;
+  if (eph1.crs != eph2.crs)
+    return false;
+  if (eph1.cuc != eph2.cuc)
+    return false;
+  if (eph1.cus != eph2.cus)
+    return false;
+  if (eph1.cic != eph2.cic)
+    return false;
+  if (eph1.cis != eph2.cis)
+    return false;
+  if (eph1.toes != eph2.toes)
+    return false;
+  if (eph1.fit != eph2.fit)
+    return false;
+  if (eph1.f0 != eph2.f0)
+    return false;
+  if (eph1.f1 != eph2.f1)
+    return false;
+  if (eph1.f2 != eph2.f2)
+    return false;
+
+  if (!equalArray(eph1.tgd, eph2.tgd, ARRAY_SIZE(eph1.tgd), ARRAY_SIZE(eph2.tgd)))
+    return false;
+
+  if (eph1.Adot != eph2.Adot)
+    return false;
+  if (eph1.ndot != eph2.ndot)
+    return false;
+
+  return true;
+}
+
+bool equalGlonassEphemeris(const geph_t& geph1, const geph_t& geph2)
+{
+  if (geph1.sat != geph2.sat)
+    return false;
+  if (geph1.iode != geph2.iode)
+    return false;
+  if (geph1.frq != geph2.frq)
+    return false;
+  if (geph1.svh != geph2.svh)
+    return false;
+  if (geph1.sva != geph2.sva)
+    return false;
+  if (geph1.age != geph2.age)
+    return false;
+
+  if (!equalTime(geph1.toe, geph2.toe))
+    return false;
+  if (!equalTime(geph1.tof, geph2.tof))
+    return false;
+
+  if (!equalArray<double>(geph1.pos, geph2.pos, ARRAY_SIZE(geph1.pos), ARRAY_SIZE(geph2.pos)))
+    return false;
+  if (!equalArray<double>(geph1.vel, geph2.vel, ARRAY_SIZE(geph1.vel), ARRAY_SIZE(geph2.vel)))
+    return false;
+  if (!equalArray<double>(geph1.acc, geph2.acc, ARRAY_SIZE(geph1.acc), ARRAY_SIZE(geph2.acc)))
+    return false;
+
+  if (geph1.taun != geph2.taun)
+    return false;
+  if (geph1.gamn != geph2.gamn)
+    return false;
+  if (geph1.dtaun != geph2.dtaun)
+    return false;
+
+  return true;
+}
+
+bool equalSbasEphemeris(const seph_t& seph1, const seph_t& seph2)
+{
+  if (seph1.sat != seph2.sat)
+    return false;
+
+  if (!equalTime(seph1.t0, seph2.t0))
+    return false;
+  if (!equalTime(seph1.tof, seph2.tof))
+    return false;
+
+  if (seph1.sva != seph2.sva)
+    return false;
+  if (seph1.svh != seph2.svh)
+    return false;
+
+  if (!equalArray<double>(seph1.pos, seph2.pos, ARRAY_SIZE(seph1.pos), ARRAY_SIZE(seph2.pos)))
+    return false;
+  if (!equalArray<double>(seph1.vel, seph2.vel, ARRAY_SIZE(seph1.vel), ARRAY_SIZE(seph2.vel)))
+    return false;
+  if (!equalArray<double>(seph1.acc, seph2.acc, ARRAY_SIZE(seph1.acc), ARRAY_SIZE(seph2.acc)))
+    return false;
+
+  if (seph1.af0 != seph2.af0)
+    return false;
+  if (seph1.af1 != seph2.af1)
+    return false;
+
+  return true;
+}
+
+bool equalPreciseEphemeris(const peph_t& peph1, const peph_t& peph2)
+{
+  if (!equalTime(peph1.time, peph2.time))
+    return false;
+
+  if (peph1.index != peph2.index)
+    return false;
+
+  if (!equalArray2d<double>(&peph1.pos[0][0],
+                            &peph2.pos[0][0],
+                            ARRAY2D_NROWS(peph1.pos),
+                            ARRAY2D_NCOLS(peph1.pos),
+                            ARRAY2D_NROWS(peph2.pos),
+                            ARRAY2D_NCOLS(peph2.pos)))
+    return false;
+  if (!equalArray2d<float>(&peph1.std[0][0],
+                           &peph2.std[0][0],
+                           ARRAY2D_NROWS(peph1.std),
+                           ARRAY2D_NCOLS(peph1.std),
+                           ARRAY2D_NROWS(peph2.std),
+                           ARRAY2D_NCOLS(peph2.std)))
+    return false;
+  if (!equalArray2d<double>(&peph1.vel[0][0],
+                            &peph2.vel[0][0],
+                            ARRAY2D_NROWS(peph1.vel),
+                            ARRAY2D_NCOLS(peph1.vel),
+                            ARRAY2D_NROWS(peph2.vel),
+                            ARRAY2D_NCOLS(peph2.vel)))
+    return false;
+  if (!equalArray2d<float>(&peph1.vst[0][0],
+                           &peph2.vst[0][0],
+                           ARRAY2D_NROWS(peph1.vst),
+                           ARRAY2D_NCOLS(peph1.vst),
+                           ARRAY2D_NROWS(peph2.vst),
+                           ARRAY2D_NCOLS(peph2.vst)))
+    return false;
+  if (!equalArray2d<float>(&peph1.cov[0][0],
+                           &peph2.cov[0][0],
+                           ARRAY2D_NROWS(peph1.cov),
+                           ARRAY2D_NCOLS(peph1.cov),
+                           ARRAY2D_NROWS(peph2.cov),
+                           ARRAY2D_NCOLS(peph2.cov)))
+    return false;
+  if (!equalArray2d<float>(&peph1.vco[0][0],
+                           &peph2.vco[0][0],
+                           ARRAY2D_NROWS(peph1.vco),
+                           ARRAY2D_NCOLS(peph1.vco),
+                           ARRAY2D_NROWS(peph2.vco),
+                           ARRAY2D_NCOLS(peph2.vco)))
+    return false;
+
+  return true;
+}
+
+bool equalPreciseClock(const pclk_t& pclk1, const pclk_t& pclk2)
+{
+  if (!equalTime(pclk1.time, pclk2.time))
+    return false;
+
+  if (pclk1.index != pclk2.index)
+    return false;
+
+  if (!equalArray2d<double>(&pclk1.clk[0][0],
+                            &pclk2.clk[0][0],
+                            ARRAY2D_NROWS(pclk1.clk),
+                            ARRAY2D_NCOLS(pclk1.clk),
+                            ARRAY2D_NROWS(pclk2.clk),
+                            ARRAY2D_NCOLS(pclk2.clk)))
+    return false;
+  if (!equalArray2d<float>(&pclk1.std[0][0],
+                           &pclk2.std[0][0],
+                           ARRAY2D_NROWS(pclk1.std),
+                           ARRAY2D_NCOLS(pclk1.std),
+                           ARRAY2D_NROWS(pclk2.std),
+                           ARRAY2D_NCOLS(pclk2.std)))
+    return false;
+
+  return true;
+}
+bool equalAlmanac(const alm_t& alm1, const alm_t& alm2)
+{
+  if (alm1.sat != alm2.sat)
+    return false;
+  if (alm1.svh != alm2.svh)
+    return false;
+  if (alm1.svconf != alm2.svconf)
+    return false;
+  if (alm1.week != alm2.week)
+    return false;
+
+  if (!equalTime(alm1.toa, alm2.toa))
+    return false;
+
+  if (alm1.A != alm2.A)
+    return false;
+  if (alm1.e != alm2.e)
+    return false;
+  if (alm1.i0 != alm2.i0)
+    return false;
+  if (alm1.OMG0 != alm2.OMG0)
+    return false;
+  if (alm1.omg != alm2.omg)
+    return false;
+  if (alm1.M0 != alm2.M0)
+    return false;
+  if (alm1.OMGd != alm2.OMGd)
+    return false;
+  if (alm1.toas != alm2.toas)
+    return false;
+  if (alm1.f0 != alm2.f0)
+    return false;
+  if (alm1.f1 != alm2.f1)
+    return false;
+
+  return true;
+}
+
+bool equalTecGrid(const tec_t& tec1, const tec_t& tec2)
+{
+  if (!equalTime(tec1.time, tec2.time))
+    return false;
+
+  if (!equalArray<int>(tec1.ndata, tec2.ndata, ARRAY_SIZE(tec1.ndata), ARRAY_SIZE(tec2.ndata)))
+    return false;
+
+  if (tec1.rb != tec2.rb)
+    return false;
+
+  if (!equalArray<double>(tec1.lats, tec2.lats, ARRAY_SIZE(tec1.lats), ARRAY_SIZE(tec2.lats)))
+    return false;
+  if (!equalArray<double>(tec1.lons, tec2.lons, ARRAY_SIZE(tec1.lons), ARRAY_SIZE(tec2.lons)))
+    return false;
+  if (!equalArray<double>(tec1.hgts, tec2.hgts, ARRAY_SIZE(tec1.hgts), ARRAY_SIZE(tec2.hgts)))
+    return false;
+
+  // TODO: *data and *rms
+
+  return true;
+}
+
+bool equalFcb(const fcbd_t& fcbd1, const fcbd_t& fcbd2)
+{
+  if (!equalTime(fcbd1.ts, fcbd2.ts))
+    return false;
+  if (!equalTime(fcbd1.te, fcbd2.te))
+    return false;
+
+  if (!equalArray2d<double>(&fcbd1.bias[0][0],
+                            &fcbd2.bias[0][0],
+                            ARRAY2D_NROWS(fcbd1.bias),
+                            ARRAY2D_NCOLS(fcbd1.bias),
+                            ARRAY2D_NROWS(fcbd2.bias),
+                            ARRAY2D_NCOLS(fcbd2.bias)))
+    return false;
+  if (!equalArray2d<double>(&fcbd1.std[0][0],
+                            &fcbd2.std[0][0],
+                            ARRAY2D_NROWS(fcbd1.std),
+                            ARRAY2D_NCOLS(fcbd1.std),
+                            ARRAY2D_NROWS(fcbd2.std),
+                            ARRAY2D_NCOLS(fcbd2.std)))
+    return false;
+
+  return true;
+}
+bool equalEarthRotationParameters(const erp_t& erp1, const erp_t& erp2)
+{
+  // TODO
+  
+  return true;
+}
+
 bool equalNavigation(const nav_t& nav1, const nav_t& nav2)
 {
   if (nav1.n != nav2.n)
@@ -186,9 +502,31 @@ bool equalNavigation(const nav_t& nav1, const nav_t& nav2)
     return false;
   if (!equalArray<double>(nav1.ion_irn, nav2.ion_irn, ARRAY_SIZE(nav1.ion_irn), ARRAY_SIZE(nav2.ion_irn)))
     return false;
-  
+
   if (nav1.leaps != nav2.leaps)
     return false;
-  
+
+  if (!equalArray2d<double>(&nav1.lam[0][0],
+                            &nav2.lam[0][0],
+                            ARRAY2D_NROWS(nav1.lam),
+                            ARRAY2D_NCOLS(nav1.lam),
+                            ARRAY2D_NROWS(nav2.lam),
+                            ARRAY2D_NCOLS(nav2.lam)))
+    return false;
+  if (!equalArray2d<double>(&nav1.cbias[0][0],
+                            &nav2.cbias[0][0],
+                            ARRAY2D_NROWS(nav1.cbias),
+                            ARRAY2D_NCOLS(nav1.cbias),
+                            ARRAY2D_NROWS(nav2.cbias),
+                            ARRAY2D_NCOLS(nav2.cbias)))
+    return false;
+  if (!equalArray3d<double>(&nav1.rbias[0][0][0], &nav2.rbias[0][0][0], MAXRCV, 2, 3, MAXRCV, 2, 3))
+    return false;
+  if (!equalArray<double>(nav1.wlbias, nav2.wlbias, ARRAY_SIZE(nav1.wlbias), ARRAY_SIZE(nav2.wlbias)))
+    return false;
+  if (!equalArray<double>(nav1.glo_cpbias, nav2.glo_cpbias, ARRAY_SIZE(nav1.glo_cpbias), ARRAY_SIZE(nav2.glo_cpbias)))
+    return false;
+  if (!equalArray<char>(nav1.glo_fcn, nav2.glo_fcn, ARRAY_SIZE(nav1.glo_fcn), ARRAY_SIZE(nav2.glo_fcn)))
+    return false;
 }
 }  // namespace GnssUtils