From 6c1bda38ca94287b70080567b1434ba7de770b5b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 7 Jul 2020 17:12:06 +0200
Subject: [PATCH] SBAS bug fixed
---
deps/RTKLIB | 2 +-
include/gnss_utils/navigation.h | 5 -----
src/navigation.cpp | 34 +++++++++++++++++++++++++++++----
src/receivers/ublox_raw.cpp | 2 +-
4 files changed, 32 insertions(+), 11 deletions(-)
diff --git a/deps/RTKLIB b/deps/RTKLIB
index 3f0bb15..c28e430 160000
--- a/deps/RTKLIB
+++ b/deps/RTKLIB
@@ -1 +1 @@
-Subproject commit 3f0bb15ea844df97592a5d0c96222da5db6566ae
+Subproject commit c28e43077f2fb431ce3e1cc306155342e7307c66
diff --git a/include/gnss_utils/navigation.h b/include/gnss_utils/navigation.h
index 929dec2..f4910da 100644
--- a/include/gnss_utils/navigation.h
+++ b/include/gnss_utils/navigation.h
@@ -132,11 +132,6 @@ inline bool Navigation::addFcb(const fcbd_t& fcb)
return addToArray<fcbd_t>(fcb, nav_.fcb, nav_.nf, nav_.nfmax);
}
-inline void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
-{
- sbsupdatecorr(&sbas_msg, &nav_);
-}
-
inline void Navigation::freeEphemeris()
{
freeEph(nav_);
diff --git a/src/navigation.cpp b/src/navigation.cpp
index c0eb72c..8c76449 100644
--- a/src/navigation.cpp
+++ b/src/navigation.cpp
@@ -158,6 +158,25 @@ void Navigation::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gti
<< std::endl;
}
+void Navigation::addSbasMessage(const sbsmsg_t& sbas_msg)
+{
+ // init seph with array of size NSATSBS*2
+ if (nav_.seph == NULL)
+ {
+ std::cout << "Navigation::addSbasMessage: initializing sbas ephemerides array...\n";
+ if (!(nav_.seph =(seph_t *)malloc(sizeof(seph_t)*NSATSBS*2)))
+ {
+ std::cout << "Navigation::addSbasMessage: could not malloc sbas ephemerides!\n";
+ return;
+ }
+ nav_.ns=NSATSBS*2;
+ nav_.nsmax=NSATSBS*2;
+ for (auto i=0; i<NSATSBS*2; i++)
+ nav_.seph[i]=seph_t{0};
+ }
+ sbsupdatecorr(&sbas_msg, &nav_);
+}
+
void Navigation::copyAllArrays(const nav_t& nav)
{
// GPS/QZS/GAL ephemeris
@@ -182,11 +201,18 @@ void Navigation::copyAllArrays(const nav_t& nav)
void Navigation::copyEphemeris(const nav_t& nav)
{
- copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
- copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
- copyArray<seph_t>(nav.seph, nav.ns, nav_.seph, nav_.ns, nav_.nsmax);
+ // Not SBAS ephemeris!
- uniqueNavigation();
+ // copy
+ copyArray<eph_t>(nav.eph, nav.n, nav_.eph, nav_.n, nav_.nmax);
+ copyArray<geph_t>(nav.geph, nav.ng, nav_.geph, nav_.ng, nav_.ngmax);
+
+ // unique
+ uniqeph (&nav_);
+ uniqgeph(&nav_);
+
+ // wavelengths
+ updatenav(&nav_);
}
void Navigation::copyAlmanac(const nav_t& nav)
diff --git a/src/receivers/ublox_raw.cpp b/src/receivers/ublox_raw.cpp
index 6c2ba9a..afaf32b 100644
--- a/src/receivers/ublox_raw.cpp
+++ b/src/receivers/ublox_raw.cpp
@@ -29,7 +29,7 @@ RawDataType UBloxRaw::addDataStream(const std::vector<uint8_t>& data_stream)
break;
case NAV_EPH: // Ephemeris
- //std::cout << "Ephemeris received!\n";
+ std::cout << "Ephemeris received!\n";
nav_.copyEphemeris(raw_data_.nav);
break;
--
GitLab