From 3a51234a6695885e1aeb2e2d3eaabe1cba8db806 Mon Sep 17 00:00:00 2001 From: PepMS <jmarti@iri.upc.edu> Date: Fri, 13 Mar 2020 08:43:07 +0100 Subject: [PATCH] [ubloxraw] added class to manage raw data stream --- CMakeLists.txt | 4 ++- include/gnss_utils/navigation.h | 2 ++ include/gnss_utils/ublox_raw.h | 35 +++++++++++++++++++ src/navigation.cpp | 32 +++++++++++++---- src/ublox_raw.cpp | 62 +++++++++++++++++++++++++++++++++ 5 files changed, 127 insertions(+), 8 deletions(-) create mode 100644 include/gnss_utils/ublox_raw.h create mode 100644 src/ublox_raw.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 877d06f..33c5324 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,8 @@ SET(SOURCES src/observations.cpp src/navigation.cpp src/TDCP.cpp - src/utils.cpp) + src/utils.cpp + src/ublox_raw.cpp) SET(RTKLIB_SRC ${RTKLIB_SRC_DIR}/pntpos.c @@ -92,6 +93,7 @@ SET(HEADERS include/gnss_utils/observations.h include/gnss_utils/navigation.h include/gnss_utils/TDCP.h + include/gnss_utils/ublox_raw.h ${RTKLIB_SRC_DIR}/rtklib.h) # Eigen ####### diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h index 9f45e39..fe11cb2 100644 --- a/include/gnss_utils/navigation.h +++ b/include/gnss_utils/navigation.h @@ -30,6 +30,8 @@ class Navigation void clearNavigation(); + void freeNavigation(); + void setNavigation(nav_t nav); void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = ""); diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h new file mode 100644 index 0000000..b4cf59c --- /dev/null +++ b/include/gnss_utils/ublox_raw.h @@ -0,0 +1,35 @@ +#ifndef UBLOX_RAW_H +#define UBLOX_RAW_H + +#include "gnss_utils/gnss_utils.h" + +namespace GNSSUtils +{ + +class UBloxRaw +{ + public: + UBloxRaw(); + ~UBloxRaw(); + + int addDataStream(const std::vector<u_int8_t>& data_stream); + + const Observations& getObservations() const; + const Navigation& getNavigation() const; + + private: + raw_t raw_data_; + + Observations obs_; + Navigation nav_; + + void updateObservations(); + void updateNavigation(); + + +}; + +} + + +#endif \ No newline at end of file diff --git a/src/navigation.cpp b/src/navigation.cpp index 8c906c6..5908061 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -14,6 +14,7 @@ Navigation::Navigation() 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; @@ -21,6 +22,7 @@ Navigation::Navigation() nav_.nc=nav_.ncmax=0; nav_.na=nav_.namax=0; nav_.nt=nav_.ntmax=0; + nav_.nf=nav_.nfmax=0; // allocateEphemeris(0); // allocateGLONASSEphemeris(0); @@ -30,16 +32,32 @@ Navigation::Navigation() Navigation::~Navigation() { - clearNavigation(); + freeNavigation(); } -void Navigation::clearNavigation() +void Navigation::freeNavigation() { - freenav(&nav_,255); -// deleteEphemeris(); -// deleteGLONASSEphemeris(); -// deleteSBASEphemeris(); -// deleteAlmanac(); + freenav(&nav_,255); +} + +void Navigation::clearNavigation() +{ + // 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; } void Navigation::setNavigation(nav_t _nav) diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp new file mode 100644 index 0000000..347bc04 --- /dev/null +++ b/src/ublox_raw.cpp @@ -0,0 +1,62 @@ +#include "gnss_utils/ublox_raw.h" + +using namespace GNSSUtils; + +UBloxRaw::UBloxRaw() +{ + if (init_raw(&raw_data_, STRFMT_UBX) == 0) + { + assert("Failed when allocating memory for raw_t"); + return; + } +}; + +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); + } + + switch (update_type) + { + case 1: + updateObservations(); + break; + case 2: + // Ephemeris + updateNavigation(); + break; + case 3: + // SBAS Message (we should update observations, right?) + updateObservations(); + break; + case 9: + // Almanac + updateNavigation(); + break; + } + + return update_type; +} + +const Observations& UBloxRaw::getObservations() const {return obs_;} +const Navigation& UBloxRaw::getNavigation() const {return nav_;} + +void UBloxRaw::updateObservations() +{ + obs_.clearObservations(); + for (int ii = 0; ii < raw_data_.obs.n; ++ii) + { + obs_.addObservation(raw_data_.obs.data[ii]); + } +} + +void UBloxRaw::updateNavigation() +{ + nav_.setNavigation(raw_data_.nav); +} \ No newline at end of file -- GitLab