diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h index f5008e9eb8f304d9132c4a0267da1b4b971c5fba..33642bff50728a9133cc685093d59e0557b9fcec 100644 --- a/include/gnss_utils/navigation.h +++ b/include/gnss_utils/navigation.h @@ -29,6 +29,7 @@ class Navigation // Public methods void setNavigation(nav_t nav); + void clearNavigation(); void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = ""); const nav_t & getNavigation() const; diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h index 4d248c2ce9634d70adb8921deb668dbea62ab70a..cc9f65be15cadcb21aebf8f9505dbf4931e54008 100644 --- a/include/gnss_utils/ublox_raw.h +++ b/include/gnss_utils/ublox_raw.h @@ -6,7 +6,7 @@ namespace GNSSUtils { -enum RawDataType {OBS = 1, NAV_EPH = 2, NAV_SBAS = 3, NAV_ALM = 9}; +enum RawDataType : int {NO = 0, OBS = 1, NAV_EPH = 2, NAV_SBAS = 3, NAV_ALM = 9, NAV_ANT = 5, NAV_DGPS = 7, NAV_SSR = 10, NAV_LEX = 31, ERROR = -1}; class UBloxRaw { @@ -14,12 +14,12 @@ class UBloxRaw UBloxRaw(); ~UBloxRaw(); - int addDataStream(const std::vector<u_int8_t>& data_stream); + RawDataType addDataStream(const std::vector<u_int8_t>& data_stream); - Observations getObservations(); - Navigation getNavigation(); + Observations& getObservations(); + Navigation& getNavigation(); - const int& getRawDataType() const; + RawDataType getRawDataType() const; private: raw_t raw_data_; @@ -27,23 +27,23 @@ class UBloxRaw Observations obs_; Navigation nav_; - int raw_data_type_; + RawDataType raw_data_type_; void updateObservations(); }; -inline GNSSUtils::Observations UBloxRaw::getObservations() +inline GNSSUtils::Observations& UBloxRaw::getObservations() { return obs_; } -inline GNSSUtils::Navigation UBloxRaw::getNavigation() +inline GNSSUtils::Navigation& UBloxRaw::getNavigation() { return nav_; } -inline const int& UBloxRaw::getRawDataType() const +inline RawDataType UBloxRaw::getRawDataType() const { return raw_data_type_; } diff --git a/src/navigation.cpp b/src/navigation.cpp index a91efa575706df1ff4ae723f32de073903cc78f1..3cfecdc23e7317eb69658c0432c3e30f3d127334 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -5,22 +5,7 @@ using namespace GNSSUtils; Navigation::Navigation() { // initialization - freeNavigationArrays(); - memset (nav_.utc_gps, 0, sizeof (nav_.utc_gps)); - memset (nav_.utc_glo, 0, sizeof (nav_.utc_glo)); - memset (nav_.utc_gal, 0, sizeof (nav_.utc_gal)); - memset (nav_.utc_qzs, 0, sizeof (nav_.utc_qzs)); - memset (nav_.utc_cmp, 0, sizeof (nav_.utc_cmp)); - memset (nav_.utc_irn, 0, sizeof (nav_.utc_irn)); - memset (nav_.utc_sbs, 0, sizeof (nav_.utc_sbs)); - memset (nav_.ion_gps, 0, sizeof (nav_.ion_gps)); - memset (nav_.ion_gal, 0, sizeof (nav_.ion_gal)); - memset (nav_.ion_qzs, 0, sizeof (nav_.ion_qzs)); - memset (nav_.ion_cmp, 0, sizeof (nav_.ion_cmp)); - memset (nav_.ion_irn, 0, sizeof (nav_.ion_irn)); - nav_.leaps = 0; - sbssat_t sbssat_zero = {0}; - nav_.sbssat = sbssat_zero; + clearNavigation(); } Navigation::Navigation(const Navigation& nav) @@ -91,6 +76,26 @@ void Navigation::setNavigation(nav_t _nav) // pppcorr_t pppcorr; /* ppp corrections */ } +void Navigation::clearNavigation() +{ + freeNavigationArrays(); + memset (nav_.utc_gps, 0, sizeof (nav_.utc_gps)); + memset (nav_.utc_glo, 0, sizeof (nav_.utc_glo)); + memset (nav_.utc_gal, 0, sizeof (nav_.utc_gal)); + memset (nav_.utc_qzs, 0, sizeof (nav_.utc_qzs)); + memset (nav_.utc_cmp, 0, sizeof (nav_.utc_cmp)); + memset (nav_.utc_irn, 0, sizeof (nav_.utc_irn)); + memset (nav_.utc_sbs, 0, sizeof (nav_.utc_sbs)); + memset (nav_.ion_gps, 0, sizeof (nav_.ion_gps)); + memset (nav_.ion_gal, 0, sizeof (nav_.ion_gal)); + memset (nav_.ion_qzs, 0, sizeof (nav_.ion_qzs)); + memset (nav_.ion_cmp, 0, sizeof (nav_.ion_cmp)); + memset (nav_.ion_irn, 0, sizeof (nav_.ion_irn)); + nav_.leaps = 0; + sbssat_t sbssat_zero = {0}; + nav_.sbssat = sbssat_zero; +} + void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt) { auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL); diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp index dcb60bfbedf23add6a0429dcd53f3bb6111275ed..5886ce2c8f5ba09659a7a70b50fdc14fdcbed426 100644 --- a/src/ublox_raw.cpp +++ b/src/ublox_raw.cpp @@ -3,13 +3,13 @@ using namespace GNSSUtils; UBloxRaw::UBloxRaw() : - raw_data_type_(0) + raw_data_type_(NO) { - if (init_raw(&raw_data_, STRFMT_UBX) == 0) - { - assert("Failed when allocating memory for raw_t"); - return; - } + if (init_raw(&raw_data_, STRFMT_UBX) == 0) + { + assert("Failed when allocating memory for raw_t"); + return; + } }; UBloxRaw::~UBloxRaw() @@ -17,51 +17,51 @@ UBloxRaw::~UBloxRaw() free_raw(&raw_data_); }; -int UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream) +RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream) { - // Update type based on RTKLIB - for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte) - { - raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte); - } + // Update type based on RTKLIB + for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte) + raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte); + + switch (raw_data_type_) + { + case OBS: // Observations + updateObservations(); + break; + + case NAV_EPH: // Ephemeris + nav_.copyEphemeris(raw_data_.nav); + break; - switch (raw_data_type_) - { - case 1: - updateObservations(); - break; - case 2: - // Ephemeris - nav_.copyEphemeris(raw_data_.nav); - break; - case 3: - // SBAS - nav_.addSbasMessage(raw_data_.sbsmsg); - break; - case 9: - // Almanac and ion/utc parameters - nav_.freeAlmanac(); - nav_.copyAlmanac(raw_data_.nav); - nav_.copyIonUtc(raw_data_.nav); - break; - case 5: - std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n"; - break; - case 7: - std::cout << "UBloxRaw: Received dgps correction. Not handled.\n"; - break; - case 10: - std::cout << "UBloxRaw: Received ssr message. Not handled.\n"; - break; - case 31: - std::cout << "UBloxRaw: Received lex message. Not handled.\n"; - break; - case -1: - std::cout << "UBloxRaw: Received error message. Not handled.\n"; - break; - default: - std::cout << "UBloxRaw: Received unknown message. Not handled.\n"; - break; + case NAV_SBAS: // SBAS + nav_.addSbasMessage(raw_data_.sbsmsg); + break; + + case NAV_ALM: // Almanac and ion/utc parameters + nav_.freeAlmanac(); + nav_.copyAlmanac(raw_data_.nav); + nav_.copyIonUtc(raw_data_.nav); + break; + + // Not handled messages + case NAV_ANT: + std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n"; + break; + case NAV_DGPS: + std::cout << "UBloxRaw: Received dgps correction. Not handled.\n"; + break; + case NAV_SSR: + std::cout << "UBloxRaw: Received ssr message. Not handled.\n"; + break; + case NAV_LEX: + std::cout << "UBloxRaw: Received lex message. Not handled.\n"; + break; + case ERROR: + std::cout << "UBloxRaw: Received error message. Not handled.\n"; + break; + default: + std::cout << "UBloxRaw: Received unknown message. Not handled.\n"; + break; } return raw_data_type_; @@ -70,16 +70,14 @@ int UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream) void UBloxRaw::updateObservations() { // sort and remove duplicated observations - sortobs(&raw_data_.obs); - // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl; - // obs_.print(); - obs_.clearObservations(); - for (int ii = 0; ii < raw_data_.obs.n; ++ii) - { - obs_.addObservation(raw_data_.obs.data[ii]); - } + sortobs(&raw_data_.obs); + // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl; + // obs_.print(); - // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl; - // obs_.print(); + obs_.clearObservations(); + for (int ii = 0; ii < raw_data_.obs.n; ++ii) + obs_.addObservation(raw_data_.obs.data[ii]); + // std::cout << "--------------------------JUST AFTER!---------------------" << std::endl; + // obs_.print(); }