diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index 3f0317e17c1ada0c8a5c5dfa2c586b96cff02fba..ba4c3f594f6fa59e852575fb87d9d6c49b4cf4da 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -13,24 +13,6 @@ extern "C"
namespace GNSSUtils
{
-//////////////////////////////// nav UTILS //////////////////////////////////////
-template <typename T>
-bool addToArray(const T &new_element, T *array, int &n, int &nmax);
-template <typename T>
-bool copyArray(const T *array_in, const int &n_in, T *array_out, int &n_out, int &nmax_out);
-template <typename T>
-void freeArray(T *array, int &n, int &nmax);
-void freeNavArrays(nav_t& nav);
-void freeEph(nav_t& nav);
-void freeGeph(nav_t& nav);
-void freeSeph(nav_t& nav);
-void freePeph(nav_t& nav);
-void freeAlm(nav_t& nav);
-void freePclk(nav_t& nav);
-void freeTec(nav_t& nav);
-void freeFcb(nav_t& nav);
-
-//////////////////////////////// Navigation Class //////////////////////////////////////
class Navigation;
typedef std::shared_ptr<Navigation> NavigationPtr;
typedef std::shared_ptr<const Navigation> NavigationConstPtr;
@@ -47,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;
@@ -84,6 +67,23 @@ class Navigation
void print();
+ //////////////////////////////// nav UTILS //////////////////////////////////////
+ template <typename T>
+ static bool addToArray(const T &new_element, T *&array, int &n, int &nmax);
+ template <typename T>
+ static bool copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out);
+ template <typename T>
+ static void freeArray(T *&array, int &n, int &nmax);
+ static void freeEph(nav_t &nav);
+ static void freeGeph(nav_t &nav);
+ static void freeSeph(nav_t &nav);
+ static void freePeph(nav_t &nav);
+ static void freeAlm(nav_t &nav);
+ static void freePclk(nav_t &nav);
+ static void freeTec(nav_t &nav);
+ static void freeFcb(nav_t &nav);
+ static void freeNavArrays(nav_t &nav);
+
private:
// rtklib-like attribute to represent the different navigation msgs for a given epoch
@@ -251,12 +251,14 @@ inline void Navigation::freeAlmanac()
//////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T>
-bool addToArray(const T &new_element, T *array, int &n, int &nmax)
+inline bool Navigation::addToArray(const T &new_element, T *&array, int &n, int &nmax)
{
+ //std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
// "inspired" from RTKLIB rinex.c
T *array_ref;
if (nmax <= n)
{
+ //std::cout << "addToArray: nmax <= n\n";
nmax += 1024;
if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
{
@@ -266,27 +268,42 @@ bool addToArray(const T &new_element, T *array, int &n, int &nmax)
n = nmax = 0;
return false;
}
+ //std::cout << "addToArray: assigning reallocated array\n";
array = array_ref;
}
+ //std::cout << "addToArray: adding element " << n << "\n";
array[n++] = new_element;
+ //std::cout << "addToArray: added!\n";
return true;
}
template<typename T>
-bool copyArray(const T *array_in, const int &n_in, T *array_out, int &n_out, int &nmax_out)
+inline bool Navigation::copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out)
{
+ //std::cout << "copyArray: " << n_in << " elements\n";
if (array_in == NULL)
return false;
+ //std::cout << "copyArray: array in not null\n";
+
for (int i = 0; i<n_in; i++)
+ {
+ //std::cout << "copyArray: adding element " << i << "\n";
if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
+ {
+ //std::cout << "copyArray: failed to add..\n";
return false;
+ }
+ //std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
+ }
+
+ //std::cout << "copyArray: all copied\n";
return true;
}
template<typename T>
-void freeArray(T *array, int &n, int &nmax)
+void Navigation::freeArray(T *&array, int &n, int &nmax)
{
if (array != NULL)
free(array);
@@ -294,57 +311,58 @@ void freeArray(T *array, int &n, int &nmax)
n = nmax = 0;
}
-void freeNavArrays(nav_t& nav)
-{
- // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
- freeEph(nav);
- freeGeph(nav);
- freeSeph(nav);
- freePeph(nav);
- freePclk(nav);
- freeAlm(nav);
- freeTec(nav);
- freeFcb(nav);
-}
-
-void freeEph(nav_t& nav)
+inline void Navigation::freeEph(nav_t &nav)
{
freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
}
-void freeGeph(nav_t& nav)
+inline void Navigation::freeGeph(nav_t &nav)
{
freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
}
-void freeSeph(nav_t& nav)
+inline void Navigation::freeSeph(nav_t &nav)
{
freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
}
-void freePeph(nav_t& nav)
+
+inline void Navigation::freePeph(nav_t &nav)
{
freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
}
-void freeAlm(nav_t& nav)
+inline void Navigation::freeAlm(nav_t &nav)
{
freeArray<alm_t>(nav.alm, nav.na, nav.namax);
}
-void freePclk(nav_t& nav)
+inline void Navigation::freePclk(nav_t &nav)
{
freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
}
-void freeTec(nav_t& nav)
+inline void Navigation::freeTec(nav_t &nav)
{
freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
}
-void freeFcb(nav_t& nav)
+inline void Navigation::freeFcb(nav_t &nav)
{
freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
}
+inline void Navigation::freeNavArrays(nav_t &nav)
+{
+ // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+ freeEph(nav);
+ freeGeph(nav);
+ freeSeph(nav);
+ freePeph(nav);
+ freePclk(nav);
+ freeAlm(nav);
+ freeTec(nav);
+ freeFcb(nav);
+}
+
}
#endif
diff --git a/include/gnss_utils/observations.h b/include/gnss_utils/observations.h
index f41812f2a64ebf5515b47b7a1b7564add10a8cde..f0715dc58893ee4fe3e3b48868089cbc5c480923 100644
--- a/include/gnss_utils/observations.h
+++ b/include/gnss_utils/observations.h
@@ -26,6 +26,7 @@ class Observations
public:
// Constructor & Destructor
Observations();
+ Observations(const Observations& obs);
~Observations();
// Public objects
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/gnss_utils.cpp b/src/gnss_utils.cpp
index 802d0b3ccdf9318771502a67d721f24232078f43..b3a6e0298a4859cdc689e214f1267bef1fe3b8b4 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -12,16 +12,47 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
// Define error msg
char msg[128] = "";
- GNSSUtils::ComputePosOutput output;
- sol_t sol;
- sol = { { 0 } };
-
- output.pos_stat = pntpos(
- _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
-
- if (output.pos_stat == 0)
- {
- std::cout << "Bad computing: " << msg << "\n";
+ // Remove duplicated satellites
+ _navigation.uniqueNavigation();
+
+ // Define error msg
+ char msg[128] = "";
+
+ GNSSUtils::ComputePosOutput output;
+ sol_t sol;
+ sol = {{0}};
+
+ output.pos_stat = pntpos(_observations.data(), _observations.size(),
+ &(_navigation.getNavigation()),
+ &_prcopt, &sol, NULL, NULL, msg);
+
+ if (output.pos_stat == 0)
+ {
+ std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
+ }
+
+ output.time = sol.time.time;
+ output.sec = sol.time.sec;
+ output.pos = Eigen::Vector3d(sol.rr);
+ 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],
+ sol.qr[5], sol.qr[3], sol.qr[2];
+
+ // XXX: segmentation fault here.
+ // if (sol.dtr != NULL)
+ // {
+ // 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;
+ output.age = sol.age;
+ output.ratio = sol.ratio;
+ output.lat_lon = ecefToLatLonAlt(output.pos);
+
+ return output;
}
output.time = sol.time.time;
diff --git a/src/navigation.cpp b/src/navigation.cpp
index 3f3724a75ba4e2266269a83e2f06c0f6e026224e..a3e3dac38d96b00f539d712b76e5f9f8465dba14 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -1,15 +1,35 @@
#include "gnss_utils/navigation.h"
-
using namespace GNSSUtils;
Navigation::Navigation()
{
- freeNavigationArrays();
+ // array initialization
+ nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax = nav_.na = nav_.namax = nav_.nt = nav_.ntmax = 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;
+ clearNavigation();
}
Navigation::Navigation(const Navigation& nav)
{
+ // array initialization
+ nav_.n = nav_.nmax = nav_.ng = nav_.ngmax = nav_.ns = nav_.nsmax = nav_.ne = nav_.nemax = nav_.nc = nav_.ncmax = nav_.na = nav_.namax = nav_.nt = nav_.ntmax = 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;
+ clearNavigation();
setNavigation(nav.getNavigation());
}
@@ -76,6 +96,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);
@@ -205,12 +245,19 @@ void Navigation::freeNavigationArrays()
{
// RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
freeEphemeris();
+ //std::cout << "freeing glonass ephemeris...\n";
freeGlonassEphemeris();
+ //std::cout << "freeing sbas ephemeris...\n";
freeSbasEphemeris();
+ //std::cout << "freeing precise ephemeris...\n";
freePreciseEphemeris();
+ //std::cout << "freeing precise clock...\n";
freePreciseClock();
+ //std::cout << "freeing almanac...\n";
freeAlmanac();
+ //std::cout << "freeing tec...\n";
freeTecData();
+ //std::cout << "freeing fcb...\n";
freeFcbData();
}
diff --git a/src/observations.cpp b/src/observations.cpp
index b6cb50cd2ecf6560eacb662ee0e9489f0c259032..1b39857bbe0252fdf45491c6ebba405c42c8c051 100644
--- a/src/observations.cpp
+++ b/src/observations.cpp
@@ -10,6 +10,31 @@ Observations::Observations()
}
+Observations::Observations(const Observations& obs)
+ : sat_2_idx_(obs.sat_2_idx_)
+ , idx_2_sat_(obs.idx_2_sat_)
+ , obs_(obs.obs_.size())
+{
+ // copy all elements
+ for (auto i = 0; i<obs.obs_.size(); i++)
+ {
+ obs_[i].time = obs.obs_[i].time;
+ obs_[i].eventime = obs.obs_[i].eventime;
+ obs_[i].timevalid = obs.obs_[i].timevalid;
+ obs_[i].sat = obs.obs_[i].sat;
+ obs_[i].rcv = obs.obs_[i].rcv;
+ std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ+NEXOBS, obs_[i].SNR);
+ std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ+NEXOBS, obs_[i].LLI);
+ std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ+NEXOBS, obs_[i].code);
+ std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ+NEXOBS, obs_[i].qualL);
+ std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ+NEXOBS, obs_[i].qualP);
+ obs_[i].freq = obs.obs_[i].freq;
+ std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ+NEXOBS, obs_[i].L);
+ std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ+NEXOBS, obs_[i].P);
+ std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ+NEXOBS, obs_[i].D);
+ }
+}
+
Observations::~Observations()
{
}
@@ -26,11 +51,27 @@ void Observations::addObservation(const obsd_t & obs)
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
-
- assert(!hasSatellite(obs.sat) && "adding an observation of a satellite already stored!");
- this->obs_.push_back(obs);
- idx_2_sat_.push_back(obs.sat);
- sat_2_idx_[obs.sat] = obs_.size()-1;
+ // copy obsd_t object
+ obsd_t copy_obs;
+ copy_obs.time = obs.time;
+ copy_obs.eventime = obs.eventime;
+ copy_obs.timevalid = obs.timevalid;
+ copy_obs.sat = obs.sat;
+ copy_obs.rcv = obs.rcv;
+ std::copy(obs.SNR, obs.SNR + NFREQ+NEXOBS, copy_obs.SNR);
+ std::copy(obs.LLI, obs.LLI + NFREQ+NEXOBS, copy_obs.LLI);
+ std::copy(obs.code, obs.code + NFREQ+NEXOBS, copy_obs.code);
+ std::copy(obs.qualL, obs.qualL + NFREQ+NEXOBS, copy_obs.qualL);
+ std::copy(obs.qualP, obs.qualP + NFREQ+NEXOBS, copy_obs.qualP);
+ copy_obs.freq = obs.freq;
+ std::copy(obs.L, obs.L + NFREQ+NEXOBS, copy_obs.L);
+ std::copy(obs.P, obs.P + NFREQ+NEXOBS, copy_obs.P);
+ std::copy(obs.D, obs.D + NFREQ+NEXOBS, copy_obs.D);
+
+ assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
+ this->obs_.push_back(copy_obs);
+ idx_2_sat_.push_back(copy_obs.sat);
+ sat_2_idx_[copy_obs.sat] = obs_.size()-1;
//std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp
index dcb60bfbedf23add6a0429dcd53f3bb6111275ed..8fdc8cbfa8f09ff7b71f65a5c245c8edeb1cf297 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,69 +17,83 @@ 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 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;
- }
+ switch (raw_data_type_)
+ {
+ case NO: //
+ //std::cout << "0 received!\n";
+ break;
- return raw_data_type_;
+ case OBS: // Observations
+ //std::cout << "Observations received!\n";
+ updateObservations();
+ //std::cout << "Observations updated!\n";
+ break;
+
+ case NAV_EPH: // Ephemeris
+ std::cout << "Ephemeris received!\n";
+ nav_.copyEphemeris(raw_data_.nav);
+ //std::cout << "Ephemeris copied!\n";
+ break;
+
+ case NAV_SBAS: // SBAS
+ std::cout << "SBAS received!\n";
+ nav_.addSbasMessage(raw_data_.sbsmsg);
+ //std::cout << "SBAS added!\n";
+ break;
+
+ case NAV_ALM: // Almanac and ion/utc parameters
+ std::cout << "Almanac and ion/utc parameters received!\n";
+ nav_.freeAlmanac();
+ //std::cout << "Almanac freed!\n";
+ nav_.copyAlmanac(raw_data_.nav);
+ //std::cout << "Almanac copied!\n";
+ nav_.copyIonUtc(raw_data_.nav);
+ //std::cout << "ION UTC copied!\n";
+ 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_;
}
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();
}