diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h index 2fc13a71c0eb45a88b4570c4be33db1074b12c7b..866e830eb509bc27d69dfb4a95c785b04cd04595 100644 --- a/include/gnss_utils/navigation.h +++ b/include/gnss_utils/navigation.h @@ -5,6 +5,8 @@ #include <iostream> #include <memory> +#include "gnss_utils/utils/utils.h" + extern "C" { #include "rtklib.h" } @@ -67,12 +69,6 @@ public: void print(); //////////////////////////////// nav UTILS ////////////////////////////////////// - template <typename T> - static bool addToArray(const T& new_element, T*& array, int& n, int& nmax); - template <typename T> - static bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out); - template <typename T> - static void freeArray(T*& array, int& n, int& nmax); static void freeEph(nav_t& nav); static void freeGeph(nav_t& nav); static void freeSeph(nav_t& nav); @@ -247,67 +243,6 @@ inline void Navigation::freeAlmanac() } //////////////////////////////// nav UTILS ////////////////////////////////////// -template <typename T> -inline bool Navigation::addToArray(const T& new_element, T*& array, int& n, int& nmax) -{ - // 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 Navigation::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; - - // std::cout << "copyArray: array in not null\n"; - - for (int i = 0; i < n_in; i++) - { - // 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 Navigation::freeArray(T*& array, int& n, int& nmax) -{ - if (array != NULL) - free(array); - array = NULL; - n = nmax = 0; -} - inline void Navigation::freeEph(nav_t& nav) { freeArray<eph_t>(nav.eph, nav.n, nav.nmax); diff --git a/src/navigation.cpp b/src/navigation.cpp index 66b861b71dc5587eb3bbb80b2dee0f43084ad40b..e71bbce021ceac1064d11e879d2523818d672a70 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -5,32 +5,44 @@ using namespace GnssUtils; Navigation::Navigation() { // array initialization - nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax = - nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0; - nav_.eph = NULL; - nav_.geph = NULL; - nav_.seph = NULL; - nav_.peph = NULL; - nav_.pclk = NULL; - nav_.alm = NULL; - nav_.tec = NULL; - nav_.fcb = NULL; + nav_.n = nav_.nmax = 0; + nav_.ng = nav_.ngmax = 0; + nav_.ns = nav_.nsmax = 0; + nav_.ne = nav_.nemax = 0; + nav_.nc = nav_.ncmax = 0; + nav_.na = nav_.namax = 0; + nav_.nt = nav_.ntmax = 0; + nav_.nf = nav_.nfmax = 0; + nav_.eph = NULL; + nav_.geph = NULL; + nav_.seph = NULL; + nav_.peph = NULL; + nav_.pclk = NULL; + nav_.alm = NULL; + nav_.tec = NULL; + nav_.fcb = NULL; clearNavigation(); } Navigation::Navigation(const Navigation& nav) { // array initialization - nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax = - nav_.na = nav_.namax = nav_.nt = nav_.ntmax = nav_.nf = nav_.nfmax = 0; - nav_.eph = NULL; - nav_.geph = NULL; - nav_.seph = NULL; - nav_.peph = NULL; - nav_.pclk = NULL; - nav_.alm = NULL; - nav_.tec = NULL; - nav_.fcb = NULL; + nav_.n = nav_.nmax = 0; + nav_.ng = nav_.ngmax = 0; + nav_.ns = nav_.nsmax = 0; + nav_.ne = nav_.nemax = 0; + nav_.nc = nav_.ncmax = 0; + nav_.na = nav_.namax = 0; + nav_.nt = nav_.ntmax = 0; + nav_.nf = nav_.nfmax = 0; + nav_.eph = NULL; + nav_.geph = NULL; + nav_.seph = NULL; + nav_.peph = NULL; + nav_.pclk = NULL; + nav_.alm = NULL; + nav_.tec = NULL; + nav_.fcb = NULL; clearNavigation(); setNavigation(nav.getNavigation()); } @@ -130,7 +142,8 @@ void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gti uniqueNavigation(); } else - std::cout << "Navigation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl; + std::cout << "Navigation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") + << std::endl; } void Navigation::copyAllArrays(const nav_t& nav)