From bf260a5809ebe0e9f6b382457ab590589202ba31 Mon Sep 17 00:00:00 2001 From: PepMS <jmarti@iri.upc.edu> Date: Sat, 4 Apr 2020 10:44:53 +0200 Subject: [PATCH] [utils] added methods to compare navigation --- include/gnss_utils/utils/utils.h | 46 ++++- src/utils/utils.cpp | 342 ++++++++++++++++++++++++++++++- 2 files changed, 384 insertions(+), 4 deletions(-) diff --git a/include/gnss_utils/utils/utils.h b/include/gnss_utils/utils/utils.h index 7ace499..a8c378d 100644 --- a/include/gnss_utils/utils/utils.h +++ b/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 diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp index 8f95934..7103c29 100644 --- a/src/utils/utils.cpp +++ b/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 -- GitLab