Added result get pos structure

include/gnss_utils/gnss_utils.h

@@ -19,9 +19,26 @@ extern "C"
 
 namespace GNSSUtils
 {
-  int getPos(const std::shared_ptr<Observations> & _observations,
-            const std::shared_ptr<Navigation> & _navigation,
-            sol_t & _sol);
+  struct getPosOutput{
+    time_t time;
+    double sec;
+    Eigen::Vector3d pos;        // position (m)
+    Eigen::Vector3d vel;        // velocity (m/s)
+    Eigen::Matrix3d pos_covar;  // position covariance (m^2)
+                                // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
+    Eigen::VectorXd rcv_bias;   // receiver clock bias to time systems (s)
+    int type;                   // type (0:xyz-ecef,1:enu-baseline)
+    int stat;                   // solution status (SOLQ_???)
+    int ns;                     // number of valid satellites
+    double age;                 // age of differential (s)
+    double ratio;               // AR ratio factor for valiation
+
+    int pos_stat;               // return from pntpos
+    Eigen::Vector3d lat_lon;     // latitude_longitude_altitude
+  };
+
+  GNSSUtils::getPosOutput getPos(const std::shared_ptr<Observations> & _observations,
+                                const std::shared_ptr<Navigation> & _navigation);
 
   Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
 }

src/gnss_utils.cpp

@@ -2,11 +2,9 @@
 
 namespace GNSSUtils
 {
-  int getPos(const std::shared_ptr<GNSSUtils::Observations> & _observations,
-            const std::shared_ptr<GNSSUtils::Navigation> & _navigation,
-            sol_t & _sol)
+  GNSSUtils::getPosOutput getPos(const std::shared_ptr<GNSSUtils::Observations> & _observations,
+                                const std::shared_ptr<GNSSUtils::Navigation> & _navigation)
   {
-
     // Remove duplicated satellites
     uniqnav(&(_navigation->getNavigation()));
 
@@ -24,17 +22,39 @@ namespace GNSSUtils
     prcopt.tidecorr = 0;
     prcopt.sbascorr = SBSOPT_FCORR;
 
-    // Define solution
-    _sol = {{0}};
-
     // Define error msg
-    char msg[128]="";
+    char msg[128] = "";
+
+    GNSSUtils::getPosOutput output;
+    sol_t sol;
+    sol = {{0}};
+
+    output.pos_stat = pntpos(&(_observations->getObservations()[0]), _observations->getObservations().size(),
+                            &(_navigation->getNavigation()),
+                            &prcopt, &sol, NULL, NULL, msg);
 
-    int stat = pntpos(&(_observations->getObservations()[0]), _observations->getObservations().size(),
-                      &(_navigation->getNavigation()),
-                      &prcopt, &_sol, NULL, NULL, msg);
+    output.time = sol.time.time;
+    output.time = sol.time.sec;
+    output.pos  = Eigen::Map<Eigen::Vector3d>(sol.rr);
+    output.vel  = Eigen::Map<Eigen::Vector3d>(&sol.rr[3]);
+    output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
+                        sol.qr[3], sol.qr[1], sol.qr[4],
+                        sol.qr[5], sol.qr[3], sol.qr[2];
 
-    return stat;
+    // XXX: segmentation fault here.
+    // if (sol.dtr != NULL)
+    // {
+    //   output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
+    // }
+
+    output.type  = sol.type;
+    output.stat  = sol.stat;
+    output.ns    = sol.ns;
+    output.age   = sol.age;
+    output.ratio = sol.ratio;
+    output.lat_lon = ecefToLatLon(output.pos);
+
+    return output;
   }
 
   Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
@@ -44,4 +64,6 @@ namespace GNSSUtils
 
     return Eigen::Map<Eigen::Vector3d>(pos);
   }
+
+
 }