diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1403ef88cc5639f5bc06e05882343b4799d3e30d..685a9a2f953c9e5274f29a2a470e3d5240a37f07 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,6 +52,8 @@ SET(SOURCES
src/utils/gnss_utils.cpp
src/utils/utils.cpp
src/utils/transformations.cpp
+ src/utils/rcv_position.cpp
+ src/utils/sat_position.cpp
src/observations.cpp
src/navigation.cpp
src/TDCP.cpp
@@ -92,6 +94,8 @@ SET(HEADERS
include/gnss_utils/utils/gnss_utils.h
include/gnss_utils/utils/utils.h
include/gnss_utils/utils/transformations.h
+ include/gnss_utils/utils/rcv_position.h
+ include/gnss_utils/utils/sat_position.h
include/gnss_utils/observations.h
include/gnss_utils/navigation.h
include/gnss_utils/TDCP.h
diff --git a/include/gnss_utils/TDCP.h b/include/gnss_utils/TDCP.h
index 130d9ebff6ee81e39776ec71608aa038bc540cfe..00f70f065ee7d76372e8f72848051bcca6e8a994 100644
--- a/include/gnss_utils/TDCP.h
+++ b/include/gnss_utils/TDCP.h
@@ -11,7 +11,8 @@
#define GNSS_UTILS_TDCP_DEBUG 0
#include <set>
-#include "gnss_utils/utils/gnss_utils.h"
+#include "gnss_utils/utils/rcv_position.h"
+#include "gnss_utils/utils/sat_position.h"
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
diff --git a/include/gnss_utils/utils/gnss_utils.h b/include/gnss_utils/utils/gnss_utils.h
index af36b624015fb3c3c24e15d585754f82263bd747..1e76fa4c76f5747cbac7e1f2d2f3528843554989 100644
--- a/include/gnss_utils/utils/gnss_utils.h
+++ b/include/gnss_utils/utils/gnss_utils.h
@@ -13,6 +13,7 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
#include "gnss_utils/utils/transformations.h"
+#include "gnss_utils/utils/rcv_position.h"
extern "C" {
#include "rtklib.h"
@@ -20,73 +21,6 @@ extern "C" {
namespace GNSSUtils
{
-struct ComputePosOutput
-{
- time_t time;
- double sec;
- Eigen::Vector3d pos; // position (m)
- Eigen::Vector3d vel; // velocity (m/s)
- Eigen::Matrix3d pos_covar; // position covariance (m^2)
- // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
- Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
- int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
- int stat; // solution status (SOLQ_???)
- int ns; // number of valid satellites
- double age; // age of differential (s)
- double ratio; // AR ratio factor for valiation
-
- int pos_stat; // return from pntpos
- Eigen::Vector3d lat_lon; // latitude_longitude_altitude
-};
-
-ComputePosOutput computePos(const Observations& _observations, 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);
-
-int estposOwn(const obsd_t* obs,
- int n,
- const double* rs,
- const double* dts,
- const double* vare,
- const int* svh,
- const nav_t* nav,
- const prcopt_t* opt,
- sol_t* sol,
- double* azel,
- int* vsat,
- double* resp,
- char* msg);
-
-double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
-
-void computeSatellitesPositions(const Observations& obs,
- const Navigation& nav,
- const prcopt_t& opt,
- std::map<int, Eigen::Vector3d>& sats_pos);
-
-template <typename T>
-bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2)
-{
- if (size1 != size2)
- return false;
-
- for (int ii = 0; ii < size1; ++ii)
- {
- if (array1[ii] != array2[ii])
- return false;
- }
-
- return true;
-}
-bool equalTime(const gtime_t& time1, const gtime_t& time2);
-bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
-bool equalObservations(const obs_t& obs1, const obs_t& obs2);
} // namespace GNSSUtils
diff --git a/include/gnss_utils/utils/rcv_position.h b/include/gnss_utils/utils/rcv_position.h
new file mode 100644
index 0000000000000000000000000000000000000000..55fbd80212233df121e339b21769ccb245597a95
--- /dev/null
+++ b/include/gnss_utils/utils/rcv_position.h
@@ -0,0 +1,68 @@
+/*
+ * transfromation.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
+#define INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
+
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
+
+extern "C" {
+#include "rtklib.h"
+}
+
+namespace GNSSUtils
+{
+struct ComputePosOutput
+{
+ time_t time;
+ double sec;
+ Eigen::Vector3d pos; // position (m)
+ Eigen::Vector3d vel; // velocity (m/s)
+ Eigen::Matrix3d pos_covar; // position covariance (m^2)
+ // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
+ Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
+ int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
+ int stat; // solution status (SOLQ_???)
+ int ns; // number of valid satellites
+ double age; // age of differential (s)
+ double ratio; // AR ratio factor for valiation
+
+ int pos_stat; // return from pntpos
+ Eigen::Vector3d lat_lon; // latitude_longitude_altitude
+};
+
+ComputePosOutput computePos(const Observations& _observations, 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);
+
+int estposOwn(const obsd_t* obs,
+ int n,
+ const double* rs,
+ const double* dts,
+ const double* vare,
+ const int* svh,
+ const nav_t* nav,
+ const prcopt_t* opt,
+ sol_t* sol,
+ double* azel,
+ int* vsat,
+ double* resp,
+ char* msg);
+} // namespace GNSSUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_RCV_POSITION_H_
\ No newline at end of file
diff --git a/include/gnss_utils/utils/sat_position.h b/include/gnss_utils/utils/sat_position.h
new file mode 100644
index 0000000000000000000000000000000000000000..2edb56c3a751b373a1ed26ddac76fc96250e2648
--- /dev/null
+++ b/include/gnss_utils/utils/sat_position.h
@@ -0,0 +1,32 @@
+/*
+ * sat_position.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#ifndef INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
+#define INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
+
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Geometry>
+#include <eigen3/Eigen/Sparse>
+
+#include "gnss_utils/observations.h"
+#include "gnss_utils/navigation.h"
+#include "gnss_utils/utils/transformations.h"
+
+extern "C" {
+#include "rtklib.h"
+}
+
+namespace GNSSUtils
+{
+double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
+
+void computeSatellitesPositions(const Observations& obs,
+ const Navigation& nav,
+ const prcopt_t& opt,
+ std::map<int, Eigen::Vector3d>& sats_pos);
+} // namespace GNSSUtils
+#endif // INCLUDE_GNSS_UTILS_UTILS_SAT_POSITION_H_
\ No newline at end of file
diff --git a/include/gnss_utils/utils/transformations.h b/include/gnss_utils/utils/transformations.h
index a13f6ba6974069bdcf55fb6b260e02afbe24ae3e..494d5ece16e83e8c870655e947588f4ceb782ec3 100644
--- a/include/gnss_utils/utils/transformations.h
+++ b/include/gnss_utils/utils/transformations.h
@@ -1,18 +1,13 @@
/*
- * TDCP.h
+ * transfromation.h
*
* Created on: April 3, 2020
- * Author: Pep MartÃ-Saumell
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
*/
#ifndef INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
#define INCLUDE_GNSS_UTILS_UTILS_TRANSFORMATIONS_H_
-// #include <vector>
-// #include <iostream>
-// #include <memory>
-// #include <string>
-
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Sparse>
diff --git a/include/gnss_utils/utils/utils.h b/include/gnss_utils/utils/utils.h
index fa24b0896b8d0f4158143aae1c6a6698e66fe8d3..faa390ae290e2281cdab9a37f60633414dae048a 100644
--- a/include/gnss_utils/utils/utils.h
+++ b/include/gnss_utils/utils/utils.h
@@ -9,6 +9,10 @@
#define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
#define GNSSUTILS_MSG "--GNSSUtils--"
+extern "C" {
+#include "rtklib.h"
+}
+
namespace GNSSUtils
{
void print(std::string& _msg);
@@ -16,6 +20,26 @@ void printArray(std::string _name, int* _array, int size);
void printArray(std::string _name, unsigned char* _array, int size);
void printArray(std::string _name, double* _array, int size);
void printArray(std::string _name, float* _array, int size);
+
+template <typename T>
+bool equalArray(const T* array1, const T* array2, const int& size1, const int& size2)
+{
+ if (size1 != size2)
+ return false;
+
+ for (int ii = 0; ii < size1; ++ii)
+ {
+ if (array1[ii] != array2[ii])
+ return false;
+ }
+
+ return true;
+}
+
+bool equalTime(const gtime_t& time1, const gtime_t& time2);
+bool equalObservations(const obsd_t& obs1, const obsd_t& obs2);
+bool equalObservations(const obs_t& obs1, const obs_t& obs2);
+
} // namespace GNSSUtils
#endif
\ No newline at end of file
diff --git a/src/utils/gnss_utils.cpp b/src/utils/gnss_utils.cpp
index feca02da165f8470cd0219b6b754d978cef7fb5a..fdd3296aaa19723feca9717f633100e9219f232e 100644
--- a/src/utils/gnss_utils.cpp
+++ b/src/utils/gnss_utils.cpp
@@ -2,362 +2,6 @@
namespace GNSSUtils
{
-ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
- GNSSUtils::Navigation& _navigation,
- const prcopt_t& _prcopt)
-{
- // Remove duplicated satellites
- _navigation.uniqueNavigation();
-
- // Define error msg
- char msg[128] = "";
-
- // Remove duplicated satellites
- _navigation.uniqueNavigation();
-
- GNSSUtils::ComputePosOutput output;
- sol_t sol;
- sol = { { 0 } };
-
- output.pos_stat = pntpos(
- _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
-
- if (output.pos_stat == 0)
- {
- std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
- }
-
- output.time = sol.time.time;
- output.sec = sol.time.sec;
- output.pos = Eigen::Vector3d(sol.rr);
- 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[4], sol.qr[2];
- // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
- // std::cout << "Covariance:\n" << output.pos_covar << "\n";
-
- // 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 = ecefToLatLonAlt(output.pos);
-
- 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.data(), _observations.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.sec = 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 = ecefToLatLonAlt(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=estposOwn(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;
-// }
-
-int estposOwn(const obsd_t* obs,
- int n,
- const double* rs,
- const double* dts,
- const double* vare,
- const int* svh,
- const nav_t* nav,
- const prcopt_t* opt,
- sol_t* sol,
- double* azel,
- int* vsat,
- double* resp,
- char* msg)
-{
- // double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
- // int i,j,k,info,stat,nv,ns;
- //
- // trace(3,"estpos : n=%d\n",n);
- //
- // v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
- //
- // for (i=0;i<3;i++) x[i]=sol->rr[i];
- //
- // for (i=0;i<MAXITR;i++) {
- //
- // /* pseudorange residuals */
- // nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
- // &ns);
- //
- // if (nv<NX) {
- // sprintf(msg,"lack of valid sats ns=%d",nv);
- // break;
- // }
- // /* weight by variance */
- // for (j=0;j<nv;j++) {
- // sig=sqrt(var[j]);
- // v[j]/=sig;
- // for (k=0;k<NX;k++) H[k+j*NX]/=sig;
- // }
- // /* least square estimation */
- // if ((info=lsq(H,v,NX,nv,dx,Q))) {
- // sprintf(msg,"lsq error info=%d",info);
- // break;
- // }
- // for (j=0;j<NX;j++) x[j]+=dx[j];
- //
- // if (norm(dx,NX)<1E-4) {
- // sol->type=0;
- // sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
- // sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
- // sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
- // sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
- // sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
- // for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
- // for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
- // sol->qr[3]=(float)Q[1]; /* cov xy */
- // sol->qr[4]=(float)Q[2+NX]; /* cov yz */
- // sol->qr[5]=(float)Q[2]; /* cov zx */
- // sol->ns=(unsigned char)ns;
- // sol->age=sol->ratio=0.0;
- //
- // /* validate solution */
- // if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
- // sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
- // }
- // free(v); free(H); free(var);
- //
- // return stat;
- // }
- // }
- // if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
- //
- // free(v); free(H); free(var);
-
- return 0;
-}
-
-double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
-{
- // ecef 2 geodetic
- Eigen::Vector3d receiver_geo;
- ecef2pos(receiver_ecef.data(), receiver_geo.data());
-
- // receiver-sat vector ecef
- Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
-
- // receiver-sat vector enu (receiver ecef as origin)
- Eigen::Vector3d receiver_sat_enu;
- ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
- receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
- receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
-
- // elevation
- return atan2(receiver_sat_enu(2),
- sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
-}
-
-void computeSatellitesPositions(const Observations& obs,
- const Navigation& nav,
- const prcopt_t& opt,
- std::map<int, Eigen::Vector3d>& sats_pos)
-{
- double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
- int svh[obs.size()];
-
- // std::cout << "computing sats position from sats: ";
- // for (auto&& obs_ref : obs.getObservations())
- // std::cout << (int)obs_ref.sat << " ";
- // std::cout << std::endl;
-
- // compute positions
- satposs(
- obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
-
- // store positions
- // std::cout << "filling sats positions: \n";
- for (int i = 0; i < obs.size(); i++)
- {
- if (svh[i] < 0) // ephemeris unavailable
- sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
- else
- sats_pos[obs.getObservationByIdx(i).sat] << rs[6 * i], rs[6 * i + 1], rs[6 * i + 2];
- // std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
- // sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
- }
-}
-
-bool equalTime(const gtime_t& time1, const gtime_t& time2)
-{
- return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec);
-}
-
-bool equalObservations(const obsd_t& obs1, const obsd_t& obs2)
-{
- if (!equalTime(obs1.time, obs2.time))
- return false;
- if (!equalTime(obs1.eventime, obs2.eventime))
- return false;
- if (obs1.timevalid != obs2.timevalid)
- return false;
- if (obs1.sat != obs2.sat)
- return false;
- if (obs1.rcv != obs2.rcv)
- return false;
- if (memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) != 0)
- return false;
- if (memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) != 0)
- return false;
- if (memcmp(obs1.code, obs2.code, sizeof(obs1.code)) != 0)
- return false;
- if (memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) != 0)
- return false;
- if (memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) != 0)
- return false;
- if (obs1.freq != obs2.freq)
- return false;
- if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])))
- return false;
- if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])))
- return false;
- if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])))
- return false;
-
- return true;
-}
-
-bool equalObservations(const obs_t& obs1, const obs_t& obs2)
-{
- if (obs1.n != obs2.n)
- return false;
- if (obs1.nmax != obs2.nmax)
- return false;
- if (obs1.flag != obs2.flag)
- return false;
- if (obs1.rcvcount != obs2.rcvcount)
- return false;
- if (obs1.tmcount != obs2.tmcount)
- return false;
- if (!equalObservations(*(obs1.data), *(obs2.data)))
- return false;
- return true;
-}
} // namespace GNSSUtils
diff --git a/src/utils/rcv_position.cpp b/src/utils/rcv_position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32ced1767ae1bf991cc7f92b0f1c21966072d602
--- /dev/null
+++ b/src/utils/rcv_position.cpp
@@ -0,0 +1,264 @@
+/*
+ * transfromation.h
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#include "gnss_utils/utils/rcv_position.h"
+
+using namespace GNSSUtils;
+
+namespace GNSSUtils
+{
+ComputePosOutput computePos(const GNSSUtils::Observations& _observations,
+ GNSSUtils::Navigation& _navigation,
+ const prcopt_t& _prcopt)
+{
+ // Remove duplicated satellites
+ _navigation.uniqueNavigation();
+
+ // Define error msg
+ char msg[128] = "";
+
+ // Remove duplicated satellites
+ _navigation.uniqueNavigation();
+
+ GNSSUtils::ComputePosOutput output;
+ sol_t sol;
+ sol = { { 0 } };
+
+ output.pos_stat = pntpos(
+ _observations.data(), _observations.size(), &(_navigation.getNavigation()), &_prcopt, &sol, NULL, NULL, msg);
+
+ if (output.pos_stat == 0)
+ {
+ std::cout << "computePos: error in computing positioning, message: " << msg << "\n";
+ }
+
+ output.time = sol.time.time;
+ output.sec = sol.time.sec;
+ output.pos = Eigen::Vector3d(sol.rr);
+ 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[4], sol.qr[2];
+ // std::cout << "Compute pos: " << output.pos.transpose() << "\n";
+ // std::cout << "Covariance:\n" << output.pos_covar << "\n";
+
+ // 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 = ecefToLatLonAlt(output.pos);
+
+ 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.data(), _observations.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.sec = 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 = ecefToLatLonAlt(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=estposOwn(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;
+// }
+
+int estposOwn(const obsd_t* obs,
+ int n,
+ const double* rs,
+ const double* dts,
+ const double* vare,
+ const int* svh,
+ const nav_t* nav,
+ const prcopt_t* opt,
+ sol_t* sol,
+ double* azel,
+ int* vsat,
+ double* resp,
+ char* msg)
+{
+ // double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig;
+ // int i,j,k,info,stat,nv,ns;
+ //
+ // trace(3,"estpos : n=%d\n",n);
+ //
+ // v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1);
+ //
+ // for (i=0;i<3;i++) x[i]=sol->rr[i];
+ //
+ // for (i=0;i<MAXITR;i++) {
+ //
+ // /* pseudorange residuals */
+ // nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp,
+ // &ns);
+ //
+ // if (nv<NX) {
+ // sprintf(msg,"lack of valid sats ns=%d",nv);
+ // break;
+ // }
+ // /* weight by variance */
+ // for (j=0;j<nv;j++) {
+ // sig=sqrt(var[j]);
+ // v[j]/=sig;
+ // for (k=0;k<NX;k++) H[k+j*NX]/=sig;
+ // }
+ // /* least square estimation */
+ // if ((info=lsq(H,v,NX,nv,dx,Q))) {
+ // sprintf(msg,"lsq error info=%d",info);
+ // break;
+ // }
+ // for (j=0;j<NX;j++) x[j]+=dx[j];
+ //
+ // if (norm(dx,NX)<1E-4) {
+ // sol->type=0;
+ // sol->time=timeadd(obs[0].time,-x[3]/CLIGHT);
+ // sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */
+ // sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */
+ // sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */
+ // sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */
+ // for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0;
+ // for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX];
+ // sol->qr[3]=(float)Q[1]; /* cov xy */
+ // sol->qr[4]=(float)Q[2+NX]; /* cov yz */
+ // sol->qr[5]=(float)Q[2]; /* cov zx */
+ // sol->ns=(unsigned char)ns;
+ // sol->age=sol->ratio=0.0;
+ //
+ // /* validate solution */
+ // if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) {
+ // sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE;
+ // }
+ // free(v); free(H); free(var);
+ //
+ // return stat;
+ // }
+ // }
+ // if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i);
+ //
+ // free(v); free(H); free(var);
+
+ return 0;
+}
+} // namespace GNSSUtils
\ No newline at end of file
diff --git a/src/utils/sat_position.cpp b/src/utils/sat_position.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..287fcdfc1a6a27785a4e0a3955704314c4d802db
--- /dev/null
+++ b/src/utils/sat_position.cpp
@@ -0,0 +1,63 @@
+/*
+ * sat_position.cpp
+ *
+ * Created on: April 3, 2020
+ * Author: Joan Vallvé, Pep MartÃ-Saumell
+ */
+
+#include "gnss_utils/utils/sat_position.h"
+
+using namespace GNSSUtils;
+
+namespace GNSSUtils
+{
+double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
+{
+ // ecef 2 geodetic
+ Eigen::Vector3d receiver_geo;
+ ecef2pos(receiver_ecef.data(), receiver_geo.data());
+
+ // receiver-sat vector ecef
+ Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef;
+
+ // receiver-sat vector enu (receiver ecef as origin)
+ Eigen::Vector3d receiver_sat_enu;
+ ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad)
+ receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z}
+ receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u}
+
+ // elevation
+ return atan2(receiver_sat_enu(2),
+ sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1)));
+}
+
+void computeSatellitesPositions(const Observations& obs,
+ const Navigation& nav,
+ const prcopt_t& opt,
+ std::map<int, Eigen::Vector3d>& sats_pos)
+{
+ double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()];
+ int svh[obs.size()];
+
+ // std::cout << "computing sats position from sats: ";
+ // for (auto&& obs_ref : obs.getObservations())
+ // std::cout << (int)obs_ref.sat << " ";
+ // std::cout << std::endl;
+
+ // compute positions
+ satposs(
+ obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), opt.sateph, rs, dts, var, svh);
+
+ // store positions
+ // std::cout << "filling sats positions: \n";
+ for (int i = 0; i < obs.size(); i++)
+ {
+ if (svh[i] < 0) // ephemeris unavailable
+ sats_pos[obs.getObservationByIdx(i).sat] = Eigen::Vector3d::Zero();
+ else
+ sats_pos[obs.getObservationByIdx(i).sat] << rs[6 * i], rs[6 * i + 1], rs[6 * i + 2];
+ // std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << ": " <<
+ // sats_pos[obs.getObservationByIdx(i).sat].transpose() << std::endl;
+ }
+}
+} // namespace GNSSUtils
\ No newline at end of file
diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp
index 3dcd1e2731871f74fd04d26ee1d1a26a788b5d17..5336b047d0b0901ac502de3f4d443e5868bc5168 100644
--- a/src/utils/utils.cpp
+++ b/src/utils/utils.cpp
@@ -61,4 +61,60 @@ void printArray(std::string _name, float* _array, int size)
}
}
+bool equalTime(const gtime_t& time1, const gtime_t& time2)
+{
+ return (difftime(time1.time, time2.time) == 0.0 && time1.sec == time2.sec);
+}
+
+bool equalObservations(const obsd_t& obs1, const obsd_t& obs2)
+{
+ if (!equalTime(obs1.time, obs2.time))
+ return false;
+ if (!equalTime(obs1.eventime, obs2.eventime))
+ return false;
+ if (obs1.timevalid != obs2.timevalid)
+ return false;
+ if (obs1.sat != obs2.sat)
+ return false;
+ if (obs1.rcv != obs2.rcv)
+ return false;
+ if (memcmp(obs1.SNR, obs2.SNR, sizeof(obs1.SNR)) != 0)
+ return false;
+ if (memcmp(obs1.LLI, obs2.LLI, sizeof(obs1.LLI)) != 0)
+ return false;
+ if (memcmp(obs1.code, obs2.code, sizeof(obs1.code)) != 0)
+ return false;
+ if (memcmp(obs1.qualL, obs2.qualL, sizeof(obs1.qualL)) != 0)
+ return false;
+ if (memcmp(obs1.qualP, obs2.qualP, sizeof(obs1.qualP)) != 0)
+ return false;
+ if (obs1.freq != obs2.freq)
+ return false;
+ if (!equalArray<double>(obs1.L, obs2.L, sizeof(obs1.L) / sizeof(obs1.L[0]), sizeof(obs2.L) / sizeof(obs2.L[0])))
+ return false;
+ if (!equalArray<double>(obs1.P, obs2.P, sizeof(obs1.P) / sizeof(obs1.P[0]), sizeof(obs2.P) / sizeof(obs2.P[0])))
+ return false;
+ if (!equalArray<float>(obs1.D, obs2.D, sizeof(obs1.D) / sizeof(obs1.D[0]), sizeof(obs2.D) / sizeof(obs2.D[0])))
+ return false;
+
+ return true;
+}
+
+bool equalObservations(const obs_t& obs1, const obs_t& obs2)
+{
+ if (obs1.n != obs2.n)
+ return false;
+ if (obs1.nmax != obs2.nmax)
+ return false;
+ if (obs1.flag != obs2.flag)
+ return false;
+ if (obs1.rcvcount != obs2.rcvcount)
+ return false;
+ if (obs1.tmcount != obs2.tmcount)
+ return false;
+ if (!equalObservations(*(obs1.data), *(obs2.data)))
+ return false;
+
+ return true;
+}
} // namespace GNSSUtils
diff --git a/test/gtest_transformations.cpp b/test/gtest_transformations.cpp
index 8b819bb63bf4ce6174e2dfc7ce5f5035f7202c1b..8984b1009d368b9160ae5c43b88b99d00e1b00ca 100644
--- a/test/gtest_transformations.cpp
+++ b/test/gtest_transformations.cpp
@@ -1,5 +1,5 @@
#include "gtest/utils_gtest.h"
-#include "gnss_utils/utils/gnss_utils.h"
+#include "gnss_utils/utils/sat_position.h"
// Geodetic system parameters
static double kSemimajorAxis = 6378137;