diff --git a/deps/RTKLIB b/deps/RTKLIB index 4b8f505328faeb827fe7e0eb5adac1dddaf39cf4..eac44a45c19ac56c0c031135e9818788b5993005 160000 --- a/deps/RTKLIB +++ b/deps/RTKLIB @@ -1 +1 @@ -Subproject commit 4b8f505328faeb827fe7e0eb5adac1dddaf39cf4 +Subproject commit eac44a45c19ac56c0c031135e9818788b5993005 diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index ec75472312f3add20a8730ce79abace251d5e674..5ba6069e9ace935c1f2b347e93b82af29f741862 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -40,7 +40,7 @@ namespace GNSSUtils }; ComputePosOutput computePos(const Observations & _observations, - Navigation & _navigation, + const Navigation & _navigation, const prcopt_t & _prcopt); // ComputePosOutput computePosOwn(const Observations & _observations, diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h index 54a3073702553733e054bcd7259a199c352a45d3..846e7c6a2ce20801940586675ef0c09b1392063d 100644 --- a/include/gnss_utils/navigation.h +++ b/include/gnss_utils/navigation.h @@ -22,6 +22,7 @@ class Navigation public: // Constructor & Destructor Navigation(); + Navigation(const Navigation& nav); ~Navigation(); // Public objects @@ -48,17 +49,9 @@ class Navigation void clearSBASEphemeris(); void clearAlmanac(); - //void allocateEphemeris(int n_sat = MAXSAT); - //void deleteEphemeris(); + void copyEphemeris(const nav_t& nav); + void copyAlmanac(const nav_t& nav); - //void allocateGLONASSEphemeris(int n_sat = NSATGLO); - //void deleteGLONASSEphemeris(); - - //void allocateSBASEphemeris(int n_sat = NSATSBS*2); //SBAS - //void deleteSBASEphemeris(); - - //void allocateAlmanac(int n_sat = MAXSAT); - //void deleteAlmanac(); void print(); private: diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h index 46e21c321efd8ea7983d0b248532b036eeb15ec6..2075544df2fa3360ea04214fea5e13d6577d4703 100644 --- a/include/gnss_utils/ublox_raw.h +++ b/include/gnss_utils/ublox_raw.h @@ -6,6 +6,8 @@ namespace GNSSUtils { +enum RawDataType {OBS = 1, NAV_EPH = 2, NAV_SBAS = 3, NAV_ALM = 9}; + class UBloxRaw { public: @@ -17,8 +19,7 @@ class UBloxRaw Observations getObservations(); Navigation getNavigation(); - inline bool newObsData(){return new_obs_;}; - inline bool newNavData(){return new_nav_;}; + const int& getRawDataType() const; private: raw_t raw_data_; @@ -26,8 +27,7 @@ class UBloxRaw Observations obs_; Navigation nav_; - bool new_obs_; - bool new_nav_; + int raw_data_type_; void updateObservations(); diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index 0e6409c15f96d6d6cfd5e8e372887e68c634fabe..91099bd5899a1cd1221cc8b727e59bb037021055 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -4,7 +4,7 @@ namespace GNSSUtils { ComputePosOutput computePos(const GNSSUtils::Observations & _observations, - GNSSUtils::Navigation & _navigation, + const GNSSUtils::Navigation & _navigation, const prcopt_t & _prcopt) { @@ -30,7 +30,7 @@ namespace GNSSUtils output.time = sol.time.time; output.sec = 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], @@ -41,7 +41,6 @@ namespace GNSSUtils // { // 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; diff --git a/src/navigation.cpp b/src/navigation.cpp index 8376460d00b5af67dd02f0a6b11475413f2d6a09..6784c86b3ee9bdbd6eb85ddfc1aff3859b80c410 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -3,31 +3,48 @@ using namespace GNSSUtils; - Navigation::Navigation() { + nav_.eph =NULL; + nav_.geph=NULL; + nav_.seph=NULL; + nav_.peph=NULL; + nav_.pclk=NULL; + nav_.alm =NULL; + nav_.tec =NULL; + nav_.fcb =NULL; + nav_.n =nav_.nmax =0; + nav_.ng=nav_.ngmax=0; + nav_.ns=nav_.nsmax=0; + nav_.ne=nav_.nemax=0; + nav_.nc=nav_.ncmax=0; + nav_.na=nav_.namax=0; + nav_.nt=nav_.ntmax=0; + nav_.nf=nav_.nfmax=0; +} - nav_.eph =NULL; - nav_.geph=NULL; - nav_.seph=NULL; - nav_.peph=NULL; - nav_.pclk=NULL; - nav_.alm =NULL; - nav_.tec =NULL; - nav_.fcb =NULL; - nav_.n =nav_.nmax =0; - nav_.ng=nav_.ngmax=0; - nav_.ns=nav_.nsmax=0; - nav_.ne=nav_.nemax=0; - nav_.nc=nav_.ncmax=0; - nav_.na=nav_.namax=0; - nav_.nt=nav_.ntmax=0; - nav_.nf=nav_.nfmax=0; - -// allocateEphemeris(0); -// allocateGLONASSEphemeris(0); -// allocateSBASEphemeris(0); -// allocateAlmanac(0); +Navigation::Navigation(const Navigation& nav) +{ + nav_.eph =NULL; + nav_.geph=NULL; + nav_.seph=NULL; + nav_.peph=NULL; + nav_.pclk=NULL; + nav_.alm =NULL; + nav_.tec =NULL; + nav_.fcb =NULL; + nav_.n =nav_.nmax =0; + nav_.ng=nav_.ngmax=0; + nav_.ns=nav_.nsmax=0; + nav_.ne=nav_.nemax=0; + nav_.nc=nav_.ncmax=0; + nav_.na=nav_.namax=0; + nav_.nt=nav_.ntmax=0; + nav_.nf=nav_.nfmax=0; + + nav_t nav_cpy = nav.getNavigation(); + copyEphemeris(nav_cpy); + copyAlmanac(nav_cpy); } Navigation::~Navigation() @@ -38,6 +55,27 @@ Navigation::~Navigation() void Navigation::clearNavigation() { freenav(&nav_,255); + // int opt = 255; + // if (opt&0x01) {free(nav_.eph ); nav_.eph =NULL; nav_.n =nav_.nmax =0;} + // if (opt&0x02) {free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;} + // if (opt&0x04) {free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;} + // if (opt&0x08) {free(nav_.peph); nav_.peph=NULL; nav_.ne=nav_.nemax=0;} + // if (opt&0x10) {free(nav_.pclk); nav_.pclk=NULL; nav_.nc=nav_.ncmax=0;} + // if (opt&0x20) + // { + // std::cout << "Here" << std::endl; + // if (nav_.alm == NULL) + // printf("--------------THIS POINTER IS NULL!!--------"); + + // printf("Pointer address almanac: %p \n", nav_.alm); + // free(nav_.alm); + // std::cout << "Here 1" << std::endl; + // nav_.alm = NULL; + // std::cout << "Here 2" << std::endl; + // nav_.na=nav_.namax=0; + // } + // if (opt&0x40) {free(nav_.tec ); nav_.tec =NULL; nav_.nt=nav_.ntmax=0;} + // if (opt&0x80) {free(nav_.fcb ); nav_.fcb =NULL; nav_.nf=nav_.nfmax=0;} } void Navigation::setNavigation(nav_t _nav) @@ -164,74 +202,43 @@ void Navigation::clearAlmanac() if (nav_.alm != NULL) {free(nav_.alm ); nav_.alm =NULL; nav_.na=nav_.namax=0;} } -/****************** Array memory management ******************/ -//void Navigation::allocateEphemeris(int n_sat) -//{ -// eph_t eph0 ={0,-1,-1}; -// int i; -// nav_.eph = (eph_t *) malloc(sizeof(eph_t)*n_sat); -// for (i=0;i<n_sat;i++) nav_.eph[i] = eph0; -//} -//void Navigation::deleteEphemeris() -//{ -// std::cout << "Navigation::deleteEphemeris...\n"; -// free(nav_.eph); -// nav_.eph =NULL; -// nav_.n = 0; -// nav_.nmax =0; -// std::cout << "deleted!\n"; -//} - -//void Navigation::allocateGLONASSEphemeris(int n_sat) -//{ -// geph_t geph0={0,-1}; -// int i; -// nav_.geph = (geph_t *)malloc(sizeof(geph_t)*n_sat); -//} -//void Navigation::deleteGLONASSEphemeris() -//{ -// std::cout << "Navigation::deleteGLONASSEphemeris...\n"; -// free(nav_.geph); -// nav_.geph =NULL; -// nav_.ng = 0; -// nav_.ngmax =0; -// std::cout << "deleted!\n"; -//} - -//void Navigation::allocateSBASEphemeris(int n_sat) -//{ -// seph_t seph0={0}; -// int i; -// -// nav_.seph = (seph_t *)malloc(sizeof(seph_t)*n_sat); -// for (i=0; i<n_sat; i++) nav_.seph[i] = seph0; -//} -//void Navigation::deleteSBASEphemeris() -//{ -// std::cout << "Navigation::deleteSBASEphemeris...\n"; -// free(nav_.seph); -// nav_.seph =NULL; -// nav_.ns = 0; -// nav_.nsmax =0; -// std::cout << "deleted!\n"; -//} - -//void Navigation::allocateAlmanac(int n_sat) -//{ -// alm_t alm0 ={0,-1}; -// int i; -// -// nav_.alm = (alm_t *)malloc(sizeof(alm_t)*n_sat); -// for (i=0; i<n_sat; i++) nav_.alm[i] = alm0; -//} -//void Navigation::deleteAlmanac() -//{ -// std::cout << "Navigation::deleteAlmanac...\n"; -// free(nav_.alm); -// nav_.na = 0; -// nav_.namax = 0; -// std::cout << "deleted!\n"; -//} +void Navigation::copyEphemeris(const nav_t& nav) +{ + if (nav.eph != NULL) + { + for (int ii = 0; ii < nav.n; ++ii) + { + addEphemeris(nav.eph[ii]); + } + } + + if (nav.geph != NULL) + { + for (int ii = 0; ii < nav.ng; ++ii) + { + addGLONASSEphemeris(nav.geph[ii]); + } + } + + if (nav.seph != NULL) + { + for (int ii = 0; ii < nav.ns; ++ii) + { + addSBASEphemeris(nav.seph[ii]); + } + } +} + +void Navigation::copyAlmanac(const nav_t& nav) +{ + if (nav.alm != NULL) + { + for (int ii = 0; ii < nav.na; ++ii) + { + addAlmanac(nav.alm[ii]); + } + } +} void Navigation::print() { diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp index ff396324c4c8bb3df491c6e683ce144615081661..a6613cae851c9ea86b83aec3eb7cbc25a1690746 100644 --- a/src/ublox_raw.cpp +++ b/src/ublox_raw.cpp @@ -10,8 +10,7 @@ UBloxRaw::UBloxRaw() return; } - new_obs_ = false; - new_nav_ = false; + raw_data_type_ = 0; }; UBloxRaw::~UBloxRaw(){}; @@ -19,62 +18,56 @@ UBloxRaw::~UBloxRaw(){}; int UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream) { // Update type based on RTKLIB - int update_type = 0; for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte) { - update_type = input_ubx(&raw_data_, (unsigned char)*data_byte); + raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte); } - switch (update_type) + switch (raw_data_type_) { case 1: updateObservations(); - new_obs_ = true; break; case 2: // Ephemeris nav_.clearEphemeris(); nav_.clearGLONASSEphemeris(); nav_.clearSBASEphemeris(); - nav_.addEphemeris(*(raw_data_.nav.eph)); - nav_.addGLONASSEphemeris(*(raw_data_.nav.geph)); - nav_.addSBASEphemeris(*(raw_data_.nav.seph)); - new_nav_ = true; + nav_.copyEphemeris(raw_data_.nav); break; case 3: // SBAS nav_.clearEphemeris(); nav_.clearGLONASSEphemeris(); nav_.clearSBASEphemeris(); - nav_.addEphemeris(*(raw_data_.nav.eph)); - nav_.addGLONASSEphemeris(*(raw_data_.nav.geph)); - nav_.addSBASEphemeris(*(raw_data_.nav.seph)); + nav_.copyEphemeris(raw_data_.nav); updateObservations(); - new_obs_ = true; - new_nav_ = true; break; case 9: // Almanac nav_.clearAlmanac(); - nav_.addAlmanac(*(raw_data_.nav.alm)); - new_nav_ = true; + nav_.copyAlmanac(raw_data_.nav); break; } - return update_type; + return raw_data_type_; } Observations UBloxRaw::getObservations() { - new_obs_ = false; return obs_; } + Navigation UBloxRaw::getNavigation() { - new_nav_ = false; return nav_; } +const int& UBloxRaw::getRawDataType() const +{ + return raw_data_type_; +} + void UBloxRaw::updateObservations() { // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;