diff --git a/include/gnss_utils/utils/utils.h b/include/gnss_utils/utils/utils.h index c73b035d8d8bd57f2190662fbcb3549b5dcb5d40..b5a8d134e8aef41cc06accb428e0319ded3ee340 100644 --- a/include/gnss_utils/utils/utils.h +++ b/include/gnss_utils/utils/utils.h @@ -24,7 +24,7 @@ void print(std::string& _msg); template <typename T, size_t size> void printArray(std::string name, T (&array)[size]) { - std::cout << name << ": ["; + std::cout << name << "["; for (int ii = 0; ii < size; ++ii) { if (std::is_same<T, char>::value or std::is_same<T, unsigned char>::value) diff --git a/src/navigation.cpp b/src/navigation.cpp index af421b711f719a741451bbdf9093a20b252b2614..c0eb72c4681d6cb650e960e8fba5ac50cd4bc5db 100644 --- a/src/navigation.cpp +++ b/src/navigation.cpp @@ -283,25 +283,16 @@ void Navigation::freeNavigationArrays() void Navigation::print() { - std::cout << "n: " << nav_.n << "\n"; - std::cout << "ng: " << nav_.ng << "\n"; - std::cout << "ns: " << nav_.ns << "\n"; - std::cout << "ne: " << nav_.ne << "\n"; - std::cout << "nc: " << nav_.nc << "\n"; - std::cout << "na: " << nav_.na << "\n"; - std::cout << "nt: " << nav_.nt << "\n"; - std::cout << "nf: " << nav_.nf << "\n"; - - std::cout << "nmax: " << nav_.nmax << "\n"; - std::cout << "ngmax: " << nav_.ngmax << "\n"; - std::cout << "nsmax: " << nav_.nsmax << "\n"; - std::cout << "nemax: " << nav_.nemax << "\n"; - std::cout << "ncmax: " << nav_.ncmax << "\n"; - std::cout << "namax: " << nav_.namax << "\n"; - std::cout << "ntmax: " << nav_.ntmax << "\n"; - std::cout << "nfmax: " << nav_.nfmax << "\n"; - - // TODO + std::cout << "n: " << nav_.n << "\t(nmax: " << nav_.nmax << ")\n"; + std::cout << "ng: " << nav_.ng << "\t(nfmax: " << nav_.nfmax << ")\n"; + std::cout << "ns: " << nav_.ns << "\t(nsmax: " << nav_.nsmax << ")\n"; + std::cout << "ne: " << nav_.ne << "\t(nemax: " << nav_.nemax << ")\n"; + std::cout << "nc: " << nav_.nc << "\t(ncmax: " << nav_.ncmax << ")\n"; + std::cout << "na: " << nav_.na << "\t(namax: " << nav_.namax << ")\n"; + std::cout << "nt: " << nav_.nt << "\t(ntmax: " << nav_.ntmax << ")\n"; + std::cout << "nf: " << nav_.nf << "\t(nfmax: " << nav_.nfmax << ")\n"; + + // TODO // eph_t *eph; /* GPS/QZS/GAL ephemeris */ // geph_t *geph; /* GLONASS ephemeris */ // seph_t *seph; /* SBAS ephemeris */ @@ -312,68 +303,102 @@ void Navigation::print() // fcbd_t *fcb; /* satellite fcb data */ // double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */ - 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); - printArray<double, ARRAY_SIZE(nav_.utc_cmp)>("utc_cmp: ", nav_.utc_cmp); - printArray<double, ARRAY_SIZE(nav_.utc_irn)>("utc_irn: ", nav_.utc_irn); - printArray<double, ARRAY_SIZE(nav_.utc_sbs)>("utc_sbs: ", nav_.utc_sbs); - printArray<double, ARRAY_SIZE(nav_.ion_gps)>("ion_gps: ", nav_.ion_gps); - printArray<double, ARRAY_SIZE(nav_.ion_gal)>("ion_gal: ", nav_.ion_gal); - printArray<double, ARRAY_SIZE(nav_.ion_qzs)>("ion_qzs: ", nav_.ion_qzs); - printArray<double, ARRAY_SIZE(nav_.ion_cmp)>("ion_cmp: ", nav_.ion_cmp); - printArray<double, ARRAY_SIZE(nav_.ion_irn)>("ion_irn: ", nav_.ion_irn); - - std::cout << "leaps: " << nav_.leaps << "\n"; - - std::cout << "sbssat: \n"; - std::cout << "\tiodp: " << nav_.sbssat.iodp << "\n"; - std::cout << "\tnsat: " << nav_.sbssat.nsat << "\n"; - std::cout << "\ttlat: " << nav_.sbssat.tlat << "\n"; - for (int i = 0; i < nav_.sbssat.nsat; i++) + 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_gal)>("utc_gal: ", nav_.utc_gal); + printArray<double, ARRAY_SIZE(nav_.utc_qzs)>("utc_qzs: ", nav_.utc_qzs); + printArray<double, ARRAY_SIZE(nav_.utc_cmp)>("utc_cmp: ", nav_.utc_cmp); + printArray<double, ARRAY_SIZE(nav_.utc_irn)>("utc_irn: ", nav_.utc_irn); + printArray<double, ARRAY_SIZE(nav_.utc_sbs)>("utc_sbs: ", nav_.utc_sbs); + printArray<double, ARRAY_SIZE(nav_.ion_gps)>("ion_gps: ", nav_.ion_gps); + printArray<double, ARRAY_SIZE(nav_.ion_gal)>("ion_gal: ", nav_.ion_gal); + printArray<double, ARRAY_SIZE(nav_.ion_qzs)>("ion_qzs: ", nav_.ion_qzs); + printArray<double, ARRAY_SIZE(nav_.ion_cmp)>("ion_cmp: ", nav_.ion_cmp); + printArray<double, ARRAY_SIZE(nav_.ion_irn)>("ion_irn: ", nav_.ion_irn); + + std::cout << "leaps: " << nav_.leaps << "\n"; + + std::cout << "lam: \n"; + for (int i = 0; i < ARRAY2D_NROWS(nav_.lam); i++) + printArray<double, ARRAY2D_NCOLS(nav_.lam)>("\t", nav_.lam[i]); + + std::cout << "cbias: \n"; + for (int i = 0; i < ARRAY2D_NROWS(nav_.cbias); i++) + printArray<double, ARRAY2D_NCOLS(nav_.cbias)>("\t", nav_.cbias[i]); + + printArray<double, ARRAY_SIZE(nav_.wlbias)> ("wlbias: ", nav_.wlbias); + printArray<double, ARRAY_SIZE(nav_.glo_cpbias)>("glo_cpbias: ", nav_.glo_cpbias); + printArray<char, ARRAY_SIZE(nav_.glo_fcn)> ("glo_fcn: ", nav_.glo_fcn); + + std::cout << "pcvs: \n"; + for (int i = 0; i < ARRAY_SIZE(nav_.pcvs); i++) { - std::cout << "\tsat: " << i << "\n"; - std::cout << "\t\tsat " << nav_.sbssat.sat[i].sat << "\n"; - - std::cout << "\t\tfcorr.t0 " << nav_.sbssat.sat[i].fcorr.t0.time << " + "<< nav_.sbssat.sat[i].fcorr.t0.sec << "\n"; - std::cout << "\t\tfcorr.prc " << nav_.sbssat.sat[i].fcorr.prc << "\n"; - std::cout << "\t\tfcorr.rrc " << nav_.sbssat.sat[i].fcorr.rrc << "\n"; - std::cout << "\t\tfcorr.dt " << nav_.sbssat.sat[i].fcorr.dt << "\n"; - std::cout << "\t\tfcorr.iodf " << nav_.sbssat.sat[i].fcorr.iodf << "\n"; - std::cout << "\t\tfcorr.udre " << nav_.sbssat.sat[i].fcorr.udre << "\n"; - std::cout << "\t\tfcorr.ai " << nav_.sbssat.sat[i].fcorr.ai << "\n"; - - std::cout << "\t\tlcorr.daf0 " << nav_.sbssat.sat[i].lcorr.daf0 << "\n"; - std::cout << "\t\tlcorr.daf1 " << nav_.sbssat.sat[i].lcorr.daf1 << "\n"; - std::cout << "\t\tlcorr.iode " << nav_.sbssat.sat[i].lcorr.iode << "\n"; - std::cout << "\t\tlcorr.t0 " << nav_.sbssat.sat[i].lcorr.t0.time << " + "<< nav_.sbssat.sat[i].lcorr.t0.sec << "\n"; - printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dpos)>("\t\tlcorr.dpos: ", nav_.sbssat.sat[i].lcorr.dpos); - printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dvel)>("\t\tlcorr.dvel: ", nav_.sbssat.sat[i].lcorr.dvel); + std::cout << "\tsat: " << nav_.pcvs[i].sat << "\n"; + printArray<char, ARRAY_SIZE(nav_.pcvs[i].type)>("\t\ttype: ", nav_.pcvs[i].type); + printArray<char, ARRAY_SIZE(nav_.pcvs[i].code)>("\t\tcode: ", nav_.pcvs[i].code); + + std::cout << "\t\tte: " << nav_.pcvs[i].te.time << " + "<< nav_.pcvs[i].te.sec << "\n"; + std::cout << "\t\tts: " << nav_.pcvs[i].ts.time << " + "<< nav_.pcvs[i].ts.sec << "\n"; + std::cout << "\t\toff: \n"; + for (int j = 0; j < ARRAY2D_NROWS(nav_.pcvs[i].off); j++) + printArray<double, ARRAY2D_NCOLS(nav_.pcvs[i].off)>("\t\t\t", nav_.pcvs[i].off[j]); + std::cout << "\t\tvar: \n"; + for (int j = 0; j < ARRAY2D_NROWS(nav_.pcvs[i].var); j++) + printArray<double, ARRAY2D_NCOLS(nav_.pcvs[i].var)>("\t\t\t", nav_.pcvs[i].var[j]); + } + + if (nav_.sbssat.nsat == 0) + std::cout << "sbssat: empty\n"; + else + { + std::cout << "sbssat: \n"; + std::cout << "\tiodp: " << nav_.sbssat.iodp << "\n"; + std::cout << "\tnsat: " << nav_.sbssat.nsat << "\n"; + std::cout << "\ttlat: " << nav_.sbssat.tlat << "\n"; + for (int i = 0; i < nav_.sbssat.nsat; i++) + { + std::cout << "\tsat: " << i << "\n"; + std::cout << "\t\tsat " << nav_.sbssat.sat[i].sat << "\n"; + + std::cout << "\t\tfcorr.t0 " << nav_.sbssat.sat[i].fcorr.t0.time << " + "<< nav_.sbssat.sat[i].fcorr.t0.sec << "\n"; + std::cout << "\t\tfcorr.prc " << nav_.sbssat.sat[i].fcorr.prc << "\n"; + std::cout << "\t\tfcorr.rrc " << nav_.sbssat.sat[i].fcorr.rrc << "\n"; + std::cout << "\t\tfcorr.dt " << nav_.sbssat.sat[i].fcorr.dt << "\n"; + std::cout << "\t\tfcorr.iodf " << nav_.sbssat.sat[i].fcorr.iodf << "\n"; + std::cout << "\t\tfcorr.udre " << nav_.sbssat.sat[i].fcorr.udre << "\n"; + std::cout << "\t\tfcorr.ai " << nav_.sbssat.sat[i].fcorr.ai << "\n"; + + std::cout << "\t\tlcorr.daf0 " << nav_.sbssat.sat[i].lcorr.daf0 << "\n"; + std::cout << "\t\tlcorr.daf1 " << nav_.sbssat.sat[i].lcorr.daf1 << "\n"; + std::cout << "\t\tlcorr.iode " << nav_.sbssat.sat[i].lcorr.iode << "\n"; + std::cout << "\t\tlcorr.t0 " << nav_.sbssat.sat[i].lcorr.t0.time << " + "<< nav_.sbssat.sat[i].lcorr.t0.sec << "\n"; + printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dpos)>("\t\tlcorr.dpos: ", nav_.sbssat.sat[i].lcorr.dpos); + printArray<double, ARRAY_SIZE(nav_.sbssat.sat[i].lcorr.dvel)>("\t\tlcorr.dvel: ", nav_.sbssat.sat[i].lcorr.dvel); + } } for (int i = 0; i < MAXBAND + 1; i++) { - std::cout << "sbsion: " << i << "\n"; - std::cout << "\tiodi: " << nav_.sbsion[i].iodi << "\n"; - std::cout << "\tnigp: " << nav_.sbsion[i].nigp << "\n"; - for (int j = 0; j < nav_.sbsion[i].nigp; j++) + if (nav_.sbsion[i].nigp == 0) + std::cout << "sbsion: " << i << " empty\n"; + else { - std::cout << "\tigp: " << j << "\n"; - std::cout << "\t\tt0: " << nav_.sbsion[i].igp[j].t0.time << " + "<< nav_.sbsion[i].igp[j].t0.sec << "\n"; - std::cout << "\t\tlat: " << nav_.sbsion[i].igp[j].lat << "\n"; - std::cout << "\t\tlon: " << nav_.sbsion[i].igp[j].lon << "\n"; - std::cout << "\t\tgive: " << nav_.sbsion[i].igp[j].give << "\n"; - std::cout << "\t\tdelay: " << nav_.sbsion[i].igp[j].delay << "\n"; + std::cout << "sbsion: " << i << "\n"; + std::cout << "\tiodi: " << nav_.sbsion[i].iodi << "\n"; + std::cout << "\tnigp: " << nav_.sbsion[i].nigp << "\n"; + for (int j = 0; j < nav_.sbsion[i].nigp; j++) + { + std::cout << "\tigp: " << j << "\n"; + std::cout << "\t\tt0: " << nav_.sbsion[i].igp[j].t0.time << " + "<< nav_.sbsion[i].igp[j].t0.sec << "\n"; + std::cout << "\t\tlat: " << nav_.sbsion[i].igp[j].lat << "\n"; + std::cout << "\t\tlon: " << nav_.sbsion[i].igp[j].lon << "\n"; + std::cout << "\t\tgive: " << nav_.sbsion[i].igp[j].give << "\n"; + std::cout << "\t\tdelay: " << nav_.sbsion[i].igp[j].delay << "\n"; + } } } // TODO: - // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ - // 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) */ - // char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */ - // ********** other not copied nav_t content: *********** // erp_t erp; /* earth rotation parameters */