diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h index 33642bff50728a9133cc685093d59e0557b9fcec..1c259477d067b4a0646ba43f31fc3d23e85eb522 100644 --- a/include/gnss_utils/navigation.h +++ b/include/gnss_utils/navigation.h @@ -69,11 +69,13 @@ class Navigation //////////////////////////////// nav UTILS ////////////////////////////////////// template <typename T> - static bool addToArray(const T &new_element, T *array, int &n, int &nmax); + static void copyArrayElement(const T &new_element, T& element_spot); template <typename T> - static bool copyArray(const T *array_in, const int &n_in, T *array_out, int &n_out, int &nmax_out); + static bool addToArray(const T &new_element, T *&array, int &n, int &nmax); template <typename T> - static void freeArray(T *array, int &n, int &nmax); + 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); @@ -251,12 +253,114 @@ inline void Navigation::freeAlmanac() //////////////////////////////// nav UTILS ////////////////////////////////////// template <typename T> -bool Navigation::addToArray(const T &new_element, T *array, int &n, int &nmax) +inline void Navigation::copyArrayElement(const T &new_element, T& element_spot) +{ + element_spot = new_element; +} + +//template <> +//inline void Navigation::copyArrayElement<eph_t>(const eph_t& new_element, eph_t& element_spot) +//{ +// element_spot = new_element; +// std::cout << "tgd new element " << new_element.tgd << std::endl; +// std::cout << "before tgd element_spot " << element_spot.tgd << std::endl; +// double tgd[4]; +// element_spot.tgd = tgd; +// std::cout << "tgd element_spot " << element_spot.tgd << std::endl; +//} +//template <> +//inline void Navigation::copyArrayElement<geph_t>(const geph_t& new_element, geph_t& element_spot) +//{ +// element_spot = new_element; +// std::cout << "pos new element " << new_element.pos << std::endl; +// std::cout << "before pos element_spot " << element_spot.pos << std::endl; +// double pos[3]; +// element_spot.pos = pos; +// std::cout << "pos element_spot " << element_spot.pos << std::endl; +//} +//template <> +//inline void Navigation::copyArrayElement<seph_t>(const seph_t& new_element, seph_t& element_spot) +//{ +// element_spot = new_element; +// std::cout << "pos new element " << new_element.pos << std::endl; +// std::cout << "before pos element_spot " << element_spot.pos << std::endl; +// double pos[3]; +// element_spot.pos = pos; +// std::cout << "pos element_spot " << element_spot.pos << std::endl; +//} +//template <> +//inline void Navigation::copyArrayElement<peph_t>(const peph_t& new_element, peph_t& element_spot) +//{ +// element_spot = new_element; +// std::cout << "pos new element " << new_element.pos << std::endl; +// std::cout << "before pos element_spot " << element_spot.pos << std::endl; +// double pos[MAXSAT][4]; +// element_spot.pos = pos; +// std::cout << "pos element_spot " << element_spot.pos << std::endl; +//} +//template <> +//inline void Navigation::copyArrayElement<pclk_t>(const pclk_t& new_element, pclk_t& element_spot) +//{ +// element_spot = new_element; +// std::cout << "clk new element " << new_element.clk << std::endl; +// std::cout << "before clk element_spot " << element_spot.clk << std::endl; +// double clk[MAXSAT][1]; +// element_spot.clk = clk; +// std::cout << "clk element_spot " << element_spot.clk << std::endl; +//} +template <> +inline void Navigation::copyArrayElement<alm_t>(const alm_t& new_element, alm_t& element_spot) +{ + std::cout << "copyArrayElement<alm_t>\n"; + std::cout << "new element: "; + std::cout << new_element.sat << std::endl; /* satellite number */ + std::cout << new_element.svh << std::endl; /* sv health (0:ok) */ + std::cout << new_element.svconf << std::endl; /* as and sv config */ + std::cout << new_element.week << std::endl; /* GPS/QZS: gps week, GAL: galileo week */ + //toa; /* Toa */ + + std::cout << new_element.A << std::endl; + std::cout << new_element.e << std::endl; + std::cout << new_element.i0 << std::endl; + std::cout << new_element.OMG0 << std::endl; + std::cout << new_element.omg << std::endl; + std::cout << new_element.M0 << std::endl; + std::cout << new_element.OMGd << std::endl; + std::cout << new_element.toas << std::endl; /* Toa (s) in week */ + std::cout << new_element.f0 << std::endl; + std::cout << new_element.f1 << std::endl; + + std::cout << "element_spot: " << std::endl; + std::cout << element_spot.sat << std::endl; /* satellite number */ + std::cout << element_spot.svh << std::endl; /* sv health (0:ok) */ + std::cout << element_spot.svconf << std::endl; /* as and sv config */ + std::cout << element_spot.week << std::endl; /* GPS/QZS: gps week, GAL: galileo week */ + //toa; /* Toa */ + + std::cout << element_spot.A << std::endl; + std::cout << element_spot.e << std::endl; + std::cout << element_spot.i0 << std::endl; + std::cout << element_spot.OMG0 << std::endl; + std::cout << element_spot.omg << std::endl; + std::cout << element_spot.M0 << std::endl; + std::cout << element_spot.OMGd << std::endl; + std::cout << element_spot.toas << std::endl; /* Toa (s) in week */ + std::cout << element_spot.f0 << std::endl; + std::cout << element_spot.f1 << std::endl; + + std::cout << "copying.."; + element_spot = new_element; +} + +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))) { @@ -266,27 +370,42 @@ bool Navigation::addToArray(const T &new_element, T *array, int &n, int &nmax) n = nmax = 0; return false; } + std::cout << "addToArray: assigning reallocated array\n"; array = array_ref; } - array[n++] = new_element; + std::cout << "addToArray: adding element " << n << "\n"; + copyArrayElement<T>(new_element, array[n++]); + std::cout << "addToArray: added!\n"; return true; } template<typename T> -bool Navigation::copyArray(const T *array_in, const int &n_in, T *array_out, int &n_out, int &nmax_out) +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) +void Navigation::freeArray(T *&array, int &n, int &nmax) { if (array != NULL) free(array); diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp index 5886ce2c8f5ba09659a7a70b50fdc14fdcbed426..dd1630a0d119f4405d77125477581c507b74eb6f 100644 --- a/src/ublox_raw.cpp +++ b/src/ublox_raw.cpp @@ -21,50 +21,68 @@ RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream) { // Update type based on RTKLIB for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte) + { raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte); - switch (raw_data_type_) - { - case OBS: // Observations - updateObservations(); - break; - - case NAV_EPH: // Ephemeris - nav_.copyEphemeris(raw_data_.nav); - break; - - case NAV_SBAS: // SBAS - nav_.addSbasMessage(raw_data_.sbsmsg); - break; - - case NAV_ALM: // Almanac and ion/utc parameters - nav_.freeAlmanac(); - nav_.copyAlmanac(raw_data_.nav); - nav_.copyIonUtc(raw_data_.nav); - break; - - // Not handled messages - case NAV_ANT: - std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n"; - break; - case NAV_DGPS: - std::cout << "UBloxRaw: Received dgps correction. Not handled.\n"; - break; - case NAV_SSR: - std::cout << "UBloxRaw: Received ssr message. Not handled.\n"; - break; - case NAV_LEX: - std::cout << "UBloxRaw: Received lex message. Not handled.\n"; - break; - case ERROR: - std::cout << "UBloxRaw: Received error message. Not handled.\n"; - break; - default: - std::cout << "UBloxRaw: Received unknown message. Not handled.\n"; - break; - } - - return raw_data_type_; + switch (raw_data_type_) + { + case NO: // + break; + + case OBS: // Observations + std::cout << "Observations received!\n"; + updateObservations(); + std::cout << "Observations updated!\n"; + break; + + case NAV_EPH: // Ephemeris + std::cout << "Ephemeris received!\n"; + nav_.copyEphemeris(raw_data_.nav); + std::cout << "Ephemeris copied!\n"; + break; + + case NAV_SBAS: // SBAS + std::cout << "SBAS received!\n"; + nav_.addSbasMessage(raw_data_.sbsmsg); + std::cout << "SBAS added!\n"; + break; + + case NAV_ALM: // Almanac and ion/utc parameters + std::cout << "Almanac and ion/utc parameters received!\n"; + nav_.freeAlmanac(); + std::cout << "Almanac freed!\n"; + nav_.copyAlmanac(raw_data_.nav); + std::cout << "Almanac copied!\n"; + nav_.copyIonUtc(raw_data_.nav); + std::cout << "ION UTC copied!\n"; + break; + + // Not handled messages + case NAV_ANT: + std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n"; + break; + case NAV_DGPS: + std::cout << "UBloxRaw: Received dgps correction. Not handled.\n"; + break; + case NAV_SSR: + std::cout << "UBloxRaw: Received ssr message. Not handled.\n"; + break; + case NAV_LEX: + std::cout << "UBloxRaw: Received lex message. Not handled.\n"; + break; + case ERROR: + std::cout << "UBloxRaw: Received error message. Not handled.\n"; + break; + default: + std::cout << "UBloxRaw: Received unknown message. Not handled.\n"; + break; + } + } + + + std::cout << "ALL BYTES DECODED!\n\n"; + + return raw_data_type_; } void UBloxRaw::updateObservations()