Skip to content
Snippets Groups Projects
Commit be2bdd84 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch 'single_differences' of

ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git
into single_differences

Conflicts:
	include/gnss_utils/ublox_raw.h
	src/gnss_utils.cpp
	src/ublox_raw.cpp
parents 8a059fc3 a6c2c58d
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Showing
with 2493 additions and 1754 deletions
bin/
build/
lib/
\ No newline at end of file
lib/
.clang-format
{
"configurations": [
{
"name": "Linux",
"includePath": [
"${workspaceFolder}/**",
"${workspaceFolder}/include"
],
"defines": [],
"compilerPath": "/usr/bin/clang",
"cStandard": "c11",
"intelliSenseMode": "clang-x64"
}
],
"version": 4
}
\ No newline at end of file
......@@ -17,47 +17,69 @@
namespace GNSSUtils
{
struct TDCPParams
{
int min_common_sats;
int raim_n;
double raim_min_residual;
bool use_carrier_phase;
bool correct_tropo;
bool correct_iono;
bool relinearize_jacobian;
int max_iterations;
double sigma_atm;
double sigma_code;
double sigma_carrier;
bool use_old_nav;
bool use_multi_freq;
double time_window;
};
bool TDCP(const Observations& obs_r, Navigation& nav_r,
const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
double& residual, std::set<int>& discarded_sats,
const TDCPParams& sd_opt, const prcopt_t& opt);
bool TDCP(const Observations& obs_r, const Navigation& nav_r, const Eigen::Vector3d& x_r,
const Observations& obs_k, const Navigation& nav_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
double& residual, std::set<int>& discarded_sats,
const TDCPParams& sd_opt, const prcopt_t& opt);
bool TDCP(const Observations& common_obs_r, const Navigation& nav_r, const std::map<int,Eigen::Vector3d>& common_sats_pos_r, const Eigen::Vector3d& x_r,
const Observations& common_obs_k, const Navigation& nav_k, const std::map<int,Eigen::Vector3d>& common_sats_pos_k,
Eigen::Vector4d& d, Eigen::Matrix4d& cov_d,
double& residual, std::set<int>& discarded_sats,
const TDCPParams& sd_opt);
void filterCommonSatellites(Observations& common_obs_r, std::map<int,Eigen::Vector3d>& common_sats_pos_r,
Observations& common_obs_k, std::map<int,Eigen::Vector3d>& common_sats_pos_k,
std::set<int>& discarded_sats, const Eigen::Vector3d& x_r,
const TDCPParams& sd_params, const prcopt_t& opt);
}
struct TDCPParams
{
int min_common_sats;
int raim_n;
double raim_min_residual;
bool use_carrier_phase;
bool correct_tropo;
bool correct_iono;
bool relinearize_jacobian;
int max_iterations;
double sigma_atm;
double sigma_code;
double sigma_carrier;
bool use_old_nav;
bool use_multi_freq;
double time_window;
};
bool TDCP(const Observations& obs_r,
Navigation& nav_r,
const Observations& obs_k,
const Navigation& nav_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int>& discarded_sats,
const TDCPParams& sd_opt,
const prcopt_t& opt);
bool TDCP(const Observations& obs_r,
const Navigation& nav_r,
const Eigen::Vector3d& x_r,
const Observations& obs_k,
const Navigation& nav_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int>& discarded_sats,
const TDCPParams& sd_opt,
const prcopt_t& opt);
bool TDCP(const Observations& common_obs_r,
const Navigation& nav_r,
const std::map<int, Eigen::Vector3d>& common_sats_pos_r,
const Eigen::Vector3d& x_r,
const Observations& common_obs_k,
const Navigation& nav_k,
const std::map<int, Eigen::Vector3d>& common_sats_pos_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int>& discarded_sats,
const TDCPParams& sd_opt);
void filterCommonSatellites(Observations& common_obs_r,
std::map<int, Eigen::Vector3d>& common_sats_pos_r,
Observations& common_obs_k,
std::map<int, Eigen::Vector3d>& common_sats_pos_k,
std::set<int>& discarded_sats,
const Eigen::Vector3d& x_r,
const TDCPParams& sd_params,
const prcopt_t& opt);
} // namespace GNSSUtils
#endif /* INCLUDE_GNSS_UTILS_TDCP_H_ */
#ifndef GNSS_UTILS_H
#define GNSS_UTILS_H
#include <vector>
#include <iostream>
#include <memory>
......@@ -14,62 +13,92 @@
#include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h"
extern "C"
{
#include "rtklib.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);
Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
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);
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);
Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU,
Eigen::Matrix3d& R_ENU_ECEF,
Eigen::Vector3d& t_ENU_ECEF);
void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt,
Eigen::Matrix3d& R_ENU_ECEF,
Eigen::Vector3d& t_ENU_ECEF);
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
#endif
......@@ -5,364 +5,361 @@
#include <iostream>
#include <memory>
extern "C"
{
#include "rtklib.h"
extern "C" {
#include "rtklib.h"
}
namespace GNSSUtils
{
class Navigation;
typedef std::shared_ptr<Navigation> NavigationPtr;
typedef std::shared_ptr<Navigation> NavigationPtr;
typedef std::shared_ptr<const Navigation> NavigationConstPtr;
class Navigation
{
public:
// Constructor & Destructor
Navigation();
Navigation(const Navigation& nav);
~Navigation();
// Public objects
// Public methods
void setNavigation(nav_t nav);
void clearNavigation();
void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
const nav_t & getNavigation() const;
nav_t & getNavigation();
void uniqueNavigation(); //remove duplicated ephemerides and update carrier phase wave lengths
/****************** Array memory management ******************/
bool addEphemeris(const eph_t &eph);
bool addGlonassEphemeris(const geph_t& geph);
bool addSbasEphemeris(const seph_t& seph);
bool addPreciseEphemeris(const peph_t& peph);
bool addPreciseClock(const pclk_t& pclk);
bool addAlmanac(const alm_t& alm);
bool addTec(const tec_t& tec);
bool addFcb(const fcbd_t& fcb);
void addSbasMessage(const sbsmsg_t &sbas_msg);
void copyAllArrays(const nav_t& nav);
void copyEphemeris(const nav_t& nav);
void copyAlmanac(const nav_t& nav);
void copyIonUtc(const nav_t &nav);
void copySbasCorrections(const nav_t &nav);
void freeNavigationArrays();
void freeEphemeris();
void freeGlonassEphemeris();
void freeSbasEphemeris();
void freePreciseEphemeris();
void freeAlmanac();
void freePreciseClock();
void freeTecData();
void freeFcbData();
void print();
//////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T>
static bool addToArray(const T &new_element, T *&array, int &n, int &nmax);
template <typename T>
static bool copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out);
template <typename T>
static void freeArray(T *&array, int &n, int &nmax);
static void freeEph(nav_t &nav);
static void freeGeph(nav_t &nav);
static void freeSeph(nav_t &nav);
static void freePeph(nav_t &nav);
static void freeAlm(nav_t &nav);
static void freePclk(nav_t &nav);
static void freeTec(nav_t &nav);
static void freeFcb(nav_t &nav);
static void freeNavArrays(nav_t &nav);
private:
// rtklib-like attribute to represent the different navigation msgs for a given epoch
nav_t nav_;
// Private methods
public:
// Constructor & Destructor
Navigation();
Navigation(const Navigation& nav);
~Navigation();
// Public objects
// Public methods
void setNavigation(nav_t nav);
void clearNavigation();
void
loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
const nav_t& getNavigation() const;
nav_t& getNavigation();
void uniqueNavigation(); // remove duplicated ephemerides and update carrier phase wave lengths
/****************** Array memory management ******************/
bool addEphemeris(const eph_t& eph);
bool addGlonassEphemeris(const geph_t& geph);
bool addSbasEphemeris(const seph_t& seph);
bool addPreciseEphemeris(const peph_t& peph);
bool addPreciseClock(const pclk_t& pclk);
bool addAlmanac(const alm_t& alm);
bool addTec(const tec_t& tec);
bool addFcb(const fcbd_t& fcb);
void addSbasMessage(const sbsmsg_t& sbas_msg);
void copyAllArrays(const nav_t& nav);
void copyEphemeris(const nav_t& nav);
void copyAlmanac(const nav_t& nav);
void copyIonUtc(const nav_t& nav);
void copySbasCorrections(const nav_t& nav);
void freeNavigationArrays();
void freeEphemeris();
void freeGlonassEphemeris();
void freeSbasEphemeris();
void freePreciseEphemeris();
void freeAlmanac();
void freePreciseClock();
void freeTecData();
void freeFcbData();
void print();
//////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T>
static bool addToArray(const T& new_element, T*& array, int& n, int& nmax);
template <typename T>
static bool copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out);
template <typename T>
static void freeArray(T*& array, int& n, int& nmax);
static void freeEph(nav_t& nav);
static void freeGeph(nav_t& nav);
static void freeSeph(nav_t& nav);
static void freePeph(nav_t& nav);
static void freeAlm(nav_t& nav);
static void freePclk(nav_t& nav);
static void freeTec(nav_t& nav);
static void freeFcb(nav_t& nav);
static void freeNavArrays(nav_t& nav);
private:
// rtklib-like attribute to represent the different navigation msgs for a given epoch
nav_t nav_;
// Private methods
};
inline void Navigation::uniqueNavigation() //remove duplicated ephemerides and update wave lengths
inline void Navigation::uniqueNavigation() // remove duplicated ephemerides and update wave lengths
{
uniqnav(&nav_);
uniqnav(&nav_);
}
inline const nav_t & Navigation::getNavigation() const
inline const nav_t& Navigation::getNavigation() const
{
return nav_;
}
inline nav_t & Navigation::getNavigation()
inline nav_t& Navigation::getNavigation()
{
return nav_;
}
inline bool Navigation::addEphemeris(const eph_t &eph)
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;
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;
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;
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)
{
return addToArray<peph_t>(peph, nav_.peph, nav_.ne, nav_.nemax);
return addToArray<peph_t>(peph, nav_.peph, nav_.ne, nav_.nemax);
}
inline bool Navigation::addPreciseClock(const pclk_t& pclk)
{
return addToArray<pclk_t>(pclk, nav_.pclk, nav_.nc, nav_.ncmax);
return addToArray<pclk_t>(pclk, nav_.pclk, nav_.nc, nav_.ncmax);
}
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;
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)
{
return addToArray<tec_t>(tec, nav_.tec, nav_.nt, nav_.ntmax);
return addToArray<tec_t>(tec, nav_.tec, nav_.nt, nav_.ntmax);
}
inline bool Navigation::addFcb(const fcbd_t& fcb)
{
return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
}
inline void Navigation::addSbasMessage(const sbsmsg_t &sbas_msg)
inline void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
{
sbsupdatecorr(&sbas_msg, &nav_);
sbsupdatecorr(&sbas_msg, &nav_);
}
inline void Navigation::freeEphemeris()
{
freeEph(nav_);
freeEph(nav_);
}
inline void Navigation::freeGlonassEphemeris()
{
freeGeph(nav_);
freeGeph(nav_);
}
inline void Navigation::freePreciseEphemeris()
{
freePeph(nav_);
freePeph(nav_);
}
inline void Navigation::freeSbasEphemeris()
{
freeSeph(nav_);
freeSeph(nav_);
}
inline void Navigation::freePreciseClock()
{
freePclk(nav_);
freePclk(nav_);
}
inline void Navigation::freeTecData()
{
freeTec(nav_);
freeTec(nav_);
}
inline void Navigation::freeFcbData()
{
freeFcb(nav_);
freeFcb(nav_);
}
inline void Navigation::freeAlmanac()
{
freeAlm(nav_);
freeAlm(nav_);
}
//////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T>
inline bool Navigation::addToArray(const T &new_element, T *&array, int &n, int &nmax)
{
//std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
// "inspired" from RTKLIB rinex.c
T *array_ref;
if (nmax <= n)
inline bool Navigation::addToArray(const T& new_element, T*& array, int& n, int& nmax)
{
// std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
// "inspired" from RTKLIB rinex.c
T* array_ref;
if (nmax <= n)
{
// std::cout << "addToArray: nmax <= n\n";
nmax += 1024;
if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
{
//std::cout << "addToArray: nmax <= n\n";
nmax += 1024;
if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
{
printf("addToArray malloc error: n=%d\n", nmax);
free(array);
array = NULL;
n = nmax = 0;
return false;
}
//std::cout << "addToArray: assigning reallocated array\n";
array = array_ref;
printf("addToArray malloc error: n=%d\n", nmax);
free(array);
array = NULL;
n = nmax = 0;
return false;
}
//std::cout << "addToArray: adding element " << n << "\n";
array[n++] = new_element;
//std::cout << "addToArray: added!\n";
return true;
// std::cout << "addToArray: assigning reallocated array\n";
array = array_ref;
}
// std::cout << "addToArray: adding element " << n << "\n";
array[n++] = new_element;
// std::cout << "addToArray: added!\n";
return true;
}
template<typename T>
inline bool Navigation::copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out)
template <typename T>
inline bool Navigation::copyArray(const T* array_in, const int& n_in, T*& array_out, int& n_out, int& nmax_out)
{
//std::cout << "copyArray: " << n_in << " elements\n";
if (array_in == NULL)
return false;
// std::cout << "copyArray: " << n_in << " elements\n";
if (array_in == NULL)
return false;
//std::cout << "copyArray: array in not null\n";
// std::cout << "copyArray: array in not null\n";
for (int i = 0; i<n_in; i++)
for (int i = 0; i < n_in; i++)
{
// std::cout << "copyArray: adding element " << i << "\n";
if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
{
//std::cout << "copyArray: adding element " << i << "\n";
if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
{
//std::cout << "copyArray: failed to add..\n";
return false;
}
//std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
// std::cout << "copyArray: failed to add..\n";
return false;
}
// std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
}
//std::cout << "copyArray: all copied\n";
// std::cout << "copyArray: all copied\n";
return true;
return true;
}
template<typename T>
void Navigation::freeArray(T *&array, int &n, int &nmax)
template <typename T>
void Navigation::freeArray(T*& array, int& n, int& nmax)
{
if (array != NULL)
free(array);
array = NULL;
n = nmax = 0;
if (array != NULL)
free(array);
array = NULL;
n = nmax = 0;
}
inline void Navigation::freeEph(nav_t &nav)
inline void Navigation::freeEph(nav_t& nav)
{
freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
freeArray<eph_t>(nav.eph, nav.n, nav.nmax);
}
inline void Navigation::freeGeph(nav_t &nav)
inline void Navigation::freeGeph(nav_t& nav)
{
freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
freeArray<geph_t>(nav.geph, nav.ng, nav.ngmax);
}
inline void Navigation::freeSeph(nav_t &nav)
inline void Navigation::freeSeph(nav_t& nav)
{
freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
freeArray<seph_t>(nav.seph, nav.ns, nav.nsmax);
}
inline void Navigation::freePeph(nav_t &nav)
inline void Navigation::freePeph(nav_t& nav)
{
freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
freeArray<peph_t>(nav.peph, nav.ne, nav.nemax);
}
inline void Navigation::freeAlm(nav_t &nav)
inline void Navigation::freeAlm(nav_t& nav)
{
freeArray<alm_t>(nav.alm, nav.na, nav.namax);
freeArray<alm_t>(nav.alm, nav.na, nav.namax);
}
inline void Navigation::freePclk(nav_t &nav)
inline void Navigation::freePclk(nav_t& nav)
{
freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
freeArray<pclk_t>(nav.pclk, nav.nc, nav.ncmax);
}
inline void Navigation::freeTec(nav_t &nav)
inline void Navigation::freeTec(nav_t& nav)
{
freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
freeArray<tec_t>(nav.tec, nav.nt, nav.ntmax);
}
inline void Navigation::freeFcb(nav_t &nav)
inline void Navigation::freeFcb(nav_t& nav)
{
freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
freeArray<fcbd_t>(nav.fcb, nav.nf, nav.nfmax);
}
inline void Navigation::freeNavArrays(nav_t &nav)
inline void Navigation::freeNavArrays(nav_t& nav)
{
// RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
freeEph(nav);
freeGeph(nav);
freeSeph(nav);
freePeph(nav);
freePclk(nav);
freeAlm(nav);
freeTec(nav);
freeFcb(nav);
// RTKLIB "freenav(&nav_,255)" doesn't check if is NULL before freeing
freeEph(nav);
freeGeph(nav);
freeSeph(nav);
freePeph(nav);
freePclk(nav);
freeAlm(nav);
freeTec(nav);
freeFcb(nav);
}
}
} // namespace GNSSUtils
#endif
......@@ -9,127 +9,127 @@
#include "gnss_utils/utils.h"
extern "C"
{
#include "rtklib.h"
extern "C" {
#include "rtklib.h"
}
namespace GNSSUtils
{
class Observations;
typedef std::shared_ptr<Observations> ObservationsPtr;
typedef std::shared_ptr<Observations> ObservationsPtr;
typedef std::shared_ptr<const Observations> ObservationsConstPtr;
class Observations
{
public:
// Constructor & Destructor
Observations();
Observations(const Observations& obs);
~Observations();
public:
// Constructor & Destructor
Observations();
Observations(const Observations& obs);
~Observations();
// Public objects
// Public objects
// Public methods
// Public methods
/* - Observations - */
/* - Observations - */
void clearObservations();
void clearObservations();
void addObservation(const obsd_t& obs);
void
loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt = 0.0, const char* opt = "");
void addObservation(const obsd_t & obs);
void loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt=0.0, const char* opt="");
void removeObservationByIdx(const int& _idx);
void removeObservationBySat(const int& _sat);
void removeObservationByIdx(const int& _idx);
void removeObservationBySat(const int& _sat);
std::vector<obsd_t>& getObservations();
const std::vector<obsd_t>& getObservations() const;
std::vector<obsd_t>& getObservations();
const std::vector<obsd_t> & getObservations() const;
obsd_t& getObservationBySat(const unsigned char& sat_number);
const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
obsd_t& getObservationBySat(const unsigned char& sat_number);
const obsd_t& getObservationBySat(const unsigned char& sat_number) const;
obsd_t& getObservationByIdx(const int& idx);
const obsd_t& getObservationByIdx(const int& idx) const;
obsd_t& getObservationByIdx(const int& idx);
const obsd_t& getObservationByIdx(const int& idx) const;
obsd_t* data();
const obsd_t* data() const;
obsd_t* data();
const obsd_t* data() const;
size_t size() const;
size_t size() const;
bool hasSatellite(const unsigned char& i) const;
bool hasSatellite(const unsigned char& i) const;
void print(obsd_t& _obs);
void printBySat(const int& _sat);
void printByIdx(const int& _idx);
void print();
void print(obsd_t & _obs);
void printBySat(const int& _sat);
void printByIdx(const int& _idx);
void print();
static void findCommonObservations(const Observations& obs_1,
const Observations& obs_2,
Observations& common_obs_1,
Observations& common_obs_2);
static void findCommonObservations(const Observations& obs_1, const Observations& obs_2,
Observations& common_obs_1, Observations& common_obs_2);
private:
// Private objects
std::map<unsigned char, int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector
std::vector<unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
private:
// Private objects
std::map<unsigned char,int> sat_2_idx_; //< key: corresponding sat number, value: idx in obs_ vector
std::vector<unsigned char> idx_2_sat_; //< key: idx in obs_ vector, value: corresponding sat number
std::vector<obsd_t> obs_; //< vector of RTKLIB observations for a given epoch
// Private methods
// Private methods
};
inline const std::vector<obsd_t>& Observations::getObservations() const
{
return this->obs_;
return this->obs_;
}
inline std::vector<obsd_t>& Observations::getObservations()
{
return this->obs_;
return this->obs_;
}
inline obsd_t& Observations::getObservationBySat(const unsigned char& sat_number)
{
assert(sat_2_idx_.count(sat_number) != 0);
return obs_.at(sat_2_idx_.at(sat_number));
assert(sat_2_idx_.count(sat_number) != 0);
return obs_.at(sat_2_idx_.at(sat_number));
}
inline const obsd_t& Observations::getObservationBySat(const unsigned char& sat_number) const
{
assert(sat_2_idx_.count(sat_number) != 0);
return obs_.at(sat_2_idx_.at(sat_number));
assert(sat_2_idx_.count(sat_number) != 0);
return obs_.at(sat_2_idx_.at(sat_number));
}
inline obsd_t& Observations::getObservationByIdx(const int& idx)
{
assert(obs_.size() > idx);
return obs_.at(idx);
assert(obs_.size() > idx);
return obs_.at(idx);
}
inline const obsd_t& Observations::getObservationByIdx(const int& idx) const
{
assert(obs_.size() > idx);
return obs_.at(idx);
assert(obs_.size() > idx);
return obs_.at(idx);
}
inline obsd_t* Observations::data()
{
return obs_.data();
return obs_.data();
}
inline const obsd_t* Observations::data() const
{
return obs_.data();
return obs_.data();
}
inline size_t Observations::size() const
{
return obs_.size();
return obs_.size();
}
inline bool Observations::hasSatellite(const unsigned char& i) const
{
return sat_2_idx_.count(i) != 0;
return sat_2_idx_.count(i) != 0;
}
}
} // namespace GNSSUtils
#endif
......@@ -5,12 +5,23 @@
namespace GNSSUtils
{
enum RawDataType : int {NO = 0, OBS = 1, NAV_EPH = 2, NAV_SBAS = 3, NAV_ALM = 9, NAV_ANT = 5, NAV_DGPS = 7, NAV_SSR = 10, NAV_LEX = 31, ERROR = -1};
enum RawDataType : int
{
NO = 0,
OBS = 1,
NAV_EPH = 2,
NAV_SBAS = 3,
NAV_ALM = 9,
NAV_ANT = 5,
NAV_DGPS = 7,
NAV_SSR = 10,
NAV_LEX = 31,
ERROR = -1
};
class UBloxRaw
{
public:
public:
UBloxRaw();
~UBloxRaw();
......@@ -21,26 +32,25 @@ class UBloxRaw
RawDataType getRawDataType() const;
private:
private:
raw_t raw_data_;
Observations obs_;
Navigation nav_;
Navigation nav_;
RawDataType raw_data_type_;
void updateObservations();
void updateObservations();
};
inline const GNSSUtils::Observations& UBloxRaw::getObservations()
{
return obs_;
return obs_;
}
inline const GNSSUtils::Navigation& UBloxRaw::getNavigation()
{
return nav_;
return nav_;
}
inline RawDataType UBloxRaw::getRawDataType() const
......@@ -48,7 +58,6 @@ inline RawDataType UBloxRaw::getRawDataType() const
return raw_data_type_;
}
}
} // namespace GNSSUtils
#endif
......@@ -6,18 +6,16 @@
#include <memory>
#include <string>
#define ARRAY_SIZE(arr) sizeof(arr)/sizeof(arr[0])
#define ARRAY_SIZE(arr) sizeof(arr) / sizeof(arr[0])
#define GNSSUTILS_MSG "--GNSSUtils--"
namespace GNSSUtils
{
void print(std::string & _msg);
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);
}
void print(std::string& _msg);
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);
} // namespace GNSSUtils
#endif
\ No newline at end of file
This diff is collapsed.
......@@ -8,39 +8,43 @@
#include <iostream>
extern "C"
{
#include "../deps/RTKLIB/src/rinex.c"
extern "C" {
#include "../deps/RTKLIB/src/rinex.c"
}
using namespace GNSSUtils;
int main(int argc, char *argv[])
int main(int argc, char* argv[])
{
/* header */
std::cout << "--------------------------" << std::endl;
std::cout << "GNSS Utils Library Example" << std::endl;
std::cout << "--------------------------" << std::endl;
// create Observations object
Observations observations;
// create Navigation object
Navigation navigation;
gtime_t t_start{ 0, 0 }; // no limit
gtime_t t_end{ 0, 0 }; // no limit
double dt = 0.0; // no limit
const char* opt = ""; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
gtime_t t_start{0,0}; // no limit
gtime_t t_end{0,0}; // no limit
double dt = 0.0; // no limit
const char* opt = "-SYS=G"; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
// RTKLIB trace
// char str_file[80];
// snprintf(str_file, sizeof str_file, "../src/examples/trace");
// load observations from RINEX file
observations.loadFromRinex("../src/examples/raw_201805171357.obs", t_start, t_end, dt, opt);
observations.loadFromRinex("../src/examples/sample_data.obs", t_start, t_end, dt, opt);
observations.print();
// Trace close
// traceclose();
// load navigation from RINEX file
navigation.loadFromRinex("../src/examples/raw_201805171357.nav", t_start, t_end, dt, opt);
navigation.loadFromRinex("../src/examples/sample_data.nav", t_start, t_end, dt, opt);
navigation.print();
/* Set processing options */
......@@ -51,26 +55,26 @@ int main(int argc, char *argv[])
std::cout << "------------------" << std::endl;
prcopt_t prcopt = prcopt_default;
prcopt.mode = PMODE_SINGLE;
prcopt.soltype = 0;
prcopt.nf = 1;
prcopt.navsys = SYS_GPS;
prcopt.elmin = 0.525; // 60 degrees = 1.05 rad
prcopt.sateph = EPHOPT_BRDC;
prcopt.ionoopt = IONOOPT_OFF;
prcopt.tropopt = TROPOPT_OFF;
prcopt.mode = PMODE_SINGLE;
prcopt.soltype = 0;
prcopt.nf = 1;
prcopt.navsys = SYS_GPS;
prcopt.elmin = 0.525; // 60 degrees = 1.05 rad
prcopt.sateph = EPHOPT_BRDC;
prcopt.ionoopt = IONOOPT_OFF;
prcopt.tropopt = TROPOPT_OFF;
prcopt.dynamics = 0;
prcopt.tidecorr = 0;
prcopt.sbascorr = SBSOPT_FCORR;
prcopt.ru[0] = 0;//4789374.0336;
prcopt.ru[1] = 0;//177048.3292;
prcopt.ru[2] = 0;//4194542.6444;
prcopt.ru[0] = 0; // 4789374.0336;
prcopt.ru[1] = 0; // 177048.3292;
prcopt.ru[2] = 0; // 4194542.6444;
std::cout << "Processing options defined" << std::endl;
Eigen::Vector3d lla_gt(41.383293114*M_PI/180.0, 2.116101115*M_PI/180.0, -91.6641);
Eigen::Vector3d lla_gt(41.383293114 * M_PI / 180.0, 2.116101115 * M_PI / 180.0, -91.6641);
//Compute spp
// Compute spp
/* header */
std::cout << "-----------" << std::endl;
......@@ -79,9 +83,9 @@ int main(int argc, char *argv[])
int stat;
sol_t solb={{0}};
sol_t solb = { { 0 } };
char msg[128]="";
char msg[128] = "";
stat = pntpos(observations.data(), observations.size(), &navigation.getNavigation(), &prcopt, &solb, NULL, NULL, msg);
......@@ -89,6 +93,9 @@ int main(int argc, char *argv[])
std::cout << "sol.stat: " << int(solb.stat) << std::endl;
std::cout << "Position: " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl;
std::cout << "Position (GT): " << latLonAltToEcef(lla_gt).transpose() << std::endl;
std::cout << "Position LLA: " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0],solb.rr[1],solb.rr[2])).transpose() << std::endl;
std::cout << "Position LLA: " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0], solb.rr[1], solb.rr[2])).transpose()
<< std::endl;
std::cout << "Position LLA (GT): " << lla_gt.transpose() << std::endl;
traceclose();
}
This diff is collapsed.
3.04 OBSERVATION DATA M RINEX VERSION / TYPE
sbf2rin-13.4.3 20200205 113408 UTC PGM / RUN BY / DATE
SEPT MARKER NAME
Unknown MARKER NUMBER
Unknown Unknown OBSERVER / AGENCY
3021420 SEPT ASTERX-M2 4.4.0 REC # / TYPE / VERS
Unknown Unknown ANT # / TYPE
4789398.3686 176958.8129 4194502.0999 APPROX POSITION XYZ
0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N
G 7 X1 C1C L1C C2W L2W C2L L2L SYS / # / OBS TYPES
E 7 X1 C1C L1C C5Q L5Q C7Q L7Q SYS / # / OBS TYPES
S 3 X1 C1C L1C SYS / # / OBS TYPES
R 5 X1 C1C L1C C2C L2C SYS / # / OBS TYPES
C 5 X1 C2I L2I C7I L7I SYS / # / OBS TYPES
SEPTENTRIO RECEIVERS OUTPUT ALIGNED CARRIER PHASES. COMMENT
NO FURTHER PHASE SHIFT APPLIED IN THE RINEX ENCODER. COMMENT
G L1C SYS / PHASE SHIFT
G L2W SYS / PHASE SHIFT
G L2L 0.00000 SYS / PHASE SHIFT
E L1C 0.00000 SYS / PHASE SHIFT
E L5Q 0.00000 SYS / PHASE SHIFT
E L7Q 0.00000 SYS / PHASE SHIFT
S L1C SYS / PHASE SHIFT
R L1C SYS / PHASE SHIFT
R L2C SYS / PHASE SHIFT
C L2I SYS / PHASE SHIFT
C L7I SYS / PHASE SHIFT
0.050 INTERVAL
2019 12 12 11 37 42.0000000 GPS TIME OF FIRST OBS
2019 12 12 11 45 12.9000000 GPS TIME OF LAST OBS
36 # OF SATELLITES
C1C 0.000 C2C 0.000 GLONASS COD/PHS/BIS
8 R03 5 R05 1 R13 -2 R14 -7 R15 0 R19 3 R20 2 R21 4 GLONASS SLOT / FRQ #
END OF HEADER
> 2019 12 12 11 37 42.0000000 0 23
C05 16.000 40138840.659 6 209013478.40606 40138832.256 6 161622441.87406
C10 17.000 39633104.355 5 206379985.90205 39633100.852 6 159586077.69206
C23 18.000 22865676.301 8 119067585.90908
C32 19.000 22153454.599 8 115358921.19508
C37 20.000 23690566.611 7 123363010.77907
E01 11.000 26311074.676 7 138265636.53007 26311074.470 5 103250320.19005 26311070.977 7 105943790.57907
E09 12.000 25723493.482 5
E12 13.000 24803095.627 5 130341141.55605 24803092.768 4 97332661.71504 24803092.407 6 99871767.93506
E19 14.000 24540665.652 6 128962068.93606 24540664.837 5 96302837.53105 24540661.694 6 98815075.63606
E21 15.000 25609802.251 7 134580416.73507 25609802.381 7 100498367.38007 25609799.519 7 103120055.01507
G02 1.000 22032430.127 5
G06 2.000 22147435.410 6 116385570.45206 22147437.095 4 90690045.24704 22147437.422 7 90690046.16907
G07 3.000 20902411.481 8 109842911.60108 20902406.002 6 85591857.15006 20902406.930 7 85591857.16507
G09 4.000 21908403.525 6 115129430.37406 21908399.544 1 89711233.32401 21908399.320 5
G23 5.000 24104714.314 6 126671129.49306 24104706.816 2 98704739.67602
G30 6.000 21404145.095 8 112479542.93308 21404142.953 6 87646388.26706 21404143.916 7 87646383.27307
R03 7.000 21945110.617 7 117474090.63807 21945109.720 6 91368580.68606
R14 8.000 20214975.526 8 107757316.39008 20214976.579 7 83811176.41707
R15 9.000 22699008.378 5 121296675.96005 22699009.712 5 94341922.34705
R19 10.000 22853592.745 4 22853590.888 4
S23 21.000 38309228.895 7 201316353.13407
S25 22.000 37834172.957 6 198818604.39206
S36 23.000 37630702.258 7 197750698.01307
This diff is collapsed.
This diff is collapsed.
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
Observations::Observations()
{
}
Observations::Observations(const Observations& obs)
: sat_2_idx_(obs.sat_2_idx_)
, idx_2_sat_(obs.idx_2_sat_)
, obs_(obs.obs_.size())
: sat_2_idx_(obs.sat_2_idx_), idx_2_sat_(obs.idx_2_sat_), obs_(obs.obs_.size())
{
// copy all elements
for (auto i = 0; i<obs.obs_.size(); i++)
{
obs_[i].time = obs.obs_[i].time;
obs_[i].eventime = obs.obs_[i].eventime;
obs_[i].timevalid = obs.obs_[i].timevalid;
obs_[i].sat = obs.obs_[i].sat;
obs_[i].rcv = obs.obs_[i].rcv;
std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ+NEXOBS, obs_[i].SNR);
std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ+NEXOBS, obs_[i].LLI);
std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ+NEXOBS, obs_[i].code);
std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ+NEXOBS, obs_[i].qualL);
std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ+NEXOBS, obs_[i].qualP);
obs_[i].freq = obs.obs_[i].freq;
std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ+NEXOBS, obs_[i].L);
std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ+NEXOBS, obs_[i].P);
std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ+NEXOBS, obs_[i].D);
}
// copy all elements
for (auto i = 0; i < obs.obs_.size(); i++)
{
obs_[i].time = obs.obs_[i].time;
obs_[i].eventime = obs.obs_[i].eventime;
obs_[i].timevalid = obs.obs_[i].timevalid;
obs_[i].sat = obs.obs_[i].sat;
obs_[i].rcv = obs.obs_[i].rcv;
std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ + NEXOBS, obs_[i].SNR);
std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ + NEXOBS, obs_[i].LLI);
std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ + NEXOBS, obs_[i].code);
std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ + NEXOBS, obs_[i].qualL);
std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ + NEXOBS, obs_[i].qualP);
obs_[i].freq = obs.obs_[i].freq;
std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ + NEXOBS, obs_[i].L);
std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ + NEXOBS, obs_[i].P);
std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ + NEXOBS, obs_[i].D);
}
}
Observations::~Observations()
......@@ -46,116 +41,123 @@ void Observations::clearObservations()
this->sat_2_idx_.clear();
}
void Observations::addObservation(const obsd_t & obs)
void Observations::addObservation(const obsd_t& obs)
{
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
// copy obsd_t object
obsd_t copy_obs;
copy_obs.time = obs.time;
copy_obs.eventime = obs.eventime;
copy_obs.timevalid = obs.timevalid;
copy_obs.sat = obs.sat;
copy_obs.rcv = obs.rcv;
std::copy(obs.SNR, obs.SNR + NFREQ+NEXOBS, copy_obs.SNR);
std::copy(obs.LLI, obs.LLI + NFREQ+NEXOBS, copy_obs.LLI);
std::copy(obs.code, obs.code + NFREQ+NEXOBS, copy_obs.code);
std::copy(obs.qualL, obs.qualL + NFREQ+NEXOBS, copy_obs.qualL);
std::copy(obs.qualP, obs.qualP + NFREQ+NEXOBS, copy_obs.qualP);
copy_obs.freq = obs.freq;
std::copy(obs.L, obs.L + NFREQ+NEXOBS, copy_obs.L);
std::copy(obs.P, obs.P + NFREQ+NEXOBS, copy_obs.P);
std::copy(obs.D, obs.D + NFREQ+NEXOBS, copy_obs.D);
assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
this->obs_.push_back(copy_obs);
idx_2_sat_.push_back(copy_obs.sat);
sat_2_idx_[copy_obs.sat] = obs_.size()-1;
//std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
// copy obsd_t object
obsd_t copy_obs;
copy_obs.time = obs.time;
copy_obs.eventime = obs.eventime;
copy_obs.timevalid = obs.timevalid;
copy_obs.sat = obs.sat;
copy_obs.rcv = obs.rcv;
std::copy(obs.SNR, obs.SNR + NFREQ + NEXOBS, copy_obs.SNR);
std::copy(obs.LLI, obs.LLI + NFREQ + NEXOBS, copy_obs.LLI);
std::copy(obs.code, obs.code + NFREQ + NEXOBS, copy_obs.code);
std::copy(obs.qualL, obs.qualL + NFREQ + NEXOBS, copy_obs.qualL);
std::copy(obs.qualP, obs.qualP + NFREQ + NEXOBS, copy_obs.qualP);
copy_obs.freq = obs.freq;
std::copy(obs.L, obs.L + NFREQ + NEXOBS, copy_obs.L);
std::copy(obs.P, obs.P + NFREQ + NEXOBS, copy_obs.P);
std::copy(obs.D, obs.D + NFREQ + NEXOBS, copy_obs.D);
assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
this->obs_.push_back(copy_obs);
idx_2_sat_.push_back(copy_obs.sat);
sat_2_idx_[copy_obs.sat] = obs_.size() - 1;
// std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
void Observations::loadFromRinex(const std::string& rnx_file,
gtime_t t_start,
gtime_t t_end,
double dt,
const char* opt)
{
obs_t obs;
obs.data = (obsd_t *) malloc(sizeof(obsd_t)*MAXSAT);
obs.n = 0;
obs.nmax = MAXSAT;
//const char* opt = "";
auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
if (stat == 1)
sortobs(&obs);
else
{
std::cout << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
return;
}
obs_t obs;
obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
obs.n = 0;
obs.nmax = MAXSAT;
// const char* opt = "";
auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
if (stat == 1)
sortobs(&obs);
else
{
std::cout << "Observation: couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << stat << std::endl;
return;
}
for (int i=0; i < obs.n; i++)
{
// std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " << int(obs.data[i].rcv) <<
// " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " << int(obs.data[i].code[0]) <<
// " | L: " << obs.data[i].L[0] << " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] << std::endl;
addObservation(obs.data[i]);
}
for (int i = 0; i < obs.n; i++)
{
// std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " <<
// int(obs.data[i].rcv) <<
// " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " <<
// int(obs.data[i].code[0]) <<
// " | L: " << obs.data[i].L[0] << " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] <<
// std::endl;
addObservation(obs.data[i]);
}
free(obs.data);
free(obs.data);
}
void Observations::removeObservationByIdx(const int& _idx)
{
//std::cout << "removing observation of idx " << _idx << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(obs_.size() > _idx);
assert(idx_2_sat_.size() > _idx);
assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
// remove obs from sat_2_idx map
sat_2_idx_.erase(idx_2_sat_.at(_idx));
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > _idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + _idx);
idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
// std::cout << "removing observation of idx " << _idx << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(obs_.size() > _idx);
assert(idx_2_sat_.size() > _idx);
assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
// remove obs from sat_2_idx map
sat_2_idx_.erase(idx_2_sat_.at(_idx));
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > _idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + _idx);
idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::removeObservationBySat(const int& _sat)
{
//std::cout << "removing observation of sat " << _sat << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(sat_2_idx_.count(_sat) != 0);
assert(hasSatellite(_sat));
assert(obs_.size() > sat_2_idx_.at(_sat));
assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
int idx = sat_2_idx_.at(_sat);
// remove obs from sat_2_idx map
sat_2_idx_.erase(_sat);
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + idx);
idx_2_sat_.erase(idx_2_sat_.begin() + idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
// std::cout << "removing observation of sat " << _sat << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(sat_2_idx_.count(_sat) != 0);
assert(hasSatellite(_sat));
assert(obs_.size() > sat_2_idx_.at(_sat));
assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
int idx = sat_2_idx_.at(_sat);
// remove obs from sat_2_idx map
sat_2_idx_.erase(_sat);
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + idx);
idx_2_sat_.erase(idx_2_sat_.begin() + idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::print(obsd_t & _obs)
void Observations::print(obsd_t& _obs)
{
std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
GNSSUtils::print(msg);
......@@ -186,33 +188,34 @@ void Observations::print()
}
}
void Observations::findCommonObservations(const Observations& obs_1, const Observations& obs_2,
Observations& common_obs_1, Observations& common_obs_2)
void Observations::findCommonObservations(const Observations& obs_1,
const Observations& obs_2,
Observations& common_obs_1,
Observations& common_obs_2)
{
// clear and reserve
common_obs_1.clearObservations();
common_obs_2.clearObservations();
//std::cout << "obs 1 sats: ";
//for (auto&& obs_1_ref : obs_1.getObservations())
// std::cout << (int)obs_1_ref.sat << " ";
//std::cout << std::endl;
//std::cout << "obs 2 sats: ";
//for (auto&& obs_2_ref : obs_2.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
//std::cout << std::endl;
// iterate 1
for (auto&& obs_1_ref : obs_1.getObservations())
if (obs_2.hasSatellite(obs_1_ref.sat))
{
common_obs_1.addObservation(obs_1_ref);
common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
}
//std::cout << "common sats: ";
//for (auto&& obs_2_ref : common_obs_1.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
//std::cout << std::endl;
// clear and reserve
common_obs_1.clearObservations();
common_obs_2.clearObservations();
// std::cout << "obs 1 sats: ";
// for (auto&& obs_1_ref : obs_1.getObservations())
// std::cout << (int)obs_1_ref.sat << " ";
// std::cout << std::endl;
// std::cout << "obs 2 sats: ";
// for (auto&& obs_2_ref : obs_2.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
// std::cout << std::endl;
// iterate 1
for (auto&& obs_1_ref : obs_1.getObservations())
if (obs_2.hasSatellite(obs_1_ref.sat))
{
common_obs_1.addObservation(obs_1_ref);
common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
}
// std::cout << "common sats: ";
// for (auto&& obs_2_ref : common_obs_1.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
// std::cout << std::endl;
}
......@@ -2,98 +2,97 @@
using namespace GNSSUtils;
UBloxRaw::UBloxRaw() :
raw_data_type_(NO)
UBloxRaw::UBloxRaw() : raw_data_type_(NO)
{
if (init_raw(&raw_data_, STRFMT_UBX) == 0)
{
assert("Failed when allocating memory for raw_t");
return;
}
if (init_raw(&raw_data_, STRFMT_UBX) == 0)
{
assert("Failed when allocating memory for raw_t");
return;
}
};
UBloxRaw::~UBloxRaw()
{
free_raw(&raw_data_);
free_raw(&raw_data_);
};
RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
{
// Update type based on RTKLIB
for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
{
raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
// Update type based on RTKLIB
for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
{
raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
switch (raw_data_type_)
{
case NO: //
//std::cout << "0 received!\n";
break;
switch (raw_data_type_)
{
case NO: //
// std::cout << "0 received!\n";
break;
case OBS: // Observations
std::cout << "Observations received!\n";
updateObservations();
//std::cout << "Observations updated!\n";
break;
case OBS: // Observations
// std::cout << "Observations received!\n";
updateObservations();
// std::cout << "Observations updated!\n";
break;
case NAV_EPH: // Ephemeris
std::cout << "Ephemeris received!\n";
nav_.copyEphemeris(raw_data_.nav);
//std::cout << "Ephemeris copied!\n";
break;
case NAV_EPH: // Ephemeris
std::cout << "Ephemeris received!\n";
nav_.copyEphemeris(raw_data_.nav);
// std::cout << "Ephemeris copied!\n";
break;
case NAV_SBAS: // SBAS
std::cout << "SBAS received!\n";
nav_.addSbasMessage(raw_data_.sbsmsg);
//std::cout << "SBAS added!\n";
break;
case NAV_SBAS: // SBAS
std::cout << "SBAS received!\n";
nav_.addSbasMessage(raw_data_.sbsmsg);
// std::cout << "SBAS added!\n";
break;
case NAV_ALM: // Almanac and ion/utc parameters
std::cout << "Almanac and ion/utc parameters received!\n";
nav_.freeAlmanac();
//std::cout << "Almanac freed!\n";
nav_.copyAlmanac(raw_data_.nav);
//std::cout << "Almanac copied!\n";
nav_.copyIonUtc(raw_data_.nav);
//std::cout << "ION UTC copied!\n";
break;
case NAV_ALM: // Almanac and ion/utc parameters
std::cout << "Almanac and ion/utc parameters received!\n";
nav_.freeAlmanac();
// std::cout << "Almanac freed!\n";
nav_.copyAlmanac(raw_data_.nav);
// std::cout << "Almanac copied!\n";
nav_.copyIonUtc(raw_data_.nav);
// std::cout << "ION UTC copied!\n";
break;
// Not handled messages
case NAV_ANT:
std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
break;
case NAV_DGPS:
std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
break;
case NAV_SSR:
std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
break;
case NAV_LEX:
std::cout << "UBloxRaw: Received lex message. Not handled.\n";
break;
case ERROR:
std::cout << "UBloxRaw: Received error message. Not handled.\n";
break;
default:
std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
break;
}
// Not handled messages
case NAV_ANT:
std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
break;
case NAV_DGPS:
std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
break;
case NAV_SSR:
std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
break;
case NAV_LEX:
std::cout << "UBloxRaw: Received lex message. Not handled.\n";
break;
case ERROR:
std::cout << "UBloxRaw: Received error message. Not handled.\n";
break;
default:
std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
break;
}
}
return raw_data_type_;
return raw_data_type_;
}
void UBloxRaw::updateObservations()
{
// sort and remove duplicated observations
sortobs(&raw_data_.obs);
// std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
// obs_.print();
// sort and remove duplicated observations
sortobs(&raw_data_.obs);
// std::cout << "---------------------------JUST BEFORE!-------------------" << std::endl;
// obs_.print();
obs_.clearObservations();
for (int ii = 0; ii < raw_data_.obs.n; ++ii)
obs_.addObservation(raw_data_.obs.data[ii]);
obs_.clearObservations();
for (int ii = 0; ii < raw_data_.obs.n; ++ii)
obs_.addObservation(raw_data_.obs.data[ii]);
// std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
// obs_.print();
// std::cout << "--------------------------JUST AFTER!---------------------" << std::endl;
// obs_.print();
}
#include "gnss_utils/utils.h"
namespace GNSSUtils
{
void print(std::string & _msg)
{
std::string msg = GNSSUTILS_MSG + _msg;
void print(std::string& _msg)
{
std::string msg = GNSSUTILS_MSG + _msg;
std::cout << msg << "\n";
}
std::cout << msg << "\n";
}
void printArray(std::string _name, int * _array, int size)
{
std::cout << _name << ": [";
for (int ii=0; ii<size; ++ii)
{
std::cout << _array[ii];
if (ii==size-1)
std::cout << "] \n";
else
std::cout << ",";
}
void printArray(std::string _name, int* _array, int size)
{
std::cout << _name << ": [";
for (int ii = 0; ii < size; ++ii)
{
std::cout << _array[ii];
if (ii == size - 1)
std::cout << "] \n";
else
std::cout << ",";
}
}
void printArray(std::string _name, unsigned char * _array, int size)
void printArray(std::string _name, unsigned char* _array, int size)
{
std::cout << _name << ": [";
for (int ii = 0; ii < size; ++ii)
{
std::cout << _name << ": [";
for (int ii=0; ii<size; ++ii)
{
std::cout << (int)(_array[ii]);
if (ii==size-1)
std::cout << "] \n";
else
std::cout << ",";
}
std::cout << (int)(_array[ii]);
if (ii == size - 1)
std::cout << "] \n";
else
std::cout << ",";
}
}
void printArray(std::string _name, double * _array, int size)
{
std::cout << _name << ": [";
for (int ii=0; ii<size; ++ii)
{
std::cout << _array[ii];
if (ii==size-1)
std::cout << "] \n";
else
std::cout << ",";
}
void printArray(std::string _name, double* _array, int size)
{
std::cout << _name << ": [";
for (int ii = 0; ii < size; ++ii)
{
std::cout << _array[ii];
if (ii == size - 1)
std::cout << "] \n";
else
std::cout << ",";
}
}
void printArray(std::string _name, float * _array, int size)
{
std::cout << _name << ": [";
for (int ii=0; ii<size; ++ii)
{
std::cout << _array[ii];
if (ii==size-1)
std::cout << "] \n";
else
std::cout << ",";
}
void printArray(std::string _name, float* _array, int size)
{
std::cout << _name << ": [";
for (int ii = 0; ii < size; ++ii)
{
std::cout << _array[ii];
if (ii == size - 1)
std::cout << "] \n";
else
std::cout << ",";
}
}
} // namespace GNSSUtils
......@@ -15,3 +15,9 @@ include_directories(${GTEST_INCLUDE_DIRS})
# Transformations test
gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
target_link_libraries(gtest_transformations ${PROJECT_NAME})
# Observations test
add_executable(gtest_observations gtest_observations.cpp)
add_dependencies(gtest_observations libgtest)
target_link_libraries(gtest_observations libgtest ${PROJECT_NAME})
add_test(NAME gtest_observations COMMAND gtest_observations "${CMAKE_CURRENT_LIST_DIR}/../src/examples/sample_data.obs")
#include "gtest/utils_gtest.h"
#include "gnss_utils/gnss_utils.h"
TEST(NavigationTests, Whatever)
{
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
\ No newline at end of file
#include "gtest/utils_gtest.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
std::string rnx_file;
obs_t obs;
const gtime_t t_start{ 0, 0 }; // no limit
const gtime_t t_end{ 0, 0 }; // no limit
const double dt = 0.0; // no limit
const char* opt = "-SYS=G"; // only GPS | GPS+GAL: "-SYS=G,L" | ALL: ""
void loadRinex()
{
// RTKLIB utilities
obs.data = (obsd_t*)malloc(sizeof(obsd_t) * MAXSAT);
obs.n = 0;
obs.nmax = MAXSAT;
int stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
ASSERT_EQ(stat, 1);
sortobs(&obs);
}
TEST(ObservationsTest, AddClearObservation)
{
loadRinex();
Observations observations;
// testing addition
for (int ii = 0; ii < obs.n; ++ii)
{
observations.addObservation(obs.data[ii]);
ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii]));
}
ASSERT_TRUE(obs.n == observations.getObservations().size());
//Testing clear
observations.clearObservations();
ASSERT_TRUE(0 == observations.getObservations().size());
}
TEST(ObservationsTest, LoadFromRinex)
{
// GNSSUtils utilities
Observations observations;
observations.loadFromRinex(rnx_file.c_str(), t_start, t_end, dt, opt);
observations.print();
// RTKLIB utilities
loadRinex();
ASSERT_EQ(obs.n, 6);
// Comparison
ASSERT_TRUE(obs.n == observations.getObservations().size());
for (int ii = 0; ii < obs.n; ++ii)
{
ASSERT_TRUE(equalObservations(obs.data[ii], observations.getObservations()[ii]));
}
free(obs.data);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
rnx_file = argv[1];
return RUN_ALL_TESTS();
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment