From a3294f760af0bfbf677eddd49b00fcfcb02559f3 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 25 Mar 2020 16:49:57 +0100
Subject: [PATCH] bug found, still implementing copyArrayElement specialization

---
 include/gnss_utils/navigation.h | 133 ++++++++++++++++++++++++++++++--
 src/ublox_raw.cpp               | 102 ++++++++++++++----------
 2 files changed, 186 insertions(+), 49 deletions(-)

diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index 33642bf..1c25947 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 5886ce2..dd1630a 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()
-- 
GitLab