From 882d0aa868916ea6356e85630d4fd0c00ea33c49 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 19 Nov 2019 16:01:53 +0100
Subject: [PATCH] WIP
---
include/gnss_utils/gnss_utils.h | 8 ++
src/examples/CMakeLists.txt | 2 +-
src/gnss_utils.cpp | 126 ++++++++++++++++++++++++++++++++
3 files changed, 135 insertions(+), 1 deletion(-)
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 6260309..f2c3aa0 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 12d787d..fc610d3 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 dcc3f24..018da28 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];
--
GitLab