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

debugging

parent 5991f48e
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
#include "gnss_utils/gnss_utils.h" #include "gnss_utils/gnss_utils.h"
namespace GNSSUtils namespace GNSSUtils
{ {
GNSSUtils::ComputePosOutput computePos(const GNSSUtils::Observations & _observations, GNSSUtils::ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
...@@ -7,21 +8,24 @@ namespace GNSSUtils ...@@ -7,21 +8,24 @@ namespace GNSSUtils
const prcopt_t & _prcopt) const prcopt_t & _prcopt)
{ {
// Remove duplicated satellites // Remove duplicated satellites
uniqnav(&(_navigation.getNavigation())); // uniqnav(&(_navigation.getNavigation()));
std::cout << "Number of navigation satellites: " << _navigation.getNavigation().n << "\n";
std::cout << "Number of observations: " << _observations.getObservations().size() << "\n";
// Define processing options // Define processing options
// prcopt_t prcopt = prcopt_default; prcopt_t prcopt = prcopt_default;
// prcopt.mode = PMODE_SINGLE; prcopt.mode = PMODE_SINGLE;
// prcopt.soltype = 0; prcopt.soltype = 0;
// prcopt.nf = 1; prcopt.nf = 1;
// prcopt.navsys = SYS_GPS; prcopt.navsys = SYS_GPS;
// //prcopt.elmin = 1.05; // 60 degrees = 1.05 rad // prcopt.elmin = 1.05; // 60 degrees = 1.05 rad
// prcopt.sateph = EPHOPT_BRDC; prcopt.sateph = EPHOPT_BRDC;
// prcopt.ionoopt = IONOOPT_OFF; prcopt.ionoopt = IONOOPT_OFF;
// prcopt.tropopt = TROPOPT_OFF; prcopt.tropopt = TROPOPT_OFF;
// prcopt.dynamics = 0; prcopt.dynamics = 0;
// prcopt.tidecorr = 0; prcopt.tidecorr = 0;
// prcopt.sbascorr = SBSOPT_FCORR; prcopt.sbascorr = SBSOPT_FCORR;
// Define error msg // Define error msg
char msg[128] = ""; char msg[128] = "";
...@@ -32,11 +36,17 @@ namespace GNSSUtils ...@@ -32,11 +36,17 @@ namespace GNSSUtils
output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(), output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(),
&(_navigation.getNavigation()), &(_navigation.getNavigation()),
&_prcopt, &sol, NULL, NULL, msg); &prcopt, &sol, NULL, NULL, msg);
if (output.pos_stat == 0)
{
std::cout << "Bad computing: " << msg << "\n";
}
output.time = sol.time.time; output.time = sol.time.time;
output.time = sol.time.sec; output.time = sol.time.sec;
output.pos = Eigen::Vector3d(sol.rr); output.pos = Eigen::Vector3d(sol.rr);
std::cout << "Compute pos: " << output.pos.transpose() << "\n";
output.vel = Eigen::Vector3d(&sol.rr[3]); output.vel = Eigen::Vector3d(&sol.rr[3]);
output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
sol.qr[3], sol.qr[1], sol.qr[4], sol.qr[3], sol.qr[1], sol.qr[4],
......
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