diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index 646dd912149c8c523bba954dd4bd8e435ac0b8a8..197232cf32167a0cd5fd1fd0fdb8252bb94d20b3 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -4,6 +4,7 @@ #include <vector> #include <iostream> #include <memory> +#include <string> #include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Geometry> @@ -17,6 +18,8 @@ extern "C" #include "rtklib.h" } +#define GNSSUTILS_MSG "--GNSSUtils--" + namespace GNSSUtils { struct ComputePosOutput{ @@ -37,10 +40,17 @@ namespace GNSSUtils Eigen::Vector3d lat_lon; // latitude_longitude_altitude }; - GNSSUtils::ComputePosOutput computePos(const Observations & _observations, + ComputePosOutput computePos(const Observations & _observations, Navigation & _navigation, const prcopt_t & _prcopt); Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef); + + void print(std::string & _msg); + void printArray(std::string _name, int * _array, int size); + void printArray(std::string _name, unsigned char * _array, int size); + void printArray(std::string _name, double * _array, int size); + void printArray(std::string _name, float * _array, int size); } + #endif diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h index 43a3e21b554a78e5e5580b004a22d1b429738987..b1e6ab26d22dcbfa7a35578dc39f142c6c44f9a9 100644 --- a/include/gnss_utils/observations.h +++ b/include/gnss_utils/observations.h @@ -28,12 +28,16 @@ namespace GNSSUtils void clearObservations(); - void pushObservation(obsd_t obs); + void pushObservation(obsd_t & obs); void reserveObservations(unsigned int n); const std::vector<obsd_t> & getObservations() const; + void print(obsd_t & _obs); + void print(int _obs_idx); + void print(); + private: diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp index 569d1448d6cf5b8451f2b8acae56108cf24e825f..fd5589b48b65ae24c960a19b75900cd8c419cc00 100644 --- a/src/examples/gnss_utils_test.cpp +++ b/src/examples/gnss_utils_test.cpp @@ -128,10 +128,6 @@ int createObsAndNav(Observations* observations, char* obs_path, Navigation* nav return 1; } - - - - int main(int argc, char *argv[]) { /* header */ diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index 439e81bbea321ae2c5c07f51f334e2e09898f77a..a976c3b2ec1f55e87e63bcc14c0397b6f2485c7a 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -3,30 +3,15 @@ namespace GNSSUtils { - GNSSUtils::ComputePosOutput computePos(const GNSSUtils::Observations & _observations, + + ComputePosOutput computePos(const GNSSUtils::Observations & _observations, GNSSUtils::Navigation & _navigation, const prcopt_t & _prcopt) { + // Remove duplicated satellites uniqnav(&(_navigation.getNavigation())); - std::cout << "Number of navigation satellites: " << _navigation.getNavigation().n << "\n"; - std::cout << "Number of observations: " << _observations.getObservations().size() << "\n"; - - // Define processing options - prcopt_t prcopt = prcopt_default; - prcopt.mode = PMODE_SINGLE; - prcopt.soltype = 0; - prcopt.nf = 1; - prcopt.navsys = SYS_GPS; - // prcopt.elmin = 1.05; // 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; - // Define error msg char msg[128] = ""; @@ -36,7 +21,7 @@ namespace GNSSUtils output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(), &(_navigation.getNavigation()), - &prcopt, &sol, NULL, NULL, msg); + &_prcopt, &sol, NULL, NULL, msg); if (output.pos_stat == 0) { @@ -46,7 +31,7 @@ namespace GNSSUtils output.time = sol.time.time; output.time = sol.time.sec; output.pos = Eigen::Vector3d(sol.rr); - std::cout << "Compute pos: " << output.pos.transpose() << "\n"; + // std::cout << "Compute pos: " << output.pos.transpose() << "\n"; 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], @@ -75,4 +60,64 @@ namespace GNSSUtils return Eigen::Vector3d(pos); } + + void print(std::string & _msg) + { + std::string msg = GNSSUTILS_MSG + _msg; + + std::cout << msg << "\n"; + + } + + void printArray(std::string _name, int * _array, int size) + { + std::cout << _name << ": ["; + for (int ii=0; ii<size; ++ii) + { + std::cout << _array[ii]; + if (ii==size-1) + std::cout << "] \n"; + else + std::cout << ","; + } + } + + void printArray(std::string _name, unsigned char * _array, int size) + { + std::cout << _name << ": ["; + for (int ii=0; ii<size; ++ii) + { + std::cout << (unsigned)_array[ii]; + if (ii==size-1) + std::cout << "] \n"; + else + std::cout << ","; + } + } + + void printArray(std::string _name, double * _array, int size) + { + std::cout << _name << ": ["; + for (int ii=0; ii<size; ++ii) + { + std::cout << _array[ii]; + if (ii==size-1) + std::cout << "] \n"; + else + std::cout << ","; + } + } + + void printArray(std::string _name, float * _array, int size) + { + std::cout << _name << ": ["; + for (int ii=0; ii<size; ++ii) + { + std::cout << _array[ii]; + if (ii==size-1) + std::cout << "] \n"; + else + std::cout << ","; + } + } } diff --git a/src/observations.cpp b/src/observations.cpp index 87c6eea9bb6b4840716289a6d1e50a6862ef8a5f..9892903180717c65a832ca35dda5cdc428a6fd20 100644 --- a/src/observations.cpp +++ b/src/observations.cpp @@ -1,5 +1,7 @@ +#include "gnss_utils/gnss_utils.h" #include "gnss_utils/observations.h" + using namespace GNSSUtils; @@ -18,7 +20,7 @@ void Observations::clearObservations() this->obs_vector_.clear(); } -void Observations::pushObservation(obsd_t obs) +void Observations::pushObservation(obsd_t & obs) { this->obs_vector_.push_back(obs); } @@ -32,3 +34,29 @@ const std::vector<obsd_t> & Observations::getObservations() const { return this->obs_vector_; } + +void Observations::print(obsd_t & _obs) +{ + std::string msg = "Observation of satellite #" + std::to_string(_obs.sat); + GNSSUtils::print(msg); + std::cout << "Time [s]: " << _obs.time.time << "\n"; + printArray("SNR: ", _obs.SNR, sizeof(_obs.SNR) / sizeof(_obs.SNR[0])); + printArray("LLI: ", _obs.LLI, sizeof(_obs.LLI) / sizeof(_obs.LLI[0])); + printArray("code: ", _obs.code, sizeof(_obs.code) / sizeof(_obs.code[0])); + printArray("L: ", _obs.L, sizeof(_obs.L) / sizeof(_obs.L)); + printArray("P: ", _obs.P, sizeof(_obs.P) / sizeof(_obs.P)); + printArray("D: ", _obs.D, sizeof(_obs.D) / sizeof(_obs.D)); +} + +void Observations::print(int _obs_idx) +{ + print(obs_vector_[_obs_idx]); +} + +void Observations::print() +{ + for (auto obs: obs_vector_) + { + print(obs); + } +} \ No newline at end of file