Skip to content
Snippets Groups Projects
Commit 22ca7d32 authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

Merge branch 'single_differences' of...

Merge branch 'single_differences' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils into single_differences
parents f48bc6c2 e6fb46fa
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
...@@ -13,24 +13,6 @@ extern "C" ...@@ -13,24 +13,6 @@ extern "C"
namespace GNSSUtils 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; class Navigation;
typedef std::shared_ptr<Navigation> NavigationPtr; typedef std::shared_ptr<Navigation> NavigationPtr;
typedef std::shared_ptr<const Navigation> NavigationConstPtr; typedef std::shared_ptr<const Navigation> NavigationConstPtr;
...@@ -47,6 +29,7 @@ class Navigation ...@@ -47,6 +29,7 @@ class Navigation
// Public methods // Public methods
void setNavigation(nav_t nav); 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 = ""); 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; const nav_t & getNavigation() const;
...@@ -84,6 +67,23 @@ class Navigation ...@@ -84,6 +67,23 @@ class Navigation
void print(); 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: private:
// rtklib-like attribute to represent the different navigation msgs for a given epoch // rtklib-like attribute to represent the different navigation msgs for a given epoch
...@@ -251,12 +251,14 @@ inline void Navigation::freeAlmanac() ...@@ -251,12 +251,14 @@ inline void Navigation::freeAlmanac()
//////////////////////////////// nav UTILS ////////////////////////////////////// //////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T> 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 // "inspired" from RTKLIB rinex.c
T *array_ref; T *array_ref;
if (nmax <= n) if (nmax <= n)
{ {
//std::cout << "addToArray: nmax <= n\n";
nmax += 1024; nmax += 1024;
if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax))) 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) ...@@ -266,27 +268,42 @@ bool addToArray(const T &new_element, T *array, int &n, int &nmax)
n = nmax = 0; n = nmax = 0;
return false; return false;
} }
//std::cout << "addToArray: assigning reallocated array\n";
array = array_ref; array = array_ref;
} }
//std::cout << "addToArray: adding element " << n << "\n";
array[n++] = new_element; array[n++] = new_element;
//std::cout << "addToArray: added!\n";
return true; return true;
} }
template<typename T> 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) if (array_in == NULL)
return false; return false;
//std::cout << "copyArray: array in not null\n";
for (int i = 0; i<n_in; i++) 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)) if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
{
//std::cout << "copyArray: failed to add..\n";
return false; return false;
}
//std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
}
//std::cout << "copyArray: all copied\n";
return true; return true;
} }
template<typename T> template<typename T>
void freeArray(T *array, int &n, int &nmax) void Navigation::freeArray(T *&array, int &n, int &nmax)
{ {
if (array != NULL) if (array != NULL)
free(array); free(array);
...@@ -294,57 +311,58 @@ void freeArray(T *array, int &n, int &nmax) ...@@ -294,57 +311,58 @@ void freeArray(T *array, int &n, int &nmax)
n = nmax = 0; n = nmax = 0;
} }
void freeNavArrays(nav_t& nav) inline void Navigation::freeEph(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)
{ {
freeArray<eph_t>(nav.eph, nav.n, nav.nmax); 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); 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); 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); 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); 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); 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); 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); 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 #endif
...@@ -26,6 +26,7 @@ class Observations ...@@ -26,6 +26,7 @@ class Observations
public: public:
// Constructor & Destructor // Constructor & Destructor
Observations(); Observations();
Observations(const Observations& obs);
~Observations(); ~Observations();
// Public objects // Public objects
......
...@@ -6,7 +6,7 @@ ...@@ -6,7 +6,7 @@
namespace GNSSUtils 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 class UBloxRaw
{ {
...@@ -14,12 +14,12 @@ class UBloxRaw ...@@ -14,12 +14,12 @@ class UBloxRaw
UBloxRaw(); 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(); Observations& getObservations();
Navigation getNavigation(); Navigation& getNavigation();
const int& getRawDataType() const; RawDataType getRawDataType() const;
private: private:
raw_t raw_data_; raw_t raw_data_;
...@@ -27,23 +27,23 @@ class UBloxRaw ...@@ -27,23 +27,23 @@ class UBloxRaw
Observations obs_; Observations obs_;
Navigation nav_; Navigation nav_;
int raw_data_type_; RawDataType raw_data_type_;
void updateObservations(); void updateObservations();
}; };
inline GNSSUtils::Observations UBloxRaw::getObservations() inline GNSSUtils::Observations& UBloxRaw::getObservations()
{ {
return obs_; return obs_;
} }
inline GNSSUtils::Navigation UBloxRaw::getNavigation() inline GNSSUtils::Navigation& UBloxRaw::getNavigation()
{ {
return nav_; return nav_;
} }
inline const int& UBloxRaw::getRawDataType() const inline RawDataType UBloxRaw::getRawDataType() const
{ {
return raw_data_type_; return raw_data_type_;
} }
......
...@@ -12,16 +12,47 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations, ...@@ -12,16 +12,47 @@ ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
// Define error msg // Define error msg
char msg[128] = ""; char msg[128] = "";
GNSSUtils::ComputePosOutput output; // Remove duplicated satellites
sol_t sol; _navigation.uniqueNavigation();
sol = { { 0 } };
// Define error msg
output.pos_stat = pntpos( char msg[128] = "";
_observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
GNSSUtils::ComputePosOutput output;
if (output.pos_stat == 0) sol_t sol;
{ sol = {{0}};
std::cout << "Bad computing: " << msg << "\n";
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; output.time = sol.time.time;
......
#include "gnss_utils/navigation.h" #include "gnss_utils/navigation.h"
using namespace GNSSUtils; using namespace GNSSUtils;
Navigation::Navigation() 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) 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()); setNavigation(nav.getNavigation());
} }
...@@ -76,6 +96,26 @@ void Navigation::setNavigation(nav_t _nav) ...@@ -76,6 +96,26 @@ void Navigation::setNavigation(nav_t _nav)
// pppcorr_t pppcorr; /* ppp corrections */ // 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) 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); auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, NULL, &nav_, NULL);
...@@ -205,12 +245,19 @@ void Navigation::freeNavigationArrays() ...@@ -205,12 +245,19 @@ void Navigation::freeNavigationArrays()
{ {
// RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
freeEphemeris(); freeEphemeris();
//std::cout << "freeing glonass ephemeris...\n";
freeGlonassEphemeris(); freeGlonassEphemeris();
//std::cout << "freeing sbas ephemeris...\n";
freeSbasEphemeris(); freeSbasEphemeris();
//std::cout << "freeing precise ephemeris...\n";
freePreciseEphemeris(); freePreciseEphemeris();
//std::cout << "freeing precise clock...\n";
freePreciseClock(); freePreciseClock();
//std::cout << "freeing almanac...\n";
freeAlmanac(); freeAlmanac();
//std::cout << "freeing tec...\n";
freeTecData(); freeTecData();
//std::cout << "freeing fcb...\n";
freeFcbData(); freeFcbData();
} }
......
...@@ -10,6 +10,31 @@ Observations::Observations() ...@@ -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() Observations::~Observations()
{ {
} }
...@@ -26,11 +51,27 @@ void Observations::addObservation(const obsd_t & obs) ...@@ -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() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size()); assert(sat_2_idx_.size() == obs_.size());
// copy obsd_t object
assert(!hasSatellite(obs.sat) && "adding an observation of a satellite already stored!"); obsd_t copy_obs;
this->obs_.push_back(obs); copy_obs.time = obs.time;
idx_2_sat_.push_back(obs.sat); copy_obs.eventime = obs.eventime;
sat_2_idx_[obs.sat] = obs_.size()-1; 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; //std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
......
...@@ -3,13 +3,13 @@ ...@@ -3,13 +3,13 @@
using namespace GNSSUtils; using namespace GNSSUtils;
UBloxRaw::UBloxRaw() : UBloxRaw::UBloxRaw() :
raw_data_type_(0) raw_data_type_(NO)
{ {
if (init_raw(&raw_data_, STRFMT_UBX) == 0) if (init_raw(&raw_data_, STRFMT_UBX) == 0)
{ {
assert("Failed when allocating memory for raw_t"); assert("Failed when allocating memory for raw_t");
return; return;
} }
}; };
UBloxRaw::~UBloxRaw() UBloxRaw::~UBloxRaw()
...@@ -17,69 +17,83 @@ UBloxRaw::~UBloxRaw() ...@@ -17,69 +17,83 @@ UBloxRaw::~UBloxRaw()
free_raw(&raw_data_); 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 // Update type based on RTKLIB
for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte) 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); raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
}
switch (raw_data_type_) switch (raw_data_type_)
{ {
case 1: case NO: //
updateObservations(); //std::cout << "0 received!\n";
break; 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;
}
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() void UBloxRaw::updateObservations()
{ {
// sort and remove duplicated observations // sort and remove duplicated observations
sortobs(&raw_data_.obs); sortobs(&raw_data_.obs);
// std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl; // std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
// obs_.print(); // 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_.clearObservations();
// obs_.print(); 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();
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment