diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h index 626030968e0fa13278085403435d955227a8a2f3..f2c3aa049b25b149eaf7ac825d0d859d6e704358 100644 --- a/include/gnss_utils/gnss_utils.h +++ b/include/gnss_utils/gnss_utils.h @@ -42,6 +42,14 @@ namespace GNSSUtils Navigation & _navigation, const prcopt_t & _prcopt); +// ComputePosOutput computePosOwn(const Observations & _observations, +// Navigation & _navigation, +// const prcopt_t & _prcopt); +// +// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, +// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, +// char *msg); + Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef); } diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index 12d787d210f74ab3fa8ddfe041661bd77684b075..fc610d33faaa89b374209caea5bf7afd0cfbd27f 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -5,4 +5,4 @@ INCLUDE_DIRECTORIES(../../include) ADD_EXECUTABLE(gnss_utils_test gnss_utils_test.cpp) # link necessary libraries -TARGET_LINK_LIBRARIES(gnss_utils_test gnss_utils) +TARGET_LINK_LIBRARIES(gnss_utils_test ${gnss_utils_LIBRARIES}) diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp index dcc3f24cc5aa51cb925e9211b4b610f9fe4b1502..018da28b37ad34443c03268fca6d14639a36ce8f 100644 --- a/src/gnss_utils.cpp +++ b/src/gnss_utils.cpp @@ -53,6 +53,132 @@ namespace GNSSUtils return output; } +// ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations, +// GNSSUtils::Navigation & _navigation, +// const prcopt_t & _prcopt) +// { +// +// // Remove duplicated satellites +// uniqnav(&(_navigation.getNavigation())); +// +// // Define error msg +// char msg[128] = ""; +// +// GNSSUtils::ComputePosOutput output; +// sol_t sol; +// sol = {{0}}; +// +// output.pos_stat = pntposOwn(_observations.getObservations().data(), _observations.getObservations().size(), +// &(_navigation.getNavigation()), +// &_prcopt, &sol, NULL, NULL, msg); +// +// if (output.pos_stat == 0) +// { +// std::cout << "Bad computing: " << msg << "\n"; +// } +// +// output.time = sol.time.time; +// output.time = 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 = ecefToLatLon(output.pos); +// +// return output; +// } +// +// /* single-point positioning COPIED FROM RTKLIB ------------------------------ +// * compute receiver position, velocity, clock bias by single-point positioning +// * with pseudorange and doppler observables +// * args : obsd_t *obs I observation data +// * int n I number of observation data +// * nav_t *nav I navigation data +// * prcopt_t *opt I processing options +// * sol_t *sol IO solution +// * double *azel IO azimuth/elevation angle (rad) (NULL: no output) +// * ssat_t *ssat IO satellite status (NULL: no output) +// * char *msg O error message for error exit +// * return : status(1:ok,0:error) +// * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and +// * receiver bias are negligible (only involving glonass-gps time offset +// * and receiver bias) +// *-----------------------------------------------------------------------------*/ +// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, +// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, +// char *msg) +// { +// prcopt_t opt_=*opt; +// double *rs,*dts,*var,*azel_,*resp; +// int i,stat,vsat[MAXOBS]={0},svh[MAXOBS]; +// +// sol->stat=SOLQ_NONE; +// +// if (n<=0) {strcpy(msg,"no observation data"); return 0;} +// +// trace(3,"pntpos : tobs=%s n=%d\n",time_str(obs[0].time,3),n); +// +// sol->time=obs[0].time; msg[0]='\0'; +// +// rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n); +// +// if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */ +// #if 0 +// opt_.sateph =EPHOPT_BRDC; +// #endif +// opt_.ionoopt=IONOOPT_BRDC; +// opt_.tropopt=TROPOPT_SAAS; +// } +// /* satellite positons, velocities and clocks */ +// satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh); +// +// /* estimate receiver position with pseudorange */ +// stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); +// +// /* raim fde */ +// if (!stat&&n>=6&&opt->posopt[4]) { +// stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg); +// } +// /* estimate receiver velocity with doppler */ +// //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat); +// +// if (azel) { +// for (i=0;i<n*2;i++) azel[i]=azel_[i]; +// } +// if (ssat) { +// for (i=0;i<MAXSAT;i++) { +// ssat[i].vs=0; +// ssat[i].azel[0]=ssat[i].azel[1]=0.0; +// ssat[i].resp[0]=ssat[i].resc[0]=0.0; +// ssat[i].snr[0]=0; +// } +// for (i=0;i<n;i++) { +// ssat[obs[i].sat-1].azel[0]=azel_[ i*2]; +// ssat[obs[i].sat-1].azel[1]=azel_[1+i*2]; +// ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0]; +// if (!vsat[i]) continue; +// ssat[obs[i].sat-1].vs=1; +// ssat[obs[i].sat-1].resp[0]=resp[i]; +// } +// } +// free(rs); free(dts); free(var); free(azel_); free(resp); +// return stat; +// } + Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef) { double pos[3];