diff --git a/deps/RTKLIB b/deps/RTKLIB
index eac44a45c19ac56c0c031135e9818788b5993005..4b8f505328faeb827fe7e0eb5adac1dddaf39cf4 160000
--- a/deps/RTKLIB
+++ b/deps/RTKLIB
@@ -1 +1 @@
-Subproject commit eac44a45c19ac56c0c031135e9818788b5993005
+Subproject commit 4b8f505328faeb827fe7e0eb5adac1dddaf39cf4
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 5ba6069e9ace935c1f2b347e93b82af29f741862..ec75472312f3add20a8730ce79abace251d5e674 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,
- const Navigation & _navigation,
+ 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 846e7c6a2ce20801940586675ef0c09b1392063d..3f0317e17c1ada0c8a5c5dfa2c586b96cff02fba 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -13,6 +13,24 @@ 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;
@@ -28,29 +46,41 @@ class Navigation
// Public objects
// Public methods
-
- void clearNavigation();
-
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 = "");
const nav_t & getNavigation() const;
nav_t & getNavigation();
+ void uniqueNavigation(); //remove duplicated ephemerides and update carrier phase wave lengths
/****************** Array memory management ******************/
- bool addEphemeris(const eph_t& eph);
- bool addGLONASSEphemeris(const geph_t& geph);
- bool addSBASEphemeris(const seph_t& seph);
+ bool addEphemeris(const eph_t &eph);
+ bool addGlonassEphemeris(const geph_t& geph);
+ bool addSbasEphemeris(const seph_t& seph);
+ bool addPreciseEphemeris(const peph_t& peph);
+ bool addPreciseClock(const pclk_t& pclk);
bool addAlmanac(const alm_t& alm);
-
- void clearEphemeris();
- void clearGLONASSEphemeris();
- void clearSBASEphemeris();
- void clearAlmanac();
+ bool addTec(const tec_t& tec);
+ bool addFcb(const fcbd_t& fcb);
+
+ void addSbasMessage(const sbsmsg_t &sbas_msg);
- void copyEphemeris(const nav_t& nav);
- void copyAlmanac(const nav_t& nav);
+ void copyAllArrays(const nav_t& nav);
+ void copyEphemeris(const nav_t& nav);
+ void copyAlmanac(const nav_t& nav);
+ void copyIonUtc(const nav_t &nav);
+ void copySbasCorrections(const nav_t &nav);
+
+ void freeNavigationArrays();
+ void freeEphemeris();
+ void freeGlonassEphemeris();
+ void freeSbasEphemeris();
+ void freePreciseEphemeris();
+ void freeAlmanac();
+ void freePreciseClock();
+ void freeTecData();
+ void freeFcbData();
void print();
@@ -62,5 +92,259 @@ class Navigation
// Private methods
};
+inline void Navigation::uniqueNavigation() //remove duplicated ephemerides and update wave lengths
+{
+ uniqnav(&nav_);
+}
+
+inline const nav_t & Navigation::getNavigation() const
+{
+ return nav_;
+}
+
+inline nav_t & Navigation::getNavigation()
+{
+ return nav_;
+}
+
+inline bool Navigation::addEphemeris(const eph_t &eph)
+{
+ return addToArray<eph_t>(eph, nav_.eph, nav_.n, nav_.nmax);
+ // // "inspired" from RTKLIB rinex.c
+ // eph_t *nav_eph;
+ //
+ // if (nav_.nmax<=nav_.n) {
+ // nav_.nmax+=1024;
+ // if (!(nav_eph=(eph_t *)realloc(nav_.eph,sizeof(eph_t)*nav_.nmax))) {
+ // printf("addEphemeris malloc error: n=%d\n",nav_.nmax);
+ // free(nav_.eph); nav_.eph=NULL; nav_.n=nav_.nmax=0;
+ // return false;
+ // }
+ // nav_.eph=nav_eph;
+ // }
+ // nav_.eph[nav_.n++]=eph;
+ // return true;
+}
+
+inline bool Navigation::addGlonassEphemeris(const geph_t& geph)
+{
+ return addToArray<geph_t>(geph, nav_.geph, nav_.ng, nav_.ngmax);
+// // "inspired" from RTKLIB rinex.c
+// geph_t *nav_geph;
+//
+// if (nav_.ngmax<=nav_.ng) {
+// nav_.ngmax+=1024;
+// if (!(nav_geph=(geph_t *)realloc(nav_.geph,sizeof(geph_t)*nav_.ngmax))) {
+// printf("addGLONASSEphemeris malloc error: n=%d\n",nav_.ngmax);
+// free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;
+// return false;
+// }
+// nav_.geph=nav_geph;
+// }
+// nav_.geph[nav_.ng++]=geph;
+// return true;
+}
+
+inline bool Navigation::addSbasEphemeris(const seph_t& seph)
+{
+ return addToArray<seph_t>(seph, nav_.seph, nav_.ns, nav_.nsmax);
+// // "inspired" from RTKLIB rinex.c
+// seph_t *nav_seph;
+//
+// if (nav_.nsmax<=nav_.ns) {
+// nav_.nsmax+=1024;
+// if (!(nav_seph=(seph_t *)realloc(nav_.seph,sizeof(seph_t)*nav_.nsmax))) {
+// printf("addSBASEphemeris malloc error: n=%d\n",nav_.nsmax);
+// free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;
+// return false;
+// }
+// nav_.seph=nav_seph;
+// }
+// nav_.seph[nav_.ns++]=seph;
+// return true;
+}
+
+inline bool Navigation::addPreciseEphemeris(const peph_t& peph)
+{
+ return addToArray<peph_t>(peph, nav_.peph, nav_.ne, nav_.nemax);
+}
+
+inline bool Navigation::addPreciseClock(const pclk_t& pclk)
+{
+ return addToArray<pclk_t>(pclk, nav_.pclk, nav_.nc, nav_.ncmax);
+}
+
+inline bool Navigation::addAlmanac(const alm_t& alm)
+{
+ return addToArray<alm_t>(alm, nav_.alm, nav_.na, nav_.namax);
+// // "inspired" from RTKLIB rinex.c
+// alm_t *nav_alm;
+//
+// if (nav_.namax<=nav_.na) {
+// nav_.namax+=1024;
+// if (!(nav_alm=(alm_t *)realloc(nav_.alm,sizeof(alm_t)*nav_.namax))) {
+// printf("addAlmanac malloc error: n=%d\n",nav_.namax);
+// free(nav_.alm); nav_.alm=NULL; nav_.na=nav_.namax=0;
+// return false;
+// }
+// nav_.alm=nav_alm;
+// }
+// nav_.alm[nav_.na++]=alm;
+// return true;
+}
+
+inline bool Navigation::addTec(const tec_t& tec)
+{
+ return addToArray<tec_t>(tec, nav_.tec, nav_.nt, nav_.ntmax);
+}
+
+inline bool Navigation::addFcb(const fcbd_t& fcb)
+{
+ return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
+}
+
+inline void Navigation::addSbasMessage(const sbsmsg_t &sbas_msg)
+{
+ sbsupdatecorr(&sbas_msg, &nav_);
+}
+
+
+inline void Navigation::freeEphemeris()
+{
+ freeEph(nav_);
+}
+
+inline void Navigation::freeGlonassEphemeris()
+{
+ freeGeph(nav_);
+}
+
+inline void Navigation::freePreciseEphemeris()
+{
+ freePeph(nav_);
+}
+
+inline void Navigation::freeSbasEphemeris()
+{
+ freeSeph(nav_);
+}
+
+inline void Navigation::freePreciseClock()
+{
+ freePclk(nav_);
+}
+
+inline void Navigation::freeTecData()
+{
+ freeTec(nav_);
+}
+
+inline void Navigation::freeFcbData()
+{
+ freeFcb(nav_);
+}
+
+inline void Navigation::freeAlmanac()
+{
+ freeAlm(nav_);
+}
+
+//////////////////////////////// nav UTILS //////////////////////////////////////
+template <typename T>
+bool addToArray(const T &new_element, T *array, int &n, int &nmax)
+{
+ // "inspired" from RTKLIB rinex.c
+ T *array_ref;
+ if (nmax <= n)
+ {
+ nmax += 1024;
+ if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
+ {
+ printf("addToArray malloc error: n=%d\n", nmax);
+ free(array);
+ array = NULL;
+ n = nmax = 0;
+ return false;
+ }
+ array = array_ref;
+ }
+ array[n++] = new_element;
+ return true;
+}
+
+template<typename T>
+bool copyArray(const T *array_in, const int &n_in, T *array_out, int &n_out, int &nmax_out)
+{
+ if (array_in == NULL)
+ return false;
+
+ for (int i = 0; i<n_in; i++)
+ if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
+ return false;
+
+ return true;
+}
+
+template<typename T>
+void freeArray(T *array, int &n, int &nmax)
+{
+ if (array != NULL)
+ free(array);
+ array = NULL;
+ 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)
+{
+ freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
+}
+
+void freeGeph(nav_t& nav)
+{
+ freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
+}
+
+void freeSeph(nav_t& nav)
+{
+ freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
+}
+void freePeph(nav_t& nav)
+{
+ freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
+}
+
+void freeAlm(nav_t& nav)
+{
+ freeArray<alm_t>(nav.alm, nav.na, nav.namax);
+}
+
+void freePclk(nav_t& nav)
+{
+ freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
+}
+
+void freeTec(nav_t& nav)
+{
+ freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
+}
+
+void freeFcb(nav_t& nav)
+{
+ freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
+}
+
}
#endif
diff --git a/include/gnss_utils/ublox_raw.h b/include/gnss_utils/ublox_raw.h
index 2075544df2fa3360ea04214fea5e13d6577d4703..4d248c2ce9634d70adb8921deb668dbea62ab70a 100644
--- a/include/gnss_utils/ublox_raw.h
+++ b/include/gnss_utils/ublox_raw.h
@@ -33,7 +33,22 @@ class UBloxRaw
};
+inline GNSSUtils::Observations UBloxRaw::getObservations()
+{
+ return obs_;
+}
+
+inline GNSSUtils::Navigation UBloxRaw::getNavigation()
+{
+ return nav_;
+}
+
+inline const int& UBloxRaw::getRawDataType() const
+{
+ return raw_data_type_;
+}
+
}
-#endif
\ No newline at end of file
+#endif
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 91099bd5899a1cd1221cc8b727e59bb037021055..743ef28df3a20e6bc6b556dd57f4685804a48eba 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -4,12 +4,12 @@ namespace GNSSUtils
{
ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
- const GNSSUtils::Navigation & _navigation,
- const prcopt_t & _prcopt)
+ GNSSUtils::Navigation & _navigation,
+ const prcopt_t & _prcopt)
{
// Remove duplicated satellites
- uniqnav(&(_navigation.getNavigation()));
+ _navigation.uniqueNavigation();
// Define error msg
char msg[128] = "";
diff --git a/src/navigation.cpp b/src/navigation.cpp
index 6784c86b3ee9bdbd6eb85ddfc1aff3859b80c410..3f3724a75ba4e2266269a83e2f06c0f6e026224e 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -5,93 +5,75 @@ 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;
+ freeNavigationArrays();
}
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);
+ setNavigation(nav.getNavigation());
}
Navigation::~Navigation()
{
- clearNavigation();
-}
-
-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;}
+ freeNavigationArrays();
}
void Navigation::setNavigation(nav_t _nav)
{
- clearNavigation();
- nav_ = _nav;
-}
-
-const nav_t & Navigation::getNavigation() const
-{
- return nav_;
-}
-
-nav_t & Navigation::getNavigation()
-{
- return nav_;
+ freeNavigationArrays();
+
+ copyAllArrays(_nav);
+ // int n,nmax; /* number of broadcast ephemeris */
+ // int ng,ngmax; /* number of glonass ephemeris */
+ // int ns,nsmax; /* number of sbas ephemeris */
+ // int ne,nemax; /* number of precise ephemeris */
+ // int nc,ncmax; /* number of precise clock */
+ // int na,namax; /* number of almanac data */
+ // int nt,ntmax; /* number of tec grid data */
+ // int nf,nfmax; /* number of satellite fcb data */
+ // eph_t *eph; /* GPS/QZS/GAL ephemeris */
+ // geph_t *geph; /* GLONASS ephemeris */
+ // seph_t *seph; /* SBAS ephemeris */
+ // peph_t *peph; /* precise ephemeris */
+ // pclk_t *pclk; /* precise clock */
+ // alm_t *alm; /* almanac data */
+ // tec_t *tec; /* tec grid data */
+ // fcbd_t *fcb; /* satellite fcb data */
+ // double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
+
+ copyIonUtc(_nav);
+ // double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
+ // double utc_glo[4]; /* GLONASS UTC GPS time parameters */
+ // double utc_gal[4]; /* Galileo UTC GPS time parameters */
+ // double utc_qzs[4]; /* QZS UTC GPS time parameters */
+ // double utc_cmp[4]; /* BeiDou UTC parameters */
+ // double utc_irn[4]; /* IRNSS UTC parameters */
+ // double utc_sbs[4]; /* SBAS UTC parameters */
+ // double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */
+ // double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
+ // int leaps; /* leap seconds (s) */
+
+ copySbasCorrections(_nav);
+ // sbssat_t sbssat; /* SBAS satellite corrections */
+ // sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
+
+
+ // ********** other not copied nav_t content: ***********
+ // erp_t erp; /* earth rotation parameters */
+ // double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+ // double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
+ // double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
+ // double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
+ // SHOULD WE COPY THIS???????? char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
+ // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
+ // dgps_t dgps[MAXSAT]; /* DGPS corrections */
+ // ssr_t ssr[MAXSAT]; /* SSR corrections */
+ // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
+ // lexion_t lexion; /* LEX ionosphere correction */
+ // pppcorr_t pppcorr; /* ppp corrections */
}
void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
@@ -104,140 +86,132 @@ void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gti
std::cout << "GLONASS satellites in navigation file: " << nav_.ng << std::endl;
std::cout << "SBAS satellites in navigation file: " << nav_.ns << std::endl;
std::cout << "Almanac satellites in navigation file: " << nav_.na << std::endl;
- uniqnav(&nav_);
+ uniqueNavigation();
}
else
std::cout << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
}
-bool Navigation::addEphemeris(const eph_t& eph)
+void Navigation::copyAllArrays(const nav_t& nav)
{
- // "inspired" from RTKLIB rinex.c
- eph_t *nav_eph;
+ //GPS/QZS/GAL ephemeris
+ copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+ //GLONASS ephemeris
+ copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+ //SBAS ephemeris
+ copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+ //precise ephemeris
+ copyArray<peph_t>(nav.peph, nav.ne, nav_.peph, nav_.ne, nav_.nemax);
+ //precise clock
+ copyArray<pclk_t>(nav.pclk, nav.nc, nav_.pclk, nav_.nc, nav_.ncmax);
+ //almanac data
+ copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+ //tec grid data
+ copyArray<tec_t>(nav.tec, nav.nt, nav_.tec, nav_.nt, nav_.ntmax);
+ //satellite fcb data
+ copyArray<fcbd_t>(nav.fcb, nav.nf, nav_.fcb, nav_.nf, nav_.nfmax);
- if (nav_.nmax<=nav_.n) {
- nav_.nmax+=1024;
- if (!(nav_eph=(eph_t *)realloc(nav_.eph,sizeof(eph_t)*nav_.nmax))) {
- printf("addEphemeris malloc error: n=%d\n",nav_.nmax);
- free(nav_.eph); nav_.eph=NULL; nav_.n=nav_.nmax=0;
- return false;
- }
- nav_.eph=nav_eph;
- }
- nav_.eph[nav_.n++]=eph;
- return true;
+ uniqueNavigation();
}
-bool Navigation::addGLONASSEphemeris(const geph_t& geph)
-{
- // "inspired" from RTKLIB rinex.c
- geph_t *nav_geph;
-
- if (nav_.ngmax<=nav_.ng) {
- nav_.ngmax+=1024;
- if (!(nav_geph=(geph_t *)realloc(nav_.geph,sizeof(geph_t)*nav_.ngmax))) {
- printf("addGLONASSEphemeris malloc error: n=%d\n",nav_.ngmax);
- free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;
- return false;
- }
- nav_.geph=nav_geph;
- }
- nav_.geph[nav_.ng++]=geph;
- return true;
-}
-
-bool Navigation::addSBASEphemeris(const seph_t& seph)
-{
- // "inspired" from RTKLIB rinex.c
- seph_t *nav_seph;
-
- if (nav_.nsmax<=nav_.ns) {
- nav_.nsmax+=1024;
- if (!(nav_seph=(seph_t *)realloc(nav_.seph,sizeof(seph_t)*nav_.nsmax))) {
- printf("addSBASEphemeris malloc error: n=%d\n",nav_.nsmax);
- free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;
- return false;
- }
- nav_.seph=nav_seph;
- }
- nav_.seph[nav_.ns++]=seph;
- return true;
-}
-
-bool Navigation::addAlmanac(const alm_t& alm)
+void Navigation::copyEphemeris(const nav_t& nav)
{
- // "inspired" from RTKLIB rinex.c
- alm_t *nav_alm;
-
- if (nav_.namax<=nav_.na) {
- nav_.namax+=1024;
- if (!(nav_alm=(alm_t *)realloc(nav_.alm,sizeof(alm_t)*nav_.namax))) {
- printf("addAlmanac malloc error: n=%d\n",nav_.namax);
- free(nav_.alm); nav_.alm=NULL; nav_.na=nav_.namax=0;
- return false;
- }
- nav_.alm=nav_alm;
- }
- nav_.alm[nav_.na++]=alm;
- return true;
+ copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+// if (nav.eph != NULL)
+// {
+// for (int ii = 0; ii < nav.n; ++ii)
+// {
+// addEphemeris(nav.eph[ii]);
+// }
+// }
+
+ copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+// if (nav.geph != NULL)
+// {
+// for (int ii = 0; ii < nav.ng; ++ii)
+// {
+// addGLONASSEphemeris(nav.geph[ii]);
+// }
+// }
+
+ copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+// if (nav.seph != NULL)
+// {
+// for (int ii = 0; ii < nav.ns; ++ii)
+// {
+// addSBASEphemeris(nav.seph[ii]);
+// }
+// }
+
+ uniqueNavigation();
}
-void Navigation::clearEphemeris()
+void Navigation::copyAlmanac(const nav_t& nav)
{
- if (nav_.eph != NULL) {free(nav_.eph ); nav_.eph =NULL; nav_.n =nav_.nmax =0;}
+ copyArray<alm_t>(nav.alm, nav.na, nav_.alm, nav_.na, nav_.namax);
+// if (nav.alm != NULL)
+// {
+// for (int ii = 0; ii < nav.na; ++ii)
+// {
+// addAlmanac(nav.alm[ii]);
+// }
+// }
}
-void Navigation::clearGLONASSEphemeris()
+void Navigation::copyIonUtc(const nav_t &nav)
{
- if (nav_.geph != NULL) {free(nav_.geph); nav_.geph=NULL; nav_.ng=nav_.ngmax=0;}
-}
+ std::copy(nav.utc_gps, nav.utc_gps + 4, nav_.utc_gps);
+ std::copy(nav.utc_gal, nav.utc_gal + 4, nav_.utc_gal);
+ std::copy(nav.utc_qzs, nav.utc_qzs + 4, nav_.utc_qzs);
+ std::copy(nav.utc_glo, nav.utc_glo + 4, nav_.utc_glo);
+ std::copy(nav.utc_cmp, nav.utc_cmp + 4, nav_.utc_cmp);
+ std::copy(nav.utc_irn, nav.utc_irn + 4, nav_.utc_irn);
+ std::copy(nav.utc_sbs, nav.utc_sbs + 4, nav_.utc_sbs);
-void Navigation::clearSBASEphemeris()
-{
- if (nav_.seph != NULL) {free(nav_.seph); nav_.seph=NULL; nav_.ns=nav_.nsmax=0;}
-}
+ std::copy(nav.ion_gps, nav.ion_gps + 8, nav_.ion_gps);
+ std::copy(nav.ion_gal, nav.ion_gal + 4, nav_.ion_gal);
+ std::copy(nav.ion_qzs, nav.ion_qzs + 8, nav_.ion_qzs);
+ std::copy(nav.ion_cmp, nav.ion_cmp + 4, nav_.ion_cmp);
+ std::copy(nav.ion_irn, nav.ion_irn + 8, nav_.ion_irn);
-void Navigation::clearAlmanac()
-{
- if (nav_.alm != NULL) {free(nav_.alm ); nav_.alm =NULL; nav_.na=nav_.namax=0;}
+ nav_.leaps = nav.leaps;
}
-void Navigation::copyEphemeris(const nav_t& nav)
+void Navigation::copySbasCorrections(const nav_t &nav)
{
- if (nav.eph != NULL)
- {
- for (int ii = 0; ii < nav.n; ++ii)
+ nav_.sbssat.iodp = nav.sbssat.iodp;
+ nav_.sbssat.nsat = nav.sbssat.nsat;
+ nav_.sbssat.tlat = nav.sbssat.tlat;
+ for (int i=0; i < MAXSAT; i++)
{
- addEphemeris(nav.eph[ii]);
+ nav_.sbssat.sat[i].sat = nav.sbssat.sat[i].sat;
+ nav_.sbssat.sat[i].fcorr = nav.sbssat.sat[i].fcorr;
+ nav_.sbssat.sat[i].lcorr.daf0 = nav.sbssat.sat[i].lcorr.daf0;
+ nav_.sbssat.sat[i].lcorr.daf1 = nav.sbssat.sat[i].lcorr.daf1;
+ nav_.sbssat.sat[i].lcorr.iode = nav.sbssat.sat[i].lcorr.iode;
+ nav_.sbssat.sat[i].lcorr.t0 = nav.sbssat.sat[i].lcorr.t0;
+ std::copy(nav.sbssat.sat[i].lcorr.dpos, nav.sbssat.sat[i].lcorr.dpos + 3, nav_.sbssat.sat[i].lcorr.dpos);
+ std::copy(nav.sbssat.sat[i].lcorr.dvel, nav.sbssat.sat[i].lcorr.dvel + 3, nav_.sbssat.sat[i].lcorr.dvel);
}
- }
-
- if (nav.geph != NULL)
- {
- for (int ii = 0; ii < nav.ng; ++ii)
+ for (int i=0; i < MAXBAND+1; i++)
{
- addGLONASSEphemeris(nav.geph[ii]);
+ nav_.sbsion[i].iodi = nav.sbsion[i].iodi;
+ nav_.sbsion[i].nigp = nav.sbsion[i].nigp;
+ std::copy(nav.sbsion[i].igp, nav.sbsion[i].igp + MAXNIGP, nav_.sbsion[i].igp);
}
- }
-
- if (nav.seph != NULL)
- {
- for (int ii = 0; ii < nav.ns; ++ii)
- {
- addSBASEphemeris(nav.seph[ii]);
- }
- }
}
-void Navigation::copyAlmanac(const nav_t& nav)
+void Navigation::freeNavigationArrays()
{
- if (nav.alm != NULL)
- {
- for (int ii = 0; ii < nav.na; ++ii)
- {
- addAlmanac(nav.alm[ii]);
- }
- }
+ // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
+ freeEphemeris();
+ freeGlonassEphemeris();
+ freeSbasEphemeris();
+ freePreciseEphemeris();
+ freePreciseClock();
+ freeAlmanac();
+ freeTecData();
+ freeFcbData();
}
void Navigation::print()
diff --git a/src/ublox_raw.cpp b/src/ublox_raw.cpp
index a6613cae851c9ea86b83aec3eb7cbc25a1690746..dcb60bfbedf23add6a0429dcd53f3bb6111275ed 100644
--- a/src/ublox_raw.cpp
+++ b/src/ublox_raw.cpp
@@ -2,18 +2,20 @@
using namespace GNSSUtils;
-UBloxRaw::UBloxRaw()
+UBloxRaw::UBloxRaw() :
+ raw_data_type_(0)
{
if (init_raw(&raw_data_, STRFMT_UBX) == 0)
{
assert("Failed when allocating memory for raw_t");
return;
}
-
- raw_data_type_ = 0;
};
-UBloxRaw::~UBloxRaw(){};
+UBloxRaw::~UBloxRaw()
+{
+ free_raw(&raw_data_);
+};
int UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
{
@@ -30,46 +32,45 @@ int UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
break;
case 2:
// Ephemeris
- nav_.clearEphemeris();
- nav_.clearGLONASSEphemeris();
- nav_.clearSBASEphemeris();
nav_.copyEphemeris(raw_data_.nav);
break;
case 3:
// SBAS
- nav_.clearEphemeris();
- nav_.clearGLONASSEphemeris();
- nav_.clearSBASEphemeris();
- nav_.copyEphemeris(raw_data_.nav);
- updateObservations();
+ nav_.addSbasMessage(raw_data_.sbsmsg);
break;
case 9:
- // Almanac
- nav_.clearAlmanac();
+ // 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_;
}
-Observations UBloxRaw::getObservations()
-{
- return obs_;
-}
-
-Navigation UBloxRaw::getNavigation()
-{
- return nav_;
-}
-
-const int& UBloxRaw::getRawDataType() const
-{
- 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();
@@ -81,4 +82,4 @@ void UBloxRaw::updateObservations()
// std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
// obs_.print();
-}
\ No newline at end of file
+}