From 3a51234a6695885e1aeb2e2d3eaabe1cba8db806 Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Fri, 13 Mar 2020 08:43:07 +0100
Subject: [PATCH] [ubloxraw] added class to manage raw data stream

---
 CMakeLists.txt                  |  4 ++-
 include/gnss_utils/navigation.h |  2 ++
 include/gnss_utils/ublox_raw.h  | 35 +++++++++++++++++++
 src/navigation.cpp              | 32 +++++++++++++----
 src/ublox_raw.cpp               | 62 +++++++++++++++++++++++++++++++++
 5 files changed, 127 insertions(+), 8 deletions(-)
 create mode 100644 include/gnss_utils/ublox_raw.h
 create mode 100644 src/ublox_raw.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 877d06f..33c5324 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -53,7 +53,8 @@ SET(SOURCES
     src/observations.cpp
     src/navigation.cpp
     src/TDCP.cpp
-    src/utils.cpp)
+    src/utils.cpp
+    src/ublox_raw.cpp)
 
 SET(RTKLIB_SRC
     ${RTKLIB_SRC_DIR}/pntpos.c
@@ -92,6 +93,7 @@ SET(HEADERS
     include/gnss_utils/observations.h
     include/gnss_utils/navigation.h
     include/gnss_utils/TDCP.h
+    include/gnss_utils/ublox_raw.h
   ${RTKLIB_SRC_DIR}/rtklib.h)
 
 # Eigen #######
diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index 9f45e39..fe11cb2 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -30,6 +30,8 @@ class Navigation
 
         void clearNavigation();
 
+        void freeNavigation();
+
         void setNavigation(nav_t nav);
         void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
 
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
new file mode 100644
index 0000000..b4cf59c
--- /dev/null
+++ b/include/gnss_utils/ublox_raw.h
@@ -0,0 +1,35 @@
+#ifndef UBLOX_RAW_H
+#define UBLOX_RAW_H
+
+#include "gnss_utils/gnss_utils.h"
+
+namespace GNSSUtils
+{
+
+class UBloxRaw
+{
+  public:
+  UBloxRaw();
+  ~UBloxRaw();
+
+  int addDataStream(const std::vector<u_int8_t>& data_stream);
+  
+  const Observations& getObservations() const;
+  const Navigation& getNavigation() const;
+
+  private:
+  raw_t raw_data_;
+
+  Observations obs_;
+  Navigation nav_;
+
+  void updateObservations();
+  void updateNavigation();
+  
+
+};
+
+}
+
+
+#endif
\ No newline at end of file
diff --git a/src/navigation.cpp b/src/navigation.cpp
index 8c906c6..5908061 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -14,6 +14,7 @@ Navigation::Navigation()
     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;
@@ -21,6 +22,7 @@ Navigation::Navigation()
     nav_.nc=nav_.ncmax=0;
     nav_.na=nav_.namax=0;
     nav_.nt=nav_.ntmax=0;
+    nav_.nf=nav_.nfmax=0;
 
 //    allocateEphemeris(0);
 //    allocateGLONASSEphemeris(0);
@@ -30,16 +32,32 @@ Navigation::Navigation()
 
 Navigation::~Navigation()
 {
-  clearNavigation();
+  freeNavigation();
 }
 
-void Navigation::clearNavigation()
+void Navigation::freeNavigation()
 {
-    freenav(&nav_,255);
-//  deleteEphemeris();
-//  deleteGLONASSEphemeris();
-//  deleteSBASEphemeris();
-//  deleteAlmanac();
+  freenav(&nav_,255);
+}
+
+void Navigation::clearNavigation()
+{  
+  // 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;
 }
 
 void Navigation::setNavigation(nav_t _nav)
diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp
new file mode 100644
index 0000000..347bc04
--- /dev/null
+++ b/src/ublox_raw.cpp
@@ -0,0 +1,62 @@
+#include "gnss_utils/ublox_raw.h"
+
+using namespace GNSSUtils;
+
+UBloxRaw::UBloxRaw()
+{
+  if (init_raw(&raw_data_, STRFMT_UBX) == 0)
+  {
+    assert("Failed when allocating memory for raw_t");
+    return;
+  }
+};
+
+UBloxRaw::~UBloxRaw(){};
+
+int UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
+{
+  // Update type based on RTKLIB
+  int update_type = 0;
+  for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
+  {
+    update_type = input_ubx(&raw_data_, (unsigned char)*data_byte);
+  }
+
+  switch (update_type)
+  {
+  case 1:
+    updateObservations();
+    break;
+  case 2:
+    // Ephemeris
+    updateNavigation();
+    break;
+  case 3:
+    // SBAS Message (we should update observations, right?)
+    updateObservations();
+    break;
+  case 9:
+    // Almanac
+    updateNavigation();
+    break;
+  }
+
+  return update_type;
+}
+
+const Observations& UBloxRaw::getObservations() const {return obs_;}
+const Navigation& UBloxRaw::getNavigation() const {return nav_;}
+
+void UBloxRaw::updateObservations()
+{
+  obs_.clearObservations();
+  for (int ii = 0; ii < raw_data_.obs.n; ++ii)
+  {
+    obs_.addObservation(raw_data_.obs.data[ii]);
+  }
+}
+
+void UBloxRaw::updateNavigation()
+{
+  nav_.setNavigation(raw_data_.nav);
+}
\ No newline at end of file
-- 
GitLab