[ubloxraw] added class to manage raw data stream

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 #######

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 = "");
 

include/gnss_utils/ublox_raw.h

+#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

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)

src/ublox_raw.cpp

+#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