more items in navigation

include/gnss_utils/navigation.h

@@ -37,6 +37,7 @@ public:
   bool addAlmanac(const alm_t& alm);
   bool addTec(const tec_t& tec);
   bool addFcb(const fcbd_t& fcb);
+  bool addErp(const erpd_t& erp);
 
   void addSbasMessage(const sbsmsg_t& sbas_msg);
 
@@ -45,6 +46,8 @@ public:
   void copyAlmanac(const nav_t& nav);
   void copyIonUtc(const nav_t& nav);
   void copySbasCorrections(const nav_t& nav);
+  void copyErp(const nav_t& nav);
+  void copyBias(const nav_t& nav);
 
   void freeNavigationArrays();
   void freeEphemeris();
@@ -55,6 +58,7 @@ public:
   void freePreciseClock();
   void freeTecData();
   void freeFcbData();
+  void freeErpData();
 
   void print();
 
@@ -67,6 +71,7 @@ public:
   static void freePclk(nav_t& nav);
   static void freeTec(nav_t& nav);
   static void freeFcb(nav_t& nav);
+  static void freeErp(nav_t& nav);
   static void freeNavArrays(nav_t& nav);
 
 private:
@@ -94,58 +99,16 @@ inline nav_t& Navigation::getNavigation()
 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)
@@ -161,20 +124,6 @@ inline bool Navigation::addPreciseClock(const pclk_t& pclk)
 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)
@@ -187,6 +136,11 @@ inline bool Navigation::addFcb(const fcbd_t& fcb)
   return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
 }
 
+inline bool Navigation::addErp(const erpd_t& erp)
+{
+  return addToArray<erpd_t>(erp, nav_.erp.data, nav_.erp.n, nav_.erp.nmax);
+}
+
 inline void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
 {
   sbsupdatecorr(&sbas_msg, &nav_);
@@ -232,6 +186,11 @@ inline void Navigation::freeAlmanac()
   freeAlm(nav_);
 }
 
+inline void Navigation::freeErpData()
+{
+    freeErp(nav_);
+}
+
 //////////////////////////////// nav UTILS //////////////////////////////////////
 inline void Navigation::freeEph(nav_t& nav)
 {
@@ -273,6 +232,11 @@ inline void Navigation::freeFcb(nav_t& nav)
   freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
 }
 
+inline void Navigation::freeErp(nav_t& nav)
+{
+  freeArray<erpd_t>(nav.erp.data, nav.erp.n, nav.erp.nmax);
+}
+
 inline void Navigation::freeNavArrays(nav_t& nav)
 {
   // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
@@ -284,6 +248,7 @@ inline void Navigation::freeNavArrays(nav_t& nav)
   freeAlm(nav);
   freeTec(nav);
   freeFcb(nav);
+  freeErp(nav);
 }
 
 }  // namespace GnssUtils

include/gnss_utils/utils/utils.h

@@ -11,6 +11,8 @@
 #include "gnss_utils/observations.h"
 
 #define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
+#define ARRAY2D_SIZE(arr) sizeof(arr) / sizeof(arr[0][0])
+#define ARRAY3D_SIZE(arr) sizeof(arr) / sizeof(arr[0][0][0])
 #define ARRAY2D_NROWS(arr) sizeof(arr) / sizeof(arr[0])
 #define ARRAY2D_NCOLS(arr) sizeof(arr[0]) / sizeof(arr[0][0])
 #define GNSSUTILS_MSG "--GnssUtils--"

src/navigation.cpp

@@ -94,14 +94,18 @@ void Navigation::setNavigation(nav_t _nav)
   //    sbssat_t sbssat;    /* SBAS satellite corrections */
   //    sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
 
-
-  // ********** other not copied nav_t content: ***********
+  copyErp(_nav);
   //    erp_t  erp;         /* earth rotation parameters */
+
+  copyBias(_nav);
   //    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 */
+  //    char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
+
+
+  // ********** other not copied nav_t content: ***********
   //    pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
   //    dgps_t dgps[MAXSAT]; /* DGPS corrections */
   //    ssr_t ssr[MAXSAT];  /* SSR corrections */
@@ -113,18 +117,27 @@ void Navigation::setNavigation(nav_t _nav)
 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));
+
+  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));
+
+  memset(nav_.cbias,        0, sizeof(nav_.cbias));
+  memset(nav_.rbias,        0, sizeof(nav_.rbias));
+  memset(nav_.wlbias,       0, sizeof(nav_.wlbias));
+  memset(nav_.glo_cpbias,   0, sizeof(nav_.glo_cpbias));
+  memset(nav_.glo_fcn,      0, sizeof(nav_.glo_fcn));
+
   nav_.leaps           = 0;
   sbssat_t sbssat_zero = { 0 };
   nav_.sbssat          = sbssat_zero;
@@ -172,31 +185,8 @@ void Navigation::copyAllArrays(const nav_t& nav)
 void Navigation::copyEphemeris(const nav_t& nav)
 {
   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();
 }
@@ -204,13 +194,6 @@ void Navigation::copyEphemeris(const nav_t& nav)
 void Navigation::copyAlmanac(const nav_t& nav)
 {
   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::copyIonUtc(const nav_t& nav)
@@ -258,6 +241,21 @@ void Navigation::copySbasCorrections(const nav_t& nav)
   }
 }
 
+void Navigation::copyErp(const nav_t& nav)
+{
+    copyArray<erpd_t>(nav.erp.data, nav.erp.n, nav_.erp.data, nav_.erp.n, nav_.erp.nmax);
+}
+
+void Navigation::copyBias(const nav_t& nav)
+{
+    std::copy(&nav.cbias[0][0],     &nav.cbias[0][0]    + ARRAY2D_SIZE(nav.cbias),  &nav_.cbias[0][0]);
+    std::copy(&nav.rbias[0][0][0],  &nav.rbias[0][0][0] + ARRAY3D_SIZE(nav.rbias),  &nav_.rbias[0][0][0]);
+
+    std::copy(nav.wlbias,       nav.wlbias      + ARRAY_SIZE(nav.wlbias),       nav_.wlbias);
+    std::copy(nav.glo_cpbias,   nav.glo_cpbias  + ARRAY_SIZE(nav.glo_cpbias),   nav_.glo_cpbias);
+    std::copy(nav.glo_fcn,      nav.glo_fcn     + ARRAY_SIZE(nav.glo_fcn),      nav_.glo_fcn);
+}
+
 void Navigation::freeNavigationArrays()
 {
   // RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
@@ -276,20 +274,12 @@ void Navigation::freeNavigationArrays()
   freeTecData();
   // std::cout << "freeing fcb...\n";
   freeFcbData();
+  // std::cout << "freeing erp...\n";
+  freeErpData();
 }
 
 void Navigation::print()
 {
-
-    //    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 */
-
   std::cout << "n: " << nav_.n << "\n";
   std::cout << "ng: " << nav_.ng << "\n";
   std::cout << "ns: " << nav_.ns << "\n";
@@ -308,6 +298,7 @@ void Navigation::print()
   std::cout << "ntmax: " << nav_.ntmax << "\n";
   std::cout << "nfmax: " << nav_.nfmax << "\n";
 
+  // TODO
     //    eph_t *eph;         /* GPS/QZS/GAL ephemeris */
     //    geph_t *geph;       /* GLONASS ephemeris */
     //    seph_t *seph;       /* SBAS ephemeris */
@@ -318,19 +309,6 @@ void Navigation::print()
     //    fcbd_t *fcb;        /* satellite fcb data */
     //    double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
 
-    //    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} */
-
   printArray<double, ARRAY_SIZE(nav_.utc_gps)>("utc_gps: ", nav_.utc_gps);
   printArray<double, ARRAY_SIZE(nav_.utc_glo)>("utc_glo: ", nav_.utc_glo);
   printArray<double, ARRAY_SIZE(nav_.utc_qzs)>("utc_qzs: ", nav_.utc_qzs);
@@ -343,12 +321,8 @@ void Navigation::print()
   printArray<double, ARRAY_SIZE(nav_.ion_cmp)>("ion_cmp: ", nav_.ion_cmp);
   printArray<double, ARRAY_SIZE(nav_.ion_irn)>("ion_irn: ", nav_.ion_irn);
 
-    //    int leaps;          /* leap seconds (s) */
-
   std::cout << "leaps: " << nav_.leaps << "\n";
 
-    //    sbssat_t sbssat;    /* SBAS satellite corrections */
-    //    sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
   std::cout << "sbssat: \n";
   std::cout << "\tiodp: " << nav_.sbssat.iodp << "\n";
   std::cout << "\tnsat: " << nav_.sbssat.nsat << "\n";
@@ -389,13 +363,16 @@ void Navigation::print()
         }
     }
 
-    // ********** other not copied nav_t content: ***********
+    // TODO:
     //    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 */
+    //    char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
+
+
+    // ********** other not copied nav_t content: ***********
     //    pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
     //    dgps_t dgps[MAXSAT]; /* DGPS corrections */
     //    ssr_t ssr[MAXSAT];  /* SSR corrections */