diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index 0169d3e75562003e4ac97dfd00ba7c6096eadec6..716a8fbd26dd38330afb64e0a99d42e4d9226557 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -37,9 +37,9 @@ namespace GNSSUtils Eigen::Vector3d lat_lon; // latitude_longitude_altitude }; - GNSSUtils::ComputePosOutput computePos(const std::shared_ptr<Observations> & _observations, - const std::shared_ptr<Navigation> & _navigation, - const std::shared_ptr<prcopt_t> & _prcopt); + GNSSUtils::ComputePosOutput computePos(const Observations & _observations, + const Navigation & _navigation, + const prcopt_t & _prcopt); Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef); } diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h index 336a408b4b59798cf622471e783db2fc3cd92096..44a867237b4b23100a6f0a9dce0b9dc63d950f21 100644 --- a/include/gnss_utils/observations.h +++ b/include/gnss_utils/observations.h @@ -32,7 +32,7 @@ namespace GNSSUtils void pushObservation(obsd_t obs); - std::vector<obsd_t> getObservations(); + const std::vector<obsd_t> & getObservations() const; diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index f75f69a27cdaa8a7b0403a1fd6940f877bd45fc4..f8e9b42fb8f1b2d1792339a7373ce6bbed3b4a92 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -2,12 +2,12 @@ namespace GNSSUtils { - GNSSUtils::ComputePosOutput computePos(const std::shared_ptr<GNSSUtils::Observations> & _observations, - const std::shared_ptr<GNSSUtils::Navigation> & _navigation, - const std::shared_ptr<prcopt_t> & _prcopt) + GNSSUtils::ComputePosOutput computePos(const GNSSUtils::Observations & _observations, + const GNSSUtils::Navigation & _navigation, + const prcopt_t & _prcopt) { // Remove duplicated satellites - uniqnav(&(_navigation->getNavigation())); + uniqnav(&(_navigation.getNavigation())); // Define processing options // prcopt_t prcopt = prcopt_default; @@ -30,14 +30,14 @@ namespace GNSSUtils sol_t sol; sol = {{0}}; - output.pos_stat = pntpos(&(_observations->getObservations()[0]), _observations->getObservations().size(), - &(_navigation->getNavigation()), - _prcopt.get(), &sol, NULL, NULL, msg); + output.pos_stat = pntpos(_observations.getObservations().data(), _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 = Eigen::Vector3d(sol.rr); + output.vel = 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]; diff --git a/src/observations.cpp b/src/observations.cpp index db7529d4d7ad37a518f141057b7e120322bb7952..120ba7c78e27051ea2c8c5a25d6c83187ee3663b 100644 --- a/src/observations.cpp +++ b/src/observations.cpp @@ -23,7 +23,7 @@ void Observations::pushObservation(obsd_t obs) this->obs_vector_.push_back(obs); } -std::vector<obsd_t> Observations::getObservations() +const std::vector<obsd_t> & Observations::getObservations() const { return this->obs_vector_; }