From e6f08bf72e051f98f423a3521de5de91aa932a7c Mon Sep 17 00:00:00 2001 From: PepMS <jmarti@iri.upc.edu> Date: Thu, 4 Jul 2019 14:57:18 +0200 Subject: [PATCH] Added result get pos structure --- include/gnss_utils/gnss_utils.h | 23 ++++++++++++++--- src/gnss_utils.cpp | 46 ++++++++++++++++++++++++--------- 2 files changed, 54 insertions(+), 15 deletions(-) diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index 89bacdf..f77bf48 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/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); } diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index 060a64f..ed36ae3 100644 --- a/src/gnss_utils.cpp +++ b/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); } + + } -- GitLab