diff --git a/include/gnss_utils/utils/utils.h b/include/gnss_utils/utils/utils.h index 4b73a6ed1a870a03684a51fa4c71503dd3c24147..7ace49900ddddd2603635392ae4d874f87b328b5 100644 --- a/include/gnss_utils/utils/utils.h +++ b/include/gnss_utils/utils/utils.h @@ -7,6 +7,8 @@ #include <string> #define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0]) +#define ARRAY2D_NROWS(arr) sizeof(arr) / sizeof(arr[0]) +#define ARRAY2D_NCOLS(arr) sizeof(arr[0]) / sizeof(arr[0][0]) #define GNSSUTILS_MSG "--GnssUtils--" extern "C" { @@ -22,15 +24,101 @@ void printArray(std::string _name, double* _array, int size); void printArray(std::string _name, float* _array, int size); template <typename T> -bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2) +inline bool addToArray(const T& new_element, T*& array, int& n, int& nmax) { - if (size1 != size2) + // std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n"; + // "inspired" from RTKLIB rinex.c + T* array_ref; + if (nmax <= n) + { + // std::cout << "addToArray: nmax <= n\n"; + nmax += 1024; + if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax))) + { + printf("addToArray malloc error: n=%d\n", nmax); + free(array); + array = NULL; + n = nmax = 0; + return false; + } + // std::cout << "addToArray: assigning reallocated array\n"; + array = array_ref; + } + // std::cout << "addToArray: adding element " << n << "\n"; + array[n++] = new_element; + // std::cout << "addToArray: added!\n"; + return true; +} + +template <typename T> +inline bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out) +{ + // std::cout << "copyArray: " << n_in << " elements\n"; + if (array_in == NULL) return false; - for (int ii = 0; ii < size1; ++ii) + // std::cout << "copyArray: array in not null\n"; + + for (int i = 0; i < n_in; i++) { - if (array1[ii] != array2[ii]) + // std::cout << "copyArray: adding element " << i << "\n"; + if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out)) + { + // std::cout << "copyArray: failed to add..\n"; return false; + } + // std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n"; + } + + // std::cout << "copyArray: all copied\n"; + + return true; +} + +template <typename T> +void freeArray(T*& array, int& n, int& nmax) +{ + if (array != NULL) + free(array); + array = NULL; + n = nmax = 0; +} + +template <typename T> +bool equalArray(const T* array_1, const T* array_2, const int& n1, const int& n2) +{ + if (n1 != n2) + return false; + + for (int i = 0; i < n1; ++i) + { + if (array_1[i] != array_2[i]) + return false; + } + + return true; +} + +template <typename T> +bool equalArray2d(const T* array_1, + const T* array_2, + const int& nrow_1, + const int& ncol_1, + const int& nrow_2, + const int& ncol_2) +{ + if (nrow_1 != nrow_2) + return false; + if (ncol_1 != ncol_2) + return false; + + for (int i = 0; i < nrow_1; ++i) + { + for (int j = 0; j < ncol_1; ++j) + { + if (*((array_1 + i * ncol_1) + j) != *((array_2 + i * ncol_1) + j)) + return false; + } } return true; @@ -40,6 +128,8 @@ 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); + } // namespace GnssUtils -#endif // INCLUDE_GNSS_UTILS_UTILS_UTILS_H_ \ No newline at end of file +#endif // INCLUDE_GNSS_UTILS_UTILS_UTILS_H_ \ No newline at end of file diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp index 48fca36152aec213fdc87756f423d8293374a6e8..8f959341a71ac0b4f8462ed8ac095239aafb5a9f 100644 --- a/src/utils/utils.cpp +++ b/src/utils/utils.cpp @@ -90,11 +90,11 @@ bool equalObservations(const obsd_t& obs1, const obsd_t& obs2) return false; if (obs1.freq != obs2.freq) return false; - if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0]))) + if (!equalArray<double>(obs1.L, obs2.L, ARRAY_SIZE(obs1.L), ARRAY_SIZE(obs2.L))) return false; - if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0]))) + if (!equalArray<double>(obs1.P, obs2.P, ARRAY_SIZE(obs1.P), ARRAY_SIZE(obs2.P))) return false; - if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0]))) + if (!equalArray<float>(obs1.D, obs2.D, ARRAY_SIZE(obs1.D), ARRAY_SIZE(obs2.D))) return false; return true; @@ -117,4 +117,78 @@ bool equalObservations(const obs_t& obs1, const obs_t& obs2) return true; } + +bool equalNavigation(const nav_t& nav1, const nav_t& nav2) +{ + if (nav1.n != nav2.n) + return false; + if (nav1.nmax != nav2.nmax) + return false; + if (nav1.ng != nav2.ng) + return false; + if (nav1.ngmax != nav2.ngmax) + return false; + if (nav1.ns != nav2.ns) + return false; + if (nav1.nsmax != nav2.nsmax) + return false; + if (nav1.ne != nav2.ne) + return false; + if (nav1.nemax != nav2.nemax) + return false; + if (nav1.nc != nav2.nc) + return false; + if (nav1.ncmax != nav2.ncmax) + return false; + if (nav1.na != nav2.na) + return false; + if (nav1.namax != nav2.namax) + return false; + if (nav1.nt != nav2.nt) + return false; + if (nav1.ntmax != nav2.ntmax) + return false; + if (nav1.nf != nav2.nf) + return false; + if (nav1.nfmax != nav2.nfmax) + return false; + + // eph + // geph + // seph + // peph + // pclk + // alm + // tec + // fcb + // erp + if (!equalArray<double>(nav1.utc_gps, nav2.utc_gps, ARRAY_SIZE(nav1.utc_gps), ARRAY_SIZE(nav2.utc_gps))) + return false; + if (!equalArray<double>(nav1.utc_glo, nav2.utc_glo, ARRAY_SIZE(nav1.utc_glo), ARRAY_SIZE(nav2.utc_glo))) + return false; + if (!equalArray<double>(nav1.utc_gal, nav2.utc_gal, ARRAY_SIZE(nav1.utc_gal), ARRAY_SIZE(nav2.utc_gal))) + return false; + if (!equalArray<double>(nav1.utc_qzs, nav2.utc_qzs, ARRAY_SIZE(nav1.utc_qzs), ARRAY_SIZE(nav2.utc_qzs))) + return false; + if (!equalArray<double>(nav1.utc_cmp, nav2.utc_cmp, ARRAY_SIZE(nav1.utc_cmp), ARRAY_SIZE(nav2.utc_cmp))) + return false; + if (!equalArray<double>(nav1.utc_irn, nav2.utc_irn, ARRAY_SIZE(nav1.utc_irn), ARRAY_SIZE(nav2.utc_irn))) + return false; + if (!equalArray<double>(nav1.utc_sbs, nav2.utc_sbs, ARRAY_SIZE(nav1.utc_sbs), ARRAY_SIZE(nav2.utc_sbs))) + return false; + if (!equalArray<double>(nav1.ion_gps, nav2.ion_gps, ARRAY_SIZE(nav1.ion_gps), ARRAY_SIZE(nav2.ion_gps))) + return false; + if (!equalArray<double>(nav1.ion_gal, nav2.ion_gal, ARRAY_SIZE(nav1.ion_gal), ARRAY_SIZE(nav2.ion_gal))) + return false; + if (!equalArray<double>(nav1.ion_qzs, nav2.ion_qzs, ARRAY_SIZE(nav1.ion_qzs), ARRAY_SIZE(nav2.ion_qzs))) + return false; + if (!equalArray<double>(nav1.ion_cmp, nav2.ion_cmp, ARRAY_SIZE(nav1.ion_cmp), ARRAY_SIZE(nav2.ion_cmp))) + 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; + +} } // namespace GnssUtils