Merge branch '11-add-novatel-raw-data-stream' into 'devel'

Resolve "Add Novatel raw data stream"

Closes #11

See merge request mobile_robotics/gauss_project/gnss_utils!11

CMakeLists.txt

@@ -85,7 +85,9 @@ SET(SOURCES
     src/navigation.cpp
     src/snapshot.cpp
     src/tdcp.cpp
-    src/ublox_raw.cpp)
+    src/receiver_raw_base.cpp
+    src/receivers/ublox_raw.cpp
+    src/receivers/novatel_raw.cpp)
 
 SET(RTKLIB_SRC
     ${RTKLIB_SRC_DIR}/pntpos.c

RTKLIB @ 6ecf8cf1

-Subproject commit afa374367da135226a3995b7ede987456884e6c8
+Subproject commit 6ecf8cf16c00fbcf317129a036ba2203b0a85461

include/gnss_utils/receiver_raw_base.h

+#ifndef INCLUDE_GNSS_UTILS_RECEIVER_RAW_BASE_H_
+#define INCLUDE_GNSS_UTILS_RECEIVER_RAW_BASE_H_
+
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+
+namespace GnssUtils
+{
+enum RawDataType : int
+{
+  NO             = 0,
+  OBS            = 1,
+  NAV_EPH        = 2,
+  NAV_SBAS       = 3,
+  NAV_ALM_IONUTC = 9,
+  NAV_ANT        = 5,
+  NAV_DGPS       = 7,
+  NAV_SSR        = 10,
+  NAV_LEX        = 31,
+  ERROR          = -1
+};
+
+class ReceiverRawAbstract
+{
+public:
+  ReceiverRawAbstract();
+  ~ReceiverRawAbstract();
+
+  virtual RawDataType addDataStream(const std::vector<uint8_t>& data_stream) = 0;
+
+  const Observations& getObservations() const;
+  const Navigation&   getNavigation() const;
+
+  Observations getObservations();
+  Navigation   getNavigation();
+
+  RawDataType getRawDataType() const;
+
+protected:
+  void updateObservations();
+
+  raw_t       raw_data_;
+  RawDataType raw_data_type_;
+
+  Observations obs_;
+  Navigation   nav_;
+};
+
+inline const Observations& ReceiverRawAbstract::getObservations() const
+{
+  return obs_;
+}
+
+inline Observations ReceiverRawAbstract::getObservations()
+{
+  return obs_;
+}
+
+inline const Navigation& ReceiverRawAbstract::getNavigation() const
+{
+  return nav_;
+}
+
+inline Navigation ReceiverRawAbstract::getNavigation()
+{
+  return nav_;
+}
+
+inline RawDataType ReceiverRawAbstract::getRawDataType() const
+{
+  return raw_data_type_;
+}
+
+}  // namespace GnssUtils
+#endif

include/gnss_utils/receivers/novatel_raw.h

+#ifndef INCLUDE_GNSS_UTILS_NOVATEL_RAW_H_
+#define INCLUDE_GNSS_UTILS_NOVATEL_RAW_H_
+
+#include "gnss_utils/receiver_raw_base.h"
+
+namespace GnssUtils
+{
+class NovatelRaw : public ReceiverRawAbstract
+{
+public:
+  NovatelRaw();
+  ~NovatelRaw();
+
+  RawDataType addDataStream(const std::vector<uint8_t>& data_stream);
+};
+} // namespace GnssUtils
+
+#endif
\ No newline at end of file

include/gnss_utils/receivers/ublox_raw.h

+#ifndef INCLUDE_GNSS_UTILS_UBLOX_RAW_H_
+#define INCLUDE_GNSS_UTILS_UBLOX_RAW_H_
+
+#include "gnss_utils/receiver_raw_base.h"
+
+namespace GnssUtils
+{
+class UBloxRaw : public ReceiverRawAbstract
+{
+public:
+  UBloxRaw();
+  ~UBloxRaw();
+
+  RawDataType addDataStream(const std::vector<uint8_t>& data_stream);
+
+private:
+
+};
+
+}  // namespace GnssUtils
+
+#endif  // INCLUDE_GNSS_UTILS_UBLOX_RAW_H_

include/gnss_utils/ublox_raw.h

-#ifndef INCLUDE_GNSS_UTILS_UBLOX_RAW_H_
-#define INCLUDE_GNSS_UTILS_UBLOX_RAW_H_
-
-#include "gnss_utils/observations.h"
-#include "gnss_utils/navigation.h"
-
-namespace GnssUtils
-{
-enum RawDataType : int
-{
-  NO       = 0,
-  OBS      = 1,
-  NAV_EPH  = 2,
-  NAV_SBAS = 3,
-  NAV_ALM  = 9,
-  NAV_ANT  = 5,
-  NAV_DGPS = 7,
-  NAV_SSR  = 10,
-  NAV_LEX  = 31,
-  ERROR    = -1
-};
-
-class UBloxRaw
-{
-public:
-  UBloxRaw();
-  ~UBloxRaw();
-
-  RawDataType addDataStream(const std::vector<u_int8_t>& data_stream);
-
-  const Observations& getObservations();
-  const Navigation&   getNavigation();
-
-  RawDataType getRawDataType() const;
-
-private:
-  raw_t raw_data_;
-
-  Observations obs_;
-  Navigation   nav_;
-
-  RawDataType raw_data_type_;
-
-  void updateObservations();
-};
-
-inline const Observations& UBloxRaw::getObservations()
-{
-  return obs_;
-}
-
-inline const Navigation& UBloxRaw::getNavigation()
-{
-  return nav_;
-}
-
-inline RawDataType UBloxRaw::getRawDataType() const
-{
-  return raw_data_type_;
-}
-
-}  // namespace GnssUtils
-
-#endif  // INCLUDE_GNSS_UTILS_UBLOX_RAW_H_

include/gnss_utils/utils/rcv_position.h

@@ -20,7 +20,7 @@
 namespace GnssUtils
 {
 
-ComputePosOutput computePos(const Observations& _observations, Navigation& _navigation, const prcopt_t& _prcopt);
+ComputePosOutput computePos(const Observations& _observations, const Navigation& _navigation, const prcopt_t& _prcopt);
 
 // ComputePosOutput computePosOwn(const Observations & _observations,
 //                                Navigation & _navigation,

src/receiver_raw_base.cpp

+#include "gnss_utils/receiver_raw_base.h"
+
+namespace GnssUtils
+{
+ReceiverRawAbstract::ReceiverRawAbstract() : raw_data_type_(NO)
+{
+  if (init_raw(&raw_data_, STRFMT_UBX) == 0)
+  {
+    assert("Failed when allocating memory for raw_t");
+    return;
+  }
+}
+
+ReceiverRawAbstract::~ReceiverRawAbstract()
+{
+  free_raw(&raw_data_);
+}
+
+void ReceiverRawAbstract::updateObservations()
+{
+  // sort and remove duplicated observations
+  sortobs(&raw_data_.obs);
+
+  obs_.clearObservations();
+  for (int ii = 0; ii < raw_data_.obs.n; ++ii)
+    obs_.addObservation(raw_data_.obs.data[ii]);
+}
+
+}  // namespace GnssUtils
\ No newline at end of file

src/receivers/novatel_raw.cpp

+#include "gnss_utils/receivers/novatel_raw.h"
+
+using namespace GnssUtils;
+
+NovatelRaw::NovatelRaw()
+{
+}
+
+NovatelRaw::~NovatelRaw()
+{
+}
+
+RawDataType NovatelRaw::addDataStream(const std::vector<uint8_t>& data_stream)
+{
+ 
+ for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
+  {
+    raw_data_type_ = input_oem4(&raw_data_, (unsigned char)*data_byte);
+
+    switch (raw_data_type_)
+    {
+      case NO:  //
+        // std::cout << "0 received!\n";
+        break;
+
+      case OBS:  // Observations
+        std::cout << "Observations Novatel received!\n";
+        updateObservations();
+        break;
+
+      case NAV_EPH:  // Ephemeris
+        std::cout << "Ephemeris received!\n";
+        nav_.copyEphemeris(raw_data_.nav);
+        nav_.uniqueNavigation();
+        break;
+
+      case NAV_SBAS:  // SBAS
+        std::cout << "SBAS received!\n";
+        break;
+
+      case NAV_ALM_IONUTC:  // Almanac and ion/utc parameters
+        std::cout << "Almanac and ion/utc parameters received!\n";
+        nav_.copyIonUtc(raw_data_.nav);
+        break;
+
+      // Not handled messages
+      case NAV_ANT:
+        std::cout << "NovatelRaw: Received antenna postion parameters. Not handled.\n";
+        break;
+      case NAV_DGPS:
+        std::cout << "NovatelRaw: Received dgps correction. Not handled.\n";
+        break;
+      case NAV_SSR:
+        std::cout << "NovatelRaw: Received ssr message. Not handled.\n";
+        break;
+      case NAV_LEX:
+        std::cout << "NovatelRaw: Received lex message. Not handled.\n";
+        break;
+      case ERROR:
+        std::cout << "NovatelRaw: Received error message. Not handled.\n";
+        break;
+      default:
+        std::cout << "NovatelRaw: Received unknown message. Not handled.\n";
+        break;
+    }
+  }
+}
\ No newline at end of file

src/ublox_raw.cpp → src/receivers/ublox_raw.cpp

-#include "gnss_utils/ublox_raw.h"
+#include "gnss_utils/receivers/ublox_raw.h"
 
 using namespace GnssUtils;
 
-UBloxRaw::UBloxRaw() : raw_data_type_(NO)
+UBloxRaw::UBloxRaw()
 {
-  if (init_raw(&raw_data_, STRFMT_UBX) == 0)
-  {
-    assert("Failed when allocating memory for raw_t");
-    return;
-  }
-};
+}
 
 UBloxRaw::~UBloxRaw()
 {
-  free_raw(&raw_data_);
-};
+}
 
-RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
+RawDataType UBloxRaw::addDataStream(const std::vector<uint8_t>& data_stream)
 {
   // Update type based on RTKLIB
   for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
@@ -38,7 +32,6 @@ RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
       case NAV_EPH:  // Ephemeris
         std::cout << "Ephemeris received!\n";
         nav_.copyEphemeris(raw_data_.nav);
-        // std::cout << "Ephemeris copied!\n";
         break;
 
       case NAV_SBAS:  // SBAS
@@ -47,14 +40,11 @@ RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
         // std::cout << "SBAS added!\n";
         break;
 
-      case NAV_ALM:  // Almanac and ion/utc parameters
+      case NAV_ALM_IONUTC:  // 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
@@ -81,18 +71,3 @@ RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
 
   return raw_data_type_;
 }
-
-void UBloxRaw::updateObservations()
-{
-  // sort and remove duplicated observations
-  sortobs(&raw_data_.obs);
-  // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
-  // obs_.print();
-
-  obs_.clearObservations();
-  for (int ii = 0; ii < raw_data_.obs.n; ++ii)
-    obs_.addObservation(raw_data_.obs.data[ii]);
-
-  // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
-  // obs_.print();
-}

src/utils/rcv_position.cpp

@@ -12,26 +12,26 @@ using namespace GnssUtils;
 namespace GnssUtils
 {
 ComputePosOutput computePos(const GnssUtils::Observations& _observations,
-                            GnssUtils::Navigation&         _navigation,
+                            const GnssUtils::Navigation&   _navigation,
                             const prcopt_t&                _prcopt)
 {
-  // Remove duplicated satellites
-  _navigation.uniqueNavigation();
-
   // Define error msg
   char msg[128] = "";
 
-  // Remove duplicated satellites
-  _navigation.uniqueNavigation();
-
   GnssUtils::ComputePosOutput output;
   sol_t                       sol;
   sol = { { 0 } };
 
-  std::vector<double> sat_elevations(2*_observations.size());
+  std::vector<double> sat_elevations(2 * _observations.size());
 
-  output.pos_stat = pntpos(
-      _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, sat_elevations.data(), NULL, msg);
+  output.pos_stat = pntpos(_observations.data(),
+                           _observations.size(),
+                           &(_navigation.getNavigation()),
+                           &_prcopt,
+                           &sol,
+                           sat_elevations.data(),
+                           NULL,
+                           msg);
 
   if (output.pos_stat == 0)
     std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
@@ -45,7 +45,9 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
   // std::cout << "Covariance:\n" << output.pos_covar << "\n";
 
   if (sol.dtr != NULL)
-    output.rcv_bias = (Eigen::Matrix<double,6,1>() << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5]).finished();
+    output.rcv_bias =
+        (Eigen::Matrix<double, 6, 1>() << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5])
+            .finished();
   output.type    = sol.type;
   output.stat    = sol.stat;
   output.ns      = sol.ns;
@@ -54,7 +56,8 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
   output.lat_lon = ecefToLatLonAlt(output.pos);
 
   for (auto i = 0; i < _observations.size(); i++)
-      output.sat_azel.emplace(_observations.getObservationByIdx(i).sat,Eigen::Vector2d(sat_elevations.at(2*i),sat_elevations.at(2*i+1)));
+    output.sat_azel.emplace(_observations.getObservationByIdx(i).sat,
+                            Eigen::Vector2d(sat_elevations.at(2 * i), sat_elevations.at(2 * i + 1)));
 
   return output;
 }