Skip to content
Snippets Groups Projects
Commit f48bc6c2 authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

[utils] wip

parent 29211151
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
bin/ bin/
build/ build/
lib/ lib/
\ No newline at end of file .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
Subproject commit 4b8f505328faeb827fe7e0eb5adac1dddaf39cf4 Subproject commit eac44a45c19ac56c0c031135e9818788b5993005
#ifndef GNSS_UTILS_H #ifndef GNSS_UTILS_H
#define GNSS_UTILS_H #define GNSS_UTILS_H
#include <vector> #include <vector>
#include <iostream> #include <iostream>
#include <memory> #include <memory>
...@@ -14,62 +13,91 @@ ...@@ -14,62 +13,91 @@
#include "gnss_utils/observations.h" #include "gnss_utils/observations.h"
#include "gnss_utils/navigation.h" #include "gnss_utils/navigation.h"
extern "C" extern "C" {
{ #include "rtklib.h"
#include "rtklib.h"
} }
namespace GNSSUtils namespace GNSSUtils
{ {
struct ComputePosOutput{ struct ComputePosOutput
time_t time; {
double sec; time_t time;
Eigen::Vector3d pos; // position (m) double sec;
Eigen::Vector3d vel; // velocity (m/s) Eigen::Vector3d pos; // position (m)
Eigen::Matrix3d pos_covar; // position covariance (m^2) Eigen::Vector3d vel; // velocity (m/s)
// {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} Eigen::Matrix3d pos_covar; // position covariance (m^2)
Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s) // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
int type; // coordinates used (0:xyz-ecef,1:enu-baseline) Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
int stat; // solution status (SOLQ_???) int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
int ns; // number of valid satellites int stat; // solution status (SOLQ_???)
double age; // age of differential (s) int ns; // number of valid satellites
double ratio; // AR ratio factor for valiation 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 int pos_stat; // return from pntpos
}; Eigen::Vector3d lat_lon; // latitude_longitude_altitude
};
ComputePosOutput computePos(const Observations & _observations,
Navigation & _navigation, ComputePosOutput computePos(const Observations& _observations, Navigation& _navigation, const prcopt_t& _prcopt);
const prcopt_t & _prcopt);
// ComputePosOutput computePosOwn(const Observations & _observations,
// ComputePosOutput computePosOwn(const Observations & _observations, // Navigation & _navigation,
// Navigation & _navigation, // const prcopt_t & _prcopt);
// const prcopt_t & _prcopt);
// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
// int pntposOwn(const obsd_t *obs, int n, const nav_t *nav, // const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
// const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, // char *msg);
// char *msg);
int estposOwn(const obsd_t* obs,
int estposOwn(const obsd_t *obs, int n, const double *rs, const double *dts, int n,
const double *vare, const int *svh, const nav_t *nav, const double* rs,
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, const double* dts,
double *resp, char *msg); const double* vare,
const int* svh,
Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef); const nav_t* nav,
Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon); const prcopt_t* opt,
Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef); sol_t* sol,
Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu); double* azel,
int* vsat,
void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF); double* resp,
void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF); char* msg);
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef); Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d& _ecef);
Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d& _latlon);
void computeSatellitesPositions(const Observations& obs, Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_ecef);
const Navigation& nav, Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d& _latlon, const Eigen::Matrix3d _cov_enu);
const prcopt_t& opt,
std::map<int,Eigen::Vector3d>& sats_pos); 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);
} // namespace GNSSUtils
#endif #endif
This diff is collapsed.
#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"
TEST(ObservationsTest, LoadFromRinex)
{
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
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