From d723310fe7962377c805ccc2b95afea745a4122d Mon Sep 17 00:00:00 2001
From: PepMS <jmarti@iri.upc.edu>
Date: Wed, 6 Nov 2019 10:33:09 +0100
Subject: [PATCH] debugging
---
src/gnss_utils.cpp | 38 ++++++++++++++++++++++++--------------
1 file changed, 24 insertions(+), 14 deletions(-)
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index f8e9b42..b5414ed 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -1,5 +1,6 @@
#include "gnss_utils/gnss_utils.h"
+
namespace GNSSUtils
{
GNSSUtils::ComputePosOutput computePos(const GNSSUtils::Observations & _observations,
@@ -7,21 +8,24 @@ namespace GNSSUtils
const prcopt_t & _prcopt)
{
// 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
- // prcopt_t prcopt = prcopt_default;
- // prcopt.mode = PMODE_SINGLE;
- // prcopt.soltype = 0;
- // prcopt.nf = 1;
- // prcopt.navsys = SYS_GPS;
- // //prcopt.elmin = 1.05; // 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_t prcopt = prcopt_default;
+ prcopt.mode = PMODE_SINGLE;
+ prcopt.soltype = 0;
+ prcopt.nf = 1;
+ prcopt.navsys = SYS_GPS;
+ // prcopt.elmin = 1.05; // 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;
// Define error msg
char msg[128] = "";
@@ -32,11 +36,17 @@ namespace GNSSUtils
output.pos_stat = pntpos(_observations.getObservations().data(), _observations.getObservations().size(),
&(_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.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],
--
GitLab