From b71ab39d843d1cbb241528201a0665a7b1158369 Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Sat, 2 May 2020 11:44:56 +0200
Subject: [PATCH] [feature] Load complete rinex

---
 include/gnss_utils/utils/utils.h |   4 ++
 src/examples/gnss_utils_test.cpp | 112 +++++++++++++++++--------------
 src/utils/utils.cpp              |  49 ++++++++++++++
 test/gtest_observations.cpp      |  46 +++++++++----
 4 files changed, 148 insertions(+), 63 deletions(-)

diff --git a/include/gnss_utils/utils/utils.h b/include/gnss_utils/utils/utils.h
index a107051..72a1d77 100644
--- a/include/gnss_utils/utils/utils.h
+++ b/include/gnss_utils/utils/utils.h
@@ -8,6 +8,7 @@
 
 #include "gnss_utils/internal/config.h"
 #include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/observations.h"
 
 #define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
 #define ARRAY2D_NROWS(arr) sizeof(arr) / sizeof(arr[0])
@@ -165,6 +166,9 @@ bool equalArray3d(const T*   array_1,
   return true;
 }
 
+std::vector<Observations>
+loadObservationsFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt);
+
 }  // namespace GnssUtils
 
 bool operator==(const gtime_t& time1, const gtime_t& time2);
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index fba3b7f..2f0ab31 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -1,6 +1,7 @@
 #include "gnss_utils/observations.h"
 #include "gnss_utils/navigation.h"
 #include "gnss_utils/utils/transformations.h"
+#include "gnss_utils/utils/utils.h"
 
 #include <typeinfo>
 
@@ -33,8 +34,19 @@ int main(int argc, char* argv[])
   const char* opt = "";         // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
 
   // load observations from RINEX file
-  observations.loadFromRinex("../src/examples/sample_data.obs", t_start, t_end, dt, opt);
-  observations.print();
+  std::vector<Observations> obs_rinex =
+      loadObservationsFromRinex("../src/examples/rover_sample.obs", t_start, t_end, dt, opt);
+
+  std::cout << "Number of epochs: " << obs_rinex.size() << std::endl;
+
+  observations = obs_rinex.back();
+  for (auto obs = observations.getObservations().begin(); obs != observations.getObservations().end(); ++obs)
+  {
+    printf("Satellite number: %u \n",obs->sat);
+  }
+
+  // observations = obs_rinex[0];
+  // observations.print();
 
   // RTKLIB trace
   // char str_file[80];
@@ -43,61 +55,63 @@ int main(int argc, char* argv[])
   // traceopen(str_file);
 
   // load navigation from RINEX file
-  navigation.loadFromRinex("../src/examples/sample_data.nav", t_start, t_end, dt, opt);
-  navigation.print();
+  // navigation.loadFromRinex("../src/examples/sample_data.nav", t_start, t_end, dt, opt);
+  // navigation.print();
 
-  // Trace close
-  // traceclose();
+  // // Trace close
+  // // traceclose();
 
-  /* Set processing options */
+  // /* Set processing options */
 
-  /* header */
-  std::cout << "------------------" << std::endl;
-  std::cout << "Processing options" << std::endl;
-  std::cout << "------------------" << std::endl;
-
-  prcopt_t prcopt = prcopt_default;
-  prcopt.mode     = PMODE_SINGLE;
-  prcopt.soltype  = 0;
-  prcopt.nf       = 1;
-  prcopt.navsys   = SYS_GPS;
-  prcopt.elmin    = 0.525;  // 60 degrees = 1.05 rad
-  prcopt.sateph   = EPHOPT_BRDC;
-  prcopt.ionoopt  = IONOOPT_OFF;
-  prcopt.tropopt  = TROPOPT_OFF;
-  prcopt.dynamics = 0;
-  prcopt.tidecorr = 0;
-  prcopt.sbascorr = SBSOPT_FCORR;
-  prcopt.ru[0]    = 0;  // 4789374.0336;
-  prcopt.ru[1]    = 0;  // 177048.3292;
-  prcopt.ru[2]    = 0;  // 4194542.6444;
-
-  std::cout << "Processing options defined" << std::endl;
-
-  Eigen::Vector3d lla_gt(41.383293114 * M_PI / 180.0, 2.116101115 * M_PI / 180.0, -91.6641);
-
-  // Compute spp
+  // /* header */
+  // std::cout << "------------------" << std::endl;
+  // std::cout << "Processing options" << std::endl;
+  // std::cout << "------------------" << std::endl;
 
-  /* header */
-  std::cout << "-----------" << std::endl;
-  std::cout << "pntpos call" << std::endl;
-  std::cout << "-----------" << std::endl;
+  // prcopt_t prcopt = prcopt_default;
+  // prcopt.mode     = PMODE_SINGLE;
+  // prcopt.soltype  = 0;
+  // prcopt.nf       = 1;
+  // prcopt.navsys   = SYS_GPS;
+  // prcopt.elmin    = 0.525;  // 60 degrees = 1.05 rad
+  // prcopt.sateph   = EPHOPT_BRDC;
+  // prcopt.ionoopt  = IONOOPT_OFF;
+  // prcopt.tropopt  = TROPOPT_OFF;
+  // prcopt.dynamics = 0;
+  // prcopt.tidecorr = 0;
+  // prcopt.sbascorr = SBSOPT_FCORR;
+  // prcopt.ru[0]    = 0;  // 4789374.0336;
+  // prcopt.ru[1]    = 0;  // 177048.3292;
+  // prcopt.ru[2]    = 0;  // 4194542.6444;
 
-  int stat;
+  // std::cout << "Processing options defined" << std::endl;
 
-  sol_t solb = { { 0 } };
+  // Eigen::Vector3d lla_gt(41.383293114 * M_PI / 180.0, 2.116101115 * M_PI / 180.0, -91.6641);
 
-  char msg[128] = "";
+  // // Compute spp
 
-  stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL, msg);
+  // /* header */
+  // std::cout << "-----------" << std::endl;
+  // std::cout << "pntpos call" << std::endl;
+  // std::cout << "-----------" << std::endl;
 
-  std::cout << "msg: " << msg << std::endl;
-  std::cout << "sol.stat: " << int(solb.stat) << std::endl;
-  std::cout << "Position:      " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl;
-  std::cout << "Position (GT): " << latLonAltToEcef(lla_gt).transpose() << std::endl;
-  std::cout << "Position LLA:      " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0], solb.rr[1], solb.rr[2])).transpose()
-            << std::endl;
-  std::cout << "Position LLA (GT): " << lla_gt.transpose() << std::endl;
+  // int stat;
 
-  traceclose();
+  // sol_t solb = { { 0 } };
+
+  // char msg[128] = "";
+
+  // stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL,
+  // msg);
+
+  // std::cout << "msg: " << msg << std::endl;
+  // std::cout << "sol.stat: " << int(solb.stat) << std::endl;
+  // std::cout << "Position:      " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl;
+  // std::cout << "Position (GT): " << latLonAltToEcef(lla_gt).transpose() << std::endl;
+  // std::cout << "Position LLA:      " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0], solb.rr[1],
+  // solb.rr[2])).transpose()
+  //           << std::endl;
+  // std::cout << "Position LLA (GT): " << lla_gt.transpose() << std::endl;
+
+  // traceclose();
 }
diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp
index ce565b8..2cb59ab 100644
--- a/src/utils/utils.cpp
+++ b/src/utils/utils.cpp
@@ -9,6 +9,55 @@ void print(std::string& _msg)
   std::cout << msg << "\n";
 }
 
+std::vector<Observations>
+loadObservationsFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
+{
+  obs_t obs;
+  obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
+  obs.n    = 0;
+  obs.nmax = MAXSAT;
+
+  // const char* opt = "";
+  auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
+  int  n_epochs;
+  if (stat == 1)
+    n_epochs = sortobs(&obs);
+  else
+  {
+    std::cout << "Observation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error")
+              << stat << std::endl;
+    return;
+  }
+
+  std::vector<Observations> obs_vector;
+  obs_vector.reserve(n_epochs);
+
+  Observations obs_epoch;
+  for (int i = 0; i < obs.n; i++)
+  {
+    obs_epoch.addObservation(obs.data[i]);
+
+    bool last_obs_epoch = false;
+    if (i < obs.n - 1)
+    {
+      double tt = timediff(obs.data[i].time, obs.data[i + 1].time);
+      if (fabs(tt) > DTTOL)
+      {
+        obs_vector.push_back(obs_epoch);
+        obs_epoch.clearObservations();
+      }
+    }
+    else
+    {
+      obs_vector.push_back(obs_epoch);
+    }
+  }
+
+  free(obs.data);
+
+  return obs_vector;
+}
+
 }  // namespace GnssUtils
 
 bool operator==(const gtime_t& time1, const gtime_t& time2)
diff --git a/test/gtest_observations.cpp b/test/gtest_observations.cpp
index cbb25d7..4b0699d 100644
--- a/test/gtest_observations.cpp
+++ b/test/gtest_observations.cpp
@@ -55,7 +55,9 @@ TEST(ObservationsTest, LoadFromRinex)
 
   // GnssUtils utilities
   Observations observations;
-  observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations                        = obs_rinex[0];
 
   ASSERT_EQ(obs.n, 18);
 
@@ -75,7 +77,9 @@ TEST(ObservationsTest, GetObservationBySat)
   loadRinex(obs);
 
   Observations observations;
-  observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations                        = obs_rinex[0];
 
   for (int ii = 0; ii < obs.n; ++ii)
   {
@@ -90,7 +94,9 @@ TEST(ObservationsTest, GetObservationByIdx)
   loadRinex(obs);
 
   Observations observations;
-  observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations                        = obs_rinex[0];
 
   for (int ii = 0; ii < obs.n; ++ii)
   {
@@ -104,7 +110,9 @@ TEST(ObservationsTest, data)
   loadRinex(obs);
 
   Observations observations;
-  observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations                        = obs_rinex[0];
 
   for (int ii = 0; ii < obs.n; ++ii)
   {
@@ -118,7 +126,9 @@ TEST(ObservationsTest, HasSatellite)
   loadRinex(obs);
 
   Observations observations;
-  observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations                        = obs_rinex[0];
 
   for (int ii = 0; ii < obs.n; ++ii)
   {
@@ -129,7 +139,10 @@ TEST(ObservationsTest, HasSatellite)
 TEST(ObservationsTest, FindCommonObservations)
 {
   Observations observations1;
-  observations1.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations1                       = obs_rinex[0];
+
   Observations observations2(observations1);
 
   ASSERT_TRUE(observations1 == observations2);
@@ -140,15 +153,17 @@ TEST(ObservationsTest, FindCommonObservations)
   ASSERT_TRUE(common_sats.size() == observations2.size());
   for (auto sat : common_sats)
   {
-      ASSERT_TRUE(observations1.hasSatellite(sat));
-      ASSERT_TRUE(observations2.hasSatellite(sat));
+    ASSERT_TRUE(observations1.hasSatellite(sat));
+    ASSERT_TRUE(observations2.hasSatellite(sat));
   }
 }
 
 TEST(ObservationsTest, FindCommonObservationsRemoved)
 {
   Observations observations1;
-  observations1.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations1                       = obs_rinex[0];
   Observations observations2(observations1);
 
   // Remove first observation of observations2
@@ -161,15 +176,18 @@ TEST(ObservationsTest, FindCommonObservationsRemoved)
   ASSERT_TRUE(common_sats.size() == observations2.size());
   for (auto sat : common_sats)
   {
-      ASSERT_TRUE(observations1.hasSatellite(sat));
-      ASSERT_TRUE(observations2.hasSatellite(sat));
+    ASSERT_TRUE(observations1.hasSatellite(sat));
+    ASSERT_TRUE(observations2.hasSatellite(sat));
   }
 }
 
 TEST(ObservationsTest, FindCommonObservationsChangeTime)
 {
   Observations observations1;
-  observations1.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+
+  std::vector<Observations> obs_rinex = loadObservationsFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
+  observations1                       = obs_rinex[0];
+
   Observations observations2(observations1);
 
   // Change time
@@ -184,8 +202,8 @@ TEST(ObservationsTest, FindCommonObservationsChangeTime)
   ASSERT_TRUE(common_sats.size() == observations2.size());
   for (auto sat : common_sats)
   {
-      ASSERT_TRUE(observations1.hasSatellite(sat));
-      ASSERT_TRUE(observations2.hasSatellite(sat));
+    ASSERT_TRUE(observations1.hasSatellite(sat));
+    ASSERT_TRUE(observations2.hasSatellite(sat));
   }
 }
 
-- 
GitLab