1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-02 08:13:04 +00:00

Remove build and data folders, move tests and utils to the base of the source tree

This commit is contained in:
Carles Fernandez
2024-10-04 11:55:09 +02:00
parent 5be2971c9b
commit 825037592a
251 changed files with 154 additions and 179 deletions

13
utils/CMakeLists.txt Normal file
View File

@@ -0,0 +1,13 @@
# GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
# This file is part of GNSS-SDR.
#
# SPDX-FileCopyrightText: 2010-2020 C. Fernandez-Prades cfernandez(at)cttc.es
# SPDX-License-Identifier: BSD-3-Clause
add_subdirectory(front-end-cal)
if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
add_subdirectory(rinex-tools)
add_subdirectory(rinex2assist)
endif()

View File

@@ -0,0 +1,155 @@
# GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
# This file is part of GNSS-SDR.
#
# SPDX-FileCopyrightText: 2010-2020 C. Fernandez-Prades cfernandez(at)cttc.es
# SPDX-License-Identifier: BSD-3-Clause
if(USE_CMAKE_TARGET_SOURCES)
add_library(front_end_cal_lib STATIC)
target_sources(front_end_cal_lib
PRIVATE
front_end_cal.cc
PUBLIC
front_end_cal.h
)
else()
source_group(Headers FILES front_end_cal.h)
add_library(front_end_cal_lib front_end_cal.cc front_end_cal.h)
endif()
target_link_libraries(front_end_cal_lib
PUBLIC
Armadillo::armadillo
Threads::Threads
acquisition_adapters
gnss_sdr_flags
channel_libs
algorithms_libs
core_receiver
core_libs
PRIVATE
Boost::headers
Gnuradio::blocks
Gnuradio::runtime
)
if(ENABLE_GLOG_AND_GFLAGS)
target_link_libraries(front_end_cal_lib PRIVATE Gflags::gflags Glog::glog)
target_compile_definitions(front_end_cal_lib PRIVATE -DUSE_GLOG_AND_GFLAGS=1)
else()
target_link_libraries(front_end_cal_lib PRIVATE absl::flags absl::log)
endif()
if(ENABLE_CLANG_TIDY)
if(CLANG_TIDY_EXE)
set_target_properties(front_end_cal_lib
PROPERTIES
CXX_CLANG_TIDY "${DO_CLANG_TIDY}"
)
endif()
endif()
if(USE_CMAKE_TARGET_SOURCES)
add_executable(front-end-cal)
target_sources(front-end-cal PRIVATE main.cc)
else()
add_executable(front-end-cal main.cc)
endif()
target_link_libraries(front-end-cal
PRIVATE
core_receiver
algorithms_libs
front_end_cal_lib
gnss_sdr_flags
Boost::headers
)
if(ENABLE_GLOG_AND_GFLAGS)
target_link_libraries(front-end-cal PRIVATE Glog::glog)
target_compile_definitions(front-end-cal PRIVATE -DUSE_GLOG_AND_GFLAGS=1)
else()
target_link_libraries(front-end-cal PRIVATE absl::flags absl::flags_parse absl::log absl::log_initialize absl::log_sink absl::log_sink_registry)
endif()
if(GNURADIO_USES_STD_POINTERS)
target_compile_definitions(front-end-cal
PRIVATE -DGNURADIO_USES_STD_POINTERS=1
)
endif()
target_compile_definitions(front-end-cal
PRIVATE -DGNSS_SDR_VERSION="${VERSION}"
PRIVATE -DGNSSSDR_INSTALL_DIR="${CMAKE_INSTALL_PREFIX}"
)
if(USE_GENERIC_LAMBDAS)
set(has_generic_lambdas HAS_GENERIC_LAMBDA=1)
set(no_has_generic_lambdas HAS_GENERIC_LAMBDA=0)
target_compile_definitions(front-end-cal
PRIVATE
"$<$<COMPILE_FEATURES:cxx_generic_lambdas>:${has_generic_lambdas}>"
"$<$<NOT:$<COMPILE_FEATURES:cxx_generic_lambdas>>:${no_has_generic_lambdas}>"
)
else()
target_compile_definitions(front-end-cal
PRIVATE
-DHAS_GENERIC_LAMBDA=0
)
endif()
if(USE_BOOST_BIND_PLACEHOLDERS)
target_compile_definitions(front-end-cal
PRIVATE
-DUSE_BOOST_BIND_PLACEHOLDERS=1
)
endif()
if(PMT_USES_BOOST_ANY)
target_compile_definitions(front-end-cal
PRIVATE
-DPMT_USES_BOOST_ANY=1
)
endif()
include(XcodeRemoveWarningDuplicates)
xcode_remove_warning_duplicates(front-end-cal)
if(ENABLE_STRIP)
set_target_properties(front-end-cal PROPERTIES LINK_FLAGS "-s")
endif()
if(ENABLE_CLANG_TIDY)
if(CLANG_TIDY_EXE)
set_target_properties(front-end-cal
PROPERTIES
CXX_CLANG_TIDY "${DO_CLANG_TIDY}"
)
endif()
endif()
add_custom_command(TARGET front-end-cal POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:front-end-cal>
${LOCAL_INSTALL_BASE_DIR}/install/$<TARGET_FILE_NAME:front-end-cal>
)
install(TARGETS front-end-cal
RUNTIME DESTINATION bin
COMPONENT "front-end-cal"
)
find_program(GZIP gzip
/bin
/usr/bin
/usr/local/bin
/opt/local/bin
/sbin
)
if(NOT GZIP_NOTFOUND)
execute_process(COMMAND gzip -9 -c ${GNSSSDR_SOURCE_DIR}/docs/manpage/front-end-cal-manpage
WORKING_DIRECTORY ${GNSSSDR_BINARY_DIR} OUTPUT_FILE "${GNSSSDR_BINARY_DIR}/front-end-cal.1.gz"
)
install(FILES ${GNSSSDR_BINARY_DIR}/front-end-cal.1.gz DESTINATION share/man/man1)
endif()

View File

@@ -0,0 +1,384 @@
/*!
* \file front_end_cal.cc
* \brief Implementation of the Front-end calibration program.
* \author Javier Arribas, 2013. jarribas(at)cttc.es
*
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "front_end_cal.h"
#include "GPS_L1_CA.h" // for GPS_L1_FREQ_HZ
#include "concurrent_map.h"
#include "configuration_interface.h"
#include "gnss_sdr_supl_client.h"
#include "gps_acq_assist.h" // for Gps_Acq_Assist
#include "gps_almanac.h"
#include "gps_ephemeris.h"
#include "gps_iono.h"
#include "gps_utc_model.h"
#include <boost/lexical_cast.hpp>
#include <algorithm> // for min
#include <cmath>
#include <iostream> // for operator<<
#include <map>
#include <stdexcept>
#include <utility>
#if USE_GLOG_AND_GFLAGS
#include <glog/logging.h>
#else
#include <absl/log/log.h>
#endif
extern Concurrent_Map<Gps_Ephemeris> global_gps_ephemeris_map;
extern Concurrent_Map<Gps_Iono> global_gps_iono_map;
extern Concurrent_Map<Gps_Utc_Model> global_gps_utc_model_map;
extern Concurrent_Map<Gps_Almanac> global_gps_almanac_map;
extern Concurrent_Map<Gps_Acq_Assist> global_gps_acq_assist_map;
bool FrontEndCal::read_assistance_from_XML()
{
Gnss_Sdr_Supl_Client supl_client_ephemeris_;
std::string eph_xml_filename = "gps_ephemeris.xml";
std::cout << "SUPL: Trying to read GPS ephemeris from XML file " << eph_xml_filename << '\n';
LOG(INFO) << "SUPL: Trying to read GPS ephemeris from XML file " << eph_xml_filename;
if (supl_client_ephemeris_.load_ephemeris_xml(eph_xml_filename) == true)
{
std::map<int, Gps_Ephemeris>::iterator gps_eph_iter;
for (gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.begin();
gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.end();
gps_eph_iter++)
{
std::cout << "SUPL: Read XML Ephemeris for GPS SV " << gps_eph_iter->first << '\n';
LOG(INFO) << "SUPL: Read XML Ephemeris for GPS SV " << gps_eph_iter->first;
LOG(INFO) << "New Ephemeris record inserted with Toe=" << gps_eph_iter->second.toe << " and GPS Week=" << gps_eph_iter->second.WN;
global_gps_ephemeris_map.write(gps_eph_iter->second.PRN, gps_eph_iter->second);
}
return true;
}
std::cout << "ERROR: SUPL client error reading XML\n";
LOG(WARNING) << "ERROR: SUPL client error reading XML";
return false;
}
int FrontEndCal::Get_SUPL_Assist()
{
// ######### GNSS Assistance #################################
Gnss_Sdr_Supl_Client supl_client_acquisition_;
Gnss_Sdr_Supl_Client supl_client_ephemeris_;
int supl_mcc; // Current network MCC (Mobile country code), 3 digits.
int supl_mns; // Current network MNC (Mobile Network code), 2 or 3 digits.
int supl_lac; // Current network LAC (Location area code),16 bits, 1-65520 are valid values.
int supl_ci; // Cell Identity (16 bits, 0-65535 are valid values).
// GNSS Assistance configuration
int error = 0;
bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false);
if (enable_gps_supl_assistance == true)
// SUPL SERVER TEST. Not operational yet!
{
LOG(INFO) << "SUPL RRLP GPS assistance enabled!";
std::string default_acq_server = "supl.nokia.com";
std::string default_eph_server = "supl.google.com";
supl_client_ephemeris_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_server", std::move(default_acq_server));
supl_client_acquisition_.server_name = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_server", std::move(default_eph_server));
supl_client_ephemeris_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_port", 7275);
supl_client_acquisition_.server_port = configuration_->property("GNSS-SDR.SUPL_gps_acquisition_port", 7275);
supl_mcc = configuration_->property("GNSS-SDR.SUPL_MCC", 244);
supl_mns = configuration_->property("GNSS-SDR.SUPL_MNC", 5);
std::string default_lac = "0x59e2";
std::string default_ci = "0x31b0";
try
{
supl_lac = boost::lexical_cast<int>(configuration_->property("GNSS-SDR.SUPL_LAC", std::move(default_lac)));
}
catch (boost::bad_lexical_cast &)
{
supl_lac = 0x59e2;
}
try
{
supl_ci = boost::lexical_cast<int>(configuration_->property("GNSS-SDR.SUPL_CI", std::move(default_ci)));
}
catch (boost::bad_lexical_cast &)
{
supl_ci = 0x31b0;
}
bool SUPL_read_gps_assistance_xml = configuration_->property("GNSS-SDR.SUPL_read_gps_assistance_xml", false);
if (SUPL_read_gps_assistance_xml == true)
{
// read assistance from file
read_assistance_from_XML();
}
else
{
// Request ephemeris from SUPL server
supl_client_ephemeris_.request = 1;
LOG(INFO) << "SUPL: Trying to read GPS ephemeris from SUPL server...";
std::cout << "SUPL: Trying to read GPS ephemeris from SUPL server...\n";
error = supl_client_ephemeris_.get_assistance(supl_mcc, supl_mns, supl_lac, supl_ci);
if (error == 0)
{
std::map<int, Gps_Ephemeris>::iterator gps_eph_iter;
for (gps_eph_iter = supl_client_ephemeris_.gps_ephemeris_map.begin();
gps_eph_iter != supl_client_ephemeris_.gps_ephemeris_map.end();
gps_eph_iter++)
{
LOG(INFO) << "SUPL: Received Ephemeris for GPS SV " << gps_eph_iter->first;
std::cout << "SUPL: Received Ephemeris for GPS SV " << gps_eph_iter->first << '\n';
LOG(INFO) << "New Ephemeris record inserted with Toe=" << gps_eph_iter->second.toe << " and GPS Week=" << gps_eph_iter->second.WN;
global_gps_ephemeris_map.write(gps_eph_iter->second.PRN, gps_eph_iter->second);
}
// Save ephemeris to XML file
std::string eph_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename);
if (supl_client_ephemeris_.save_ephemeris_map_xml(eph_xml_filename, supl_client_ephemeris_.gps_ephemeris_map) == true)
{
LOG(INFO) << "SUPL: XML Ephemeris file created.";
}
}
else
{
LOG(WARNING) << "ERROR: SUPL client for Ephemeris returned " << error;
std::cout << "ERROR in SUPL client. Please check your Internet connection and SUPL server configuration\n";
}
// Request almanac , IONO and UTC Model
supl_client_ephemeris_.request = 0;
LOG(INFO) << "SUPL: Try read Almanac, Iono, Utc Model, Ref Time and Ref Location from SUPL server...";
error = supl_client_ephemeris_.get_assistance(supl_mcc, supl_mns, supl_lac, supl_ci);
if (error == 0)
{
std::map<int, Gps_Almanac>::iterator gps_alm_iter;
for (gps_alm_iter = supl_client_ephemeris_.gps_almanac_map.begin();
gps_alm_iter != supl_client_ephemeris_.gps_almanac_map.end();
gps_alm_iter++)
{
LOG(INFO) << "SUPL: Received Almanac for GPS SV " << gps_alm_iter->first;
std::cout << "SUPL: Received Almanac for GPS SV " << gps_alm_iter->first << '\n';
global_gps_almanac_map.write(gps_alm_iter->first, gps_alm_iter->second);
}
if (supl_client_ephemeris_.gps_iono.valid == true)
{
LOG(INFO) << "SUPL: Received GPS Iono";
std::cout << "SUPL: Received GPS Iono\n";
global_gps_iono_map.write(0, supl_client_ephemeris_.gps_iono);
}
if (supl_client_ephemeris_.gps_utc.valid == true)
{
LOG(INFO) << "SUPL: Received GPS UTC Model";
std::cout << "SUPL: Received GPS UTC Model\n";
global_gps_utc_model_map.write(0, supl_client_ephemeris_.gps_utc);
}
}
else
{
LOG(WARNING) << "ERROR: SUPL client for Almanac returned " << error;
std::cout << "ERROR in SUPL client. Please check your Internet connection and SUPL server configuration\n";
}
// Request acquisition assistance
supl_client_acquisition_.request = 2;
LOG(INFO) << "SUPL: Trying to read Acquisition assistance from SUPL server...";
std::cout << "SUPL: Trying to read Acquisition assistance from SUPL server...\n";
error = supl_client_acquisition_.get_assistance(supl_mcc, supl_mns, supl_lac, supl_ci);
if (error == 0)
{
std::map<int, Gps_Acq_Assist>::iterator gps_acq_iter;
for (gps_acq_iter = supl_client_acquisition_.gps_acq_map.begin();
gps_acq_iter != supl_client_acquisition_.gps_acq_map.end();
gps_acq_iter++)
{
LOG(INFO) << "SUPL: Received Acquisition assistance for GPS SV " << gps_acq_iter->first;
std::cout << "SUPL: Received Acquisition assistance for GPS SV " << gps_acq_iter->first << '\n';
LOG(INFO) << "New acq assist record inserted";
global_gps_acq_assist_map.write(gps_acq_iter->second.PRN, gps_acq_iter->second);
}
}
else
{
LOG(WARNING) << "ERROR: SUPL client for Acquisition assistance returned " << error;
std::cout << "ERROR in SUPL client. Please check your Internet connection and SUPL server configuration\n";
}
}
}
return error;
}
void FrontEndCal::set_configuration(std::shared_ptr<ConfigurationInterface> configuration)
{
configuration_ = std::move(configuration);
}
bool FrontEndCal::get_ephemeris()
{
bool read_ephemeris_from_xml = configuration_->property("GNSS-SDR.read_eph_from_xml", false);
if (read_ephemeris_from_xml == true)
{
std::cout << "Trying to read ephemeris from XML file...\n";
LOG(INFO) << "Trying to read ephemeris from XML file...";
if (read_assistance_from_XML() == false)
{
std::cout << "ERROR: Could not read Ephemeris file: Trying to get ephemeris from SUPL server...\n";
LOG(INFO) << "ERROR: Could not read Ephemeris file: Trying to get ephemeris from SUPL server...";
if (Get_SUPL_Assist() == 1)
{
return true;
}
return false;
}
return true;
}
std::cout << "Trying to read ephemeris from SUPL server...\n";
LOG(INFO) << "Trying to read ephemeris from SUPL server...";
if (Get_SUPL_Assist() == 0)
{
return true;
}
return false;
}
arma::vec FrontEndCal::lla2ecef(const arma::vec &lla)
{
// WGS84 flattening
double f = 1.0 / 298.257223563;
// WGS84 equatorial radius
double R = 6378137.0;
arma::vec ellipsoid = "0.0 0.0";
double phi = (lla(0) / 360.0) * TWO_PI;
double lambda = (lla(1) / 360.0) * TWO_PI;
ellipsoid(0) = R;
ellipsoid(1) = sqrt(1.0 - (1.0 - f) * (1.0 - f));
arma::vec ecef = "0.0 0.0 0.0 0.0";
ecef = geodetic2ecef(phi, lambda, lla(3), ellipsoid);
return ecef;
}
arma::vec FrontEndCal::geodetic2ecef(double phi, double lambda, double h, const arma::vec &ellipsoid)
{
double a = ellipsoid(0);
double e2 = ellipsoid(1) * ellipsoid(1);
double sinphi = sin(phi);
double cosphi = cos(phi);
double N = a / sqrt(1.0 - e2 * sinphi * sinphi);
arma::vec ecef = "0.0 0.0 0.0 0.0";
ecef(0) = (N + h) * cosphi * cos(lambda);
ecef(1) = (N + h) * cosphi * sin(lambda);
ecef(2) = (N * (1.0 - e2) + h) * sinphi;
return ecef;
}
double FrontEndCal::estimate_doppler_from_eph(unsigned int PRN, double tow, double lat, double lon, double height) noexcept(false)
{
int num_secs = 10;
double step_secs = 0.5;
// Observer position ECEF
arma::vec obs_ecef = "0.0 0.0 0.0 0.0";
arma::vec lla = "0.0 0.0 0.0 0.0";
lla(0) = lat;
lla(1) = lon;
lla(2) = height;
obs_ecef = lla2ecef(lla);
// Satellite positions ECEF
std::map<int, Gps_Ephemeris> eph_map;
eph_map = global_gps_ephemeris_map.get_map_copy();
std::map<int, Gps_Ephemeris>::iterator eph_it;
eph_it = eph_map.find(PRN);
if (eph_it != eph_map.end())
{
arma::vec SV_pos_ecef = "0.0 0.0 0.0 0.0";
double obs_time_start;
double obs_time_stop;
obs_time_start = tow - static_cast<double>(num_secs) / 2.0;
obs_time_stop = tow + static_cast<double>(num_secs) / 2.0;
int n_points = round((obs_time_stop - obs_time_start) / step_secs);
arma::vec ranges = arma::zeros(n_points, 1);
double obs_time = obs_time_start;
for (int i = 0; i < n_points; i++)
{
eph_it->second.satellitePosition(obs_time);
SV_pos_ecef(0) = eph_it->second.satpos_X;
SV_pos_ecef(1) = eph_it->second.satpos_Y;
SV_pos_ecef(2) = eph_it->second.satpos_Z;
// SV distances to observer (true range)
ranges(i) = arma::norm(SV_pos_ecef - obs_ecef, 2);
obs_time += step_secs;
}
// Observer to satellite radial velocity
// Numeric derivative: Positive slope means that the distance from obs to
// satellite is increasing
arma::vec obs_to_sat_velocity;
obs_to_sat_velocity = (ranges.subvec(1, (n_points - 1)) - ranges.subvec(0, (n_points - 2))) / step_secs;
// Doppler equations are formulated accounting for positive velocities if the
// tx and rx are approaching to each other. So, the satellite velocity must
// be redefined as:
obs_to_sat_velocity = -obs_to_sat_velocity;
// Doppler estimation
arma::vec Doppler_Hz;
Doppler_Hz = (obs_to_sat_velocity / SPEED_OF_LIGHT_M_S) * GPS_L1_FREQ_HZ;
double mean_Doppler_Hz;
mean_Doppler_Hz = arma::mean(Doppler_Hz);
return mean_Doppler_Hz;
}
throw std::runtime_error("1");
}
void FrontEndCal::GPS_L1_front_end_model_E4000(double f_bb_true_Hz, double f_bb_meas_Hz, double fs_nominal_hz, double *estimated_fs_Hz, double *estimated_f_if_Hz, double *f_osc_err_ppm)
{
const double f_osc_n = 28.8e6;
// PLL registers settings (according to E4000 datasheet)
const double N = 109.0;
const double Y = 65536.0;
const double X = 26487.0;
const double R = 2.0;
// Obtained RF center frequency
double f_rf_pll = (f_osc_n * (N + X / Y)) / R;
// RF frequency error caused by fractional PLL roundings
double f_bb_err_pll = GPS_L1_FREQ_HZ - f_rf_pll;
// Measured F_rf error
double f_rf_err = (f_bb_meas_Hz - f_bb_true_Hz) - f_bb_err_pll;
double f_osc_err_hz = (f_rf_err * R) / (N + X / Y);
f_osc_err_hz = -f_osc_err_hz;
*f_osc_err_ppm = f_osc_err_hz / (f_osc_n / 1e6);
double frac = fs_nominal_hz / f_osc_n;
*estimated_fs_Hz = frac * (f_osc_n + f_osc_err_hz);
*estimated_f_if_Hz = f_rf_err;
}

View File

@@ -0,0 +1,129 @@
/*!
* \file front_end_cal.h
* \brief Interface of the Front-end calibration program.
* \author Javier Arribas, 2013. jarribas(at)cttc.es
*
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_FRONT_END_CAL_H
#define GNSS_SDR_FRONT_END_CAL_H
#include <armadillo>
#include <memory>
#include <string>
class ConfigurationInterface;
class FrontEndCal
{
public:
FrontEndCal() = default;
~FrontEndCal() = default;
/*!
* \brief Sets the configuration data required by get_ephemeris function
*
*/
void set_configuration(std::shared_ptr<ConfigurationInterface> configuration);
/*!
* \brief This function connects to a Secure User Location Protocol (SUPL) server to obtain
* the current GPS ephemeris and GPS assistance data. It requires the configuration parameters set by
* set_configuration function.
*
*/
bool get_ephemeris();
/*!
* \brief This function estimates the GPS L1 satellite Doppler frequency [Hz] using the following data:
* 1- Orbital model from the ephemeris
* 2- Approximate GPS Time of Week (TOW)
* 3- Approximate receiver Latitude and Longitude (WGS-84)
*
*/
double estimate_doppler_from_eph(unsigned int PRN, double tow, double lat, double lon, double height) noexcept(false);
/*!
* \brief This function models the Elonics E4000 + RTL2832 front-end
* Inputs:
* f_bb_true_Hz - Ideal output frequency in baseband [Hz]
* f_in_bb_meas_Hz - measured output frequency in baseband [Hz]
* Outputs:
* estimated_fs_Hz - Sampling frequency estimation based on the
* measurements and the front-end model
* estimated_f_if_bb_Hz - Equivalent bb if frequency estimation based on the
* measurements and the front-end model
* Front-end TUNER Elonics E4000 + RTL2832 sampler For GPS L1 1575.42 MHz
*
*/
void GPS_L1_front_end_model_E4000(double f_bb_true_Hz, double f_bb_meas_Hz, double fs_nominal_hz, double *estimated_fs_Hz, double *estimated_f_if_Hz, double *f_osc_err_ppm);
private:
std::shared_ptr<ConfigurationInterface> configuration_;
/*
* LLA2ECEF Convert geodetic coordinates to Earth-centered Earth-fixed
* (ECEF) coordinates. P = LLA2ECEF( LLA ) converts an M-by-3 array of geodetic coordinates
* (latitude, longitude and altitude), LLA, to an M-by-3 array of ECEF
* coordinates, P. LLA is in [degrees degrees meters]. P is in meters.
* The default ellipsoid planet is WGS84. Original copyright (c) by Kai Borre.
*/
arma::vec lla2ecef(const arma::vec &lla);
/*
* GEODETIC2ECEF Convert geodetic to geocentric (ECEF) coordinates
* [X, Y, Z] = GEODETIC2ECEF(PHI, LAMBDA, H, ELLIPSOID) converts geodetic
* point locations specified by the coordinate arrays PHI (geodetic
* latitude in radians), LAMBDA (longitude in radians), and H (ellipsoidal
* height) to geocentric Cartesian coordinates X, Y, and Z. The geodetic
* coordinates refer to the reference ellipsoid specified by ELLIPSOID (a
* row vector with the form [semimajor axis, eccentricity]). H must use
* the same units as the semimajor axis; X, Y, and Z will be expressed in
* these units also.
*
* The geocentric Cartesian coordinate system is fixed with respect to the
* Earth, with its origin at the center of the ellipsoid and its X-, Y-,
* and Z-axes intersecting the surface at the following points:
* PHI LAMBDA
* X-axis: 0 0 (Equator at the Prime Meridian)
* Y-axis: 0 pi/2 (Equator at 90-degrees East
* Z-axis: pi/2 0 (North Pole)
*
* A common synonym is Earth-Centered, Earth-Fixed coordinates, or ECEF.
*
* See also ECEF2GEODETIC, ECEF2LV, GEODETIC2GEOCENTRICLAT, LV2ECEF.
*
* Copyright 2004-2009 The MathWorks, Inc.
* $Revision: 1.1.6.4 $ $Date: 2009/04/15 23:34:46 $
* Reference
* ---------
* Paul R. Wolf and Bon A. Dewitt, "Elements of Photogrammetry with
* Applications in GIS," 3rd Ed., McGraw-Hill, 2000 (Appendix F-3).
*/
arma::vec geodetic2ecef(double phi, double lambda, double h, const arma::vec &ellipsoid);
/*
* Reads the ephemeris data from an external XML file
*/
bool read_assistance_from_XML();
/*
* Connects to Secure User Location Protocol (SUPL) server to obtain
* the current GPS ephemeris and GPS assistance data.
*/
int Get_SUPL_Assist();
const std::string eph_default_xml_filename = "./gps_ephemeris.xml";
};
#endif

779
utils/front-end-cal/main.cc Normal file
View File

@@ -0,0 +1,779 @@
/*!
* \file main.cc
* \brief Main file of the Front-end calibration program.
* \author Javier Arribas, 2013. jarribas(at)cttc.es
*
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef FRONT_END_CAL_VERSION
#define FRONT_END_CAL_VERSION "0.0.1"
#endif
#include "GPS_L1_CA.h" // for GPS_L1_CA_COD...
#include "concurrent_map.h"
#include "concurrent_queue.h"
#include "configuration_interface.h" // for Configuration...
#include "file_configuration.h"
#include "front_end_cal.h"
#include "gnss_block_factory.h"
#include "gnss_block_interface.h" // for GNSSBlockInte...
#include "gnss_sdr_filesystem.h"
#include "gnss_sdr_flags.h"
#include "gnss_synchro.h"
#include "gps_acq_assist.h" // for Gps_Acq_Assist
#include "gps_almanac.h"
#include "gps_ephemeris.h"
#include "gps_iono.h"
#include "gps_l1_ca_pcps_acquisition_fine_doppler.h"
#include "gps_utc_model.h"
#include "signal_source_interface.h" // for SignalSourceInterface
#include <boost/any.hpp> // for bad_any_cast
#include <boost/exception/exception.hpp>
#include <boost/lexical_cast.hpp>
#include <gnuradio/block.h> // for block
#include <gnuradio/blocks/file_sink.h>
#include <gnuradio/blocks/file_source.h>
#include <gnuradio/blocks/head.h>
#include <gnuradio/blocks/null_sink.h>
#include <gnuradio/blocks/skiphead.h>
#include <gnuradio/gr_complex.h> // for gr_complex
#include <gnuradio/io_signature.h> // for io_signature
#include <gnuradio/runtime_types.h> // for block_sptr
#include <gnuradio/top_block.h>
#include <pmt/pmt.h> // for pmt_t, to_long
#include <pmt/pmt_sugar.h> // for mp
#include <chrono>
#include <cmath> // for round
#include <cstdint>
#include <cstdlib>
#include <ctime> // for ctime
#include <exception>
#include <iomanip> // for std::setiosflags, std::setprecision
#include <iostream>
#include <map>
#include <memory>
#include <stdexcept> // for logic_error
#include <string>
#include <thread>
#include <utility>
#include <vector>
#if USE_GLOG_AND_GFLAGS
#include <gflags/gflags.h>
#include <glog/logging.h>
#else
#include <absl/flags/flag.h>
#include <absl/flags/parse.h>
#include <absl/flags/usage.h>
#include <absl/flags/usage_config.h>
#include <absl/log/globals.h>
#include <absl/log/initialize.h>
#include <absl/log/log.h>
#include <absl/log/log_sink.h>
#include <absl/log/log_sink_registry.h>
#endif
#if HAS_GENERIC_LAMBDA
#else
#include <boost/bind/bind.hpp>
#endif
#if PMT_USES_BOOST_ANY
namespace wht = boost;
#else
namespace wht = std;
#endif
#if USE_GLOG_AND_GFLAGS
#if GFLAGS_OLD_NAMESPACE
namespace gflags
{
using namespace google;
}
#endif
DECLARE_string(log_dir);
#else
class FrontEndCalLogSink : public absl::LogSink
{
public:
FrontEndCalLogSink()
{
if (!absl::GetFlag(FLAGS_log_dir).empty())
{
logfile.open(absl::GetFlag(FLAGS_log_dir) + "/front_end_cal.log");
}
else
{
logfile.open(GetTempDir() + "/front_end_cal.log");
}
}
void Send(const absl::LogEntry& entry) override
{
logfile << entry.text_message_with_prefix_and_newline() << std::flush;
}
private:
std::ofstream logfile;
};
std::string FrontEndCalVersionString() { return std::string(FRONT_END_CAL_VERSION) + "\n"; }
#endif
Concurrent_Map<Gps_Ephemeris> global_gps_ephemeris_map;
Concurrent_Map<Gps_Iono> global_gps_iono_map;
Concurrent_Map<Gps_Utc_Model> global_gps_utc_model_map;
Concurrent_Map<Gps_Almanac> global_gps_almanac_map;
Concurrent_Map<Gps_Acq_Assist> global_gps_acq_assist_map;
Concurrent_Queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
bool stop;
Concurrent_Queue<int> channel_internal_queue;
std::vector<Gnss_Synchro> gnss_sync_vector;
Gnss_Synchro gnss_synchro;
// ######## GNURADIO BLOCK MESSAGE RECEVER #########
class FrontEndCal_msg_rx;
using FrontEndCal_msg_rx_sptr = gnss_shared_ptr<FrontEndCal_msg_rx>;
FrontEndCal_msg_rx_sptr FrontEndCal_msg_rx_make();
class FrontEndCal_msg_rx : public gr::block
{
private:
friend FrontEndCal_msg_rx_sptr FrontEndCal_msg_rx_make();
void msg_handler_channel_events(const pmt::pmt_t& msg);
FrontEndCal_msg_rx();
public:
int rx_message{0};
};
FrontEndCal_msg_rx_sptr FrontEndCal_msg_rx_make()
{
return FrontEndCal_msg_rx_sptr(new FrontEndCal_msg_rx());
}
void FrontEndCal_msg_rx::msg_handler_channel_events(const pmt::pmt_t& msg)
{
try
{
int64_t message = pmt::to_long(msg);
rx_message = message;
channel_internal_queue.push(rx_message);
}
catch (const wht::bad_any_cast& e)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!\n";
rx_message = 0;
}
}
FrontEndCal_msg_rx::FrontEndCal_msg_rx()
: gr::block("FrontEndCal_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
{
this->message_port_register_in(pmt::mp("events"));
this->set_msg_handler(pmt::mp("events"),
#if HAS_GENERIC_LAMBDA
[this](auto&& PH1) { msg_handler_channel_events(PH1); });
#else
#if USE_BOOST_BIND_PLACEHOLDERS
boost::bind(&FrontEndCal_msg_rx::msg_handler_channel_events, this, boost::placeholders::_1));
#else
boost::bind(&FrontEndCal_msg_rx::msg_handler_channel_events, this, _1));
#endif
#endif
}
void wait_message()
{
while (!stop)
{
int message;
channel_internal_queue.wait_and_pop(message);
// std::cout<<"Acq message rx="<<message<< '\n';
switch (message)
{
case 1: // Positive acq
gnss_sync_vector.push_back(gnss_synchro);
// acquisition->reset();
break;
case 2: // negative acq
// acquisition->reset();
break;
case 3:
stop = true;
break;
default:
break;
}
}
}
bool front_end_capture(const std::shared_ptr<ConfigurationInterface>& configuration)
{
auto success = false;
std::string trace_step;
gr::top_block_sptr top_block;
GNSSBlockFactory block_factory;
std::shared_ptr<Concurrent_Queue<pmt::pmt_t>> queue;
queue = std::make_shared<Concurrent_Queue<pmt::pmt_t>>();
top_block = gr::make_top_block("Acquisition test");
try
{
// Note: the block_factory returns a unique_ptr (what you would get with an "auto"
// declaration), but the flowgraph uses shared_ptr. Without further understanding of why
// it should matter in this context, used shared_ptr throughout
std::shared_ptr<SignalSourceInterface> source;
std::shared_ptr<GNSSBlockInterface> conditioner;
trace_step = "creating source";
source = block_factory.GetSignalSource(configuration.get(), queue.get());
trace_step = "creating signal conditioner";
conditioner = block_factory.GetSignalConditioner(configuration.get());
trace_step = "unexpected in setup code";
gr::block_sptr sink;
sink = gr::blocks::file_sink::make(sizeof(gr_complex), "tmp_capture.dat");
// -- Find number of samples per spreading code ---
int64_t fs_in_ = configuration->property("GNSS-SDR.internal_fs_sps", 2048000);
int samples_per_code = round(fs_in_ / (GPS_L1_CA_CODE_RATE_CPS / GPS_L1_CA_CODE_LENGTH_CHIPS));
int nsamples = samples_per_code * 50;
int skip_samples = fs_in_ * 5; // skip 5 seconds
gr::block_sptr head = gr::blocks::head::make(sizeof(gr_complex), nsamples);
gr::block_sptr skiphead = gr::blocks::skiphead::make(sizeof(gr_complex), skip_samples);
trace_step = "connecting the GNU Radio blocks";
source->connect(top_block);
conditioner->connect(top_block);
top_block->connect(source->get_right_block(), 0, conditioner->get_left_block(), 0);
top_block->connect(conditioner->get_right_block(), 0, skiphead, 0);
top_block->connect(skiphead, 0, head, 0);
top_block->connect(head, 0, sink, 0);
top_block->run();
success = true;
}
catch (std::exception const& e)
{
std::cout << "Exception caught " << trace_step << ": " << e.what() << std::endl;
}
return success;
}
static time_t utc_time(int week, int64_t tow)
{
time_t t;
/* Jan 5/6 midnight 1980 - beginning of GPS time as Unix time */
t = 315964801;
/* soon week will wrap again, uh oh... */
/* TS 44.031: GPSTOW, range 0-604799.92, resolution 0.08 sec, 23-bit presentation */
/* The factor 0.08 was applied in the ephemeris SUPL class */
/* here the tow is in [s] */
t += (1024 + week) * 604800 + tow;
return t;
}
int main(int argc, char** argv)
{
try
{
const std::string intro_help(
std::string("\n RTL-SDR E4000 RF front-end center frequency and sampling rate calibration tool that uses GPS signals\n") +
"Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)\n" +
"This program comes with ABSOLUTELY NO WARRANTY;\n" +
"See COPYING file to see a copy of the General Public License\n \n");
#if USE_GLOG_AND_GFLAGS
gflags::SetUsageMessage(intro_help);
google::SetVersionString(FRONT_END_CAL_VERSION);
gflags::ParseCommandLineFlags(&argc, &argv, true);
#else
absl::FlagsUsageConfig empty_config;
empty_config.version_string = &FrontEndCalVersionString;
absl::SetFlagsUsageConfig(empty_config);
absl::SetProgramUsageMessage(intro_help);
absl::ParseCommandLine(argc, argv);
#endif
std::cout << "Initializing... Please wait.\n";
}
catch (const std::exception& e)
{
std::cerr << e.what() << '\n';
std::cout << "front-end-cal program ended.\n";
return 1;
}
#if USE_GLOG_AND_GFLAGS
google::InitGoogleLogging(argv[0]);
if (FLAGS_log_dir.empty())
#else
absl::LogSink* fecLogSink = new FrontEndCalLogSink;
absl::AddLogSink(fecLogSink);
absl::InitializeLog();
if (absl::GetFlag(FLAGS_log_dir).empty())
#endif
{
std::cout << "Logging will be done at "
<< "/tmp"
<< '\n'
<< "Use front-end-cal --log_dir=/path/to/log to change that."
<< '\n';
}
else
{
try
{
#if USE_GLOG_AND_GFLAGS
const fs::path p(FLAGS_log_dir);
#else
const fs::path p(absl::GetFlag(FLAGS_log_dir));
#endif
if (!fs::exists(p))
{
std::cout << "The path "
#if USE_GLOG_AND_GFLAGS
<< FLAGS_log_dir
#else
<< absl::GetFlag(FLAGS_log_dir)
#endif
<< " does not exist, attempting to create it"
<< '\n';
errorlib::error_code ec;
if (!fs::create_directory(p, ec))
{
#if USE_GLOG_AND_GFLAGS
std::cerr << "Could not create the " << FLAGS_log_dir << " folder. Front-end-cal program ended.\n";
gflags::ShutDownCommandLineFlags();
#else
std::cerr << "Could not create the " << absl::GetFlag(FLAGS_log_dir) << " folder. Front-end-cal program ended.\n";
#endif
return 1;
}
}
std::cout << "Logging with be done at "
#if USE_GLOG_AND_GFLAGS
<< FLAGS_log_dir << '\n';
#else
<< absl::GetFlag(FLAGS_log_dir) << '\n';
#endif
}
catch (const std::exception& e)
{
std::cerr << e.what() << '\n';
#if USE_GLOG_AND_GFLAGS
std::cerr << "Could not create the " << FLAGS_log_dir << " folder. Front-end-cal program ended.\n";
gflags::ShutDownCommandLineFlags();
#else
std::cerr << "Could not create the " << absl::GetFlag(FLAGS_log_dir) << " folder. Front-end-cal program ended.\n";
#endif
return 1;
}
}
// 0. Instantiate the FrontEnd Calibration class
try
{
FrontEndCal front_end_cal;
// 1. Load configuration parameters from config file
#if USE_GLOG_AND_GFLAGS
std::shared_ptr<ConfigurationInterface> configuration = std::make_shared<FileConfiguration>(FLAGS_config_file);
#else
std::shared_ptr<ConfigurationInterface> configuration = std::make_shared<FileConfiguration>(absl::GetFlag(FLAGS_config_file));
#endif
front_end_cal.set_configuration(configuration);
// 2. Get SUPL information from server: Ephemeris record, assistance info and TOW
try
{
if (front_end_cal.get_ephemeris() == true)
{
std::cout << "SUPL data received OK!\n";
}
else
{
std::cout << "Failure connecting to SUPL server\n";
}
}
catch (const boost::exception& e)
{
std::cout << "Failure connecting to SUPL server\n";
}
// 3. Capture some front-end samples to hard disk
try
{
if (front_end_capture(configuration))
{
std::cout << "Front-end RAW samples captured\n";
}
else
{
std::cout << "Failure capturing front-end samples\n";
}
}
catch (const boost::bad_lexical_cast& e)
{
std::cout << "Exception caught while capturing samples (bad lexical cast)\n";
}
catch (const std::exception& e)
{
std::cout << "Exception caught while capturing samples: " << e.what() << '\n';
}
catch (...)
{
std::cout << "Unexpected exception\n";
}
// 4. Setup GNU Radio flowgraph (file_source -> Acquisition_10m)
gr::top_block_sptr top_block;
top_block = gr::make_top_block("Acquisition test");
// Satellite signal definition
gnss_synchro = Gnss_Synchro();
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'G';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal, 2, 0);
gnss_synchro.PRN = 1;
int64_t fs_in_ = configuration->property("GNSS-SDR.internal_fs_sps", 2048000);
configuration->set_property("Acquisition.max_dwells", "10");
auto acquisition = std::make_shared<GpsL1CaPcpsAcquisitionFineDoppler>(configuration.get(), "Acquisition", 1, 1);
acquisition->set_channel(1);
acquisition->set_gnss_synchro(&gnss_synchro);
acquisition->set_threshold(configuration->property("Acquisition.threshold", 2.0));
acquisition->set_doppler_max(configuration->property("Acquisition.doppler_max", 10000));
acquisition->set_doppler_step(configuration->property("Acquisition.doppler_step", 250));
gr::block_sptr source;
source = gr::blocks::file_source::make(sizeof(gr_complex), "tmp_capture.dat");
#if GNURADIO_USES_STD_POINTERS
std::shared_ptr<FrontEndCal_msg_rx> msg_rx;
#else
boost::shared_ptr<FrontEndCal_msg_rx> msg_rx;
#endif
try
{
msg_rx = FrontEndCal_msg_rx_make();
}
catch (const std::exception& e)
{
std::cout << "Failure connecting the message port system: " << e.what() << '\n';
exit(0);
}
try
{
acquisition->connect(top_block);
top_block->connect(source, 0, acquisition->get_left_block(), 0);
top_block->msg_connect(acquisition->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events"));
}
catch (const std::exception& e)
{
std::cout << "Failure connecting the GNU Radio blocks: " << e.what() << '\n';
}
// 5. Run the flowgraph
// Get visible GPS satellites (positive acquisitions with Doppler measurements)
// Compute Doppler estimations
// todo: Fix the front-end cal to support new channel internal message system (no more external queues)
std::map<int, double> doppler_measurements_map;
std::map<int, double> cn0_measurements_map;
std::thread ch_thread;
// record startup time
std::chrono::time_point<std::chrono::system_clock> start;
std::chrono::time_point<std::chrono::system_clock> end;
std::chrono::duration<double> elapsed_seconds{};
start = std::chrono::system_clock::now();
bool start_msg = true;
for (unsigned int PRN = 1; PRN < 33; PRN++)
{
gnss_synchro.PRN = PRN;
acquisition->set_gnss_synchro(&gnss_synchro);
acquisition->init();
acquisition->set_local_code();
acquisition->reset();
stop = false;
try
{
ch_thread = std::thread(wait_message);
}
catch (const std::exception& e)
{
LOG(INFO) << "Exception caught (thread resource error)";
}
top_block->run();
if (start_msg == true)
{
std::cout << "Searching for GPS Satellites in L1 band...\n";
std::cout << "[";
start_msg = false;
}
if (!gnss_sync_vector.empty())
{
std::cout << " " << PRN << " ";
double doppler_measurement_hz = 0;
for (auto& it : gnss_sync_vector)
{
doppler_measurement_hz += it.Acq_doppler_hz;
}
doppler_measurement_hz = doppler_measurement_hz / gnss_sync_vector.size();
doppler_measurements_map.insert(std::pair<int, double>(PRN, doppler_measurement_hz));
}
else
{
std::cout << " . ";
}
try
{
channel_internal_queue.push(3);
}
catch (const boost::exception& e)
{
LOG(INFO) << "Exception caught while pushing to the internal queue.";
}
try
{
ch_thread.join();
}
catch (const std::exception& e)
{
LOG(INFO) << "Exception caught while joining threads.";
}
gnss_sync_vector.clear();
#if GNURADIO_USES_STD_POINTERS
std::dynamic_pointer_cast<gr::blocks::file_source>(source)->seek(0, 0);
#else
boost::dynamic_pointer_cast<gr::blocks::file_source>(source)->seek(0, 0);
#endif
std::cout.flush();
}
std::cout << "]\n";
// report the elapsed time
end = std::chrono::system_clock::now();
elapsed_seconds = end - start;
std::cout << "Total signal acquisition run time "
<< elapsed_seconds.count()
<< " [seconds]\n";
// 6. find TOW from SUPL assistance
double current_TOW = 0;
try
{
if (global_gps_ephemeris_map.size() > 0)
{
std::map<int, Gps_Ephemeris> Eph_map;
Eph_map = global_gps_ephemeris_map.get_map_copy();
current_TOW = Eph_map.begin()->second.tow;
time_t t = utc_time(Eph_map.begin()->second.WN, static_cast<int64_t>(current_TOW));
std::cout << "Reference Time:\n";
std::cout << " GPS Week: " << Eph_map.begin()->second.WN << '\n';
std::cout << " GPS TOW: " << static_cast<int64_t>(current_TOW) << " " << static_cast<int64_t>(current_TOW) * 0.08 << '\n';
std::cout << " ~ UTC: " << ctime(&t) << '\n';
std::cout << "Current TOW obtained from SUPL assistance = " << current_TOW << '\n';
}
else
{
std::cout << "Unable to get Ephemeris SUPL assistance. TOW is unknown!\n";
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
std::cout << "GNSS-SDR Front-end calibration program ended.\n";
return 0;
}
}
catch (const boost::exception& e)
{
std::cout << "Exception in getting Global ephemeris map\n";
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
std::cout << "GNSS-SDR Front-end calibration program ended.\n";
return 0;
}
// Get user position from config file (or from SUPL using GSM Cell ID)
double lat_deg = configuration->property("GNSS-SDR.init_latitude_deg", 41.0);
double lon_deg = configuration->property("GNSS-SDR.init_longitude_deg", 2.0);
double altitude_m = configuration->property("GNSS-SDR.init_altitude_m", 100);
std::cout << "Reference location (defined in config file):\n";
std::cout << "Latitude=" << lat_deg << " [º]\n";
std::cout << "Longitude=" << lon_deg << " [º]\n";
std::cout << "Altitude=" << altitude_m << " [m]\n";
if (doppler_measurements_map.empty())
{
std::cout << "Sorry, no GPS satellites detected in the front-end capture, please check the antenna setup...\n";
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
std::cout << "GNSS-SDR Front-end calibration program ended.\n";
return 0;
}
std::map<int, double> f_if_estimation_Hz_map;
std::map<int, double> f_fs_estimation_Hz_map;
std::map<int, double> f_ppm_estimation_Hz_map;
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(2) << "Doppler analysis results:\n";
std::cout << "SV ID Measured [Hz] Predicted [Hz]\n";
for (auto& it : doppler_measurements_map)
{
try
{
double doppler_estimated_hz;
doppler_estimated_hz = front_end_cal.estimate_doppler_from_eph(it.first, current_TOW, lat_deg, lon_deg, altitude_m);
std::cout << " " << it.first << " " << it.second << " " << doppler_estimated_hz << '\n';
// 7. Compute front-end IF and sampling frequency estimation
// Compare with the measurements and compute clock drift using FE model
double estimated_fs_Hz;
double estimated_f_if_Hz;
double f_osc_err_ppm;
front_end_cal.GPS_L1_front_end_model_E4000(doppler_estimated_hz, it.second, fs_in_, &estimated_fs_Hz, &estimated_f_if_Hz, &f_osc_err_ppm);
f_if_estimation_Hz_map.insert(std::pair<int, double>(it.first, estimated_f_if_Hz));
f_fs_estimation_Hz_map.insert(std::pair<int, double>(it.first, estimated_fs_Hz));
f_ppm_estimation_Hz_map.insert(std::pair<int, double>(it.first, f_osc_err_ppm));
}
catch (const std::logic_error& e)
{
std::cout << "Logic error caught: " << e.what() << '\n';
}
catch (const boost::lock_error& e)
{
std::cout << "Exception caught while reading ephemeris\n";
}
catch (const std::exception& ex)
{
std::cout << " " << it.first << " " << it.second << " (Eph not found)\n";
}
}
// FINAL FE estimations
double mean_f_if_Hz = 0;
double mean_fs_Hz = 0;
double mean_osc_err_ppm = 0;
int n_elements = f_if_estimation_Hz_map.size();
for (auto& it : f_if_estimation_Hz_map)
{
mean_f_if_Hz += it.second;
const auto est_fs = f_fs_estimation_Hz_map.find(it.first);
if (est_fs != f_fs_estimation_Hz_map.cend())
{
mean_fs_Hz += est_fs->second;
}
const auto est_ppm = f_ppm_estimation_Hz_map.find(it.first);
if (est_ppm != f_ppm_estimation_Hz_map.cend())
{
mean_osc_err_ppm += est_ppm->second;
}
}
mean_f_if_Hz /= n_elements;
mean_fs_Hz /= n_elements;
mean_osc_err_ppm /= n_elements;
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(2) << "Parameters estimation for Elonics E4000 Front-End:\n";
std::cout << "Sampling frequency =" << mean_fs_Hz << " [Hz]\n";
std::cout << "IF bias present in baseband=" << mean_f_if_Hz << " [Hz]\n";
std::cout << "Reference oscillator error =" << mean_osc_err_ppm << " [ppm]\n";
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(2)
<< "Corrected Doppler vs. Predicted\n";
std::cout << "SV ID Corrected [Hz] Predicted [Hz]\n";
for (auto& it : doppler_measurements_map)
{
try
{
double doppler_estimated_hz;
doppler_estimated_hz = front_end_cal.estimate_doppler_from_eph(it.first, current_TOW, lat_deg, lon_deg, altitude_m);
std::cout << " " << it.first << " " << it.second - mean_f_if_Hz << " " << doppler_estimated_hz << '\n';
}
catch (const std::logic_error& e)
{
std::cout << "Logic error caught: " << e.what() << '\n';
}
catch (const boost::lock_error& e)
{
std::cout << "Exception caught while reading ephemeris\n";
}
catch (const std::exception& ex)
{
std::cout << " " << it.first << " " << it.second - mean_f_if_Hz << " (Eph not found)\n";
}
}
}
catch (const std::exception& e)
{
std::cerr << "Exception: " << e.what();
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
catch (...)
{
std::cerr << "Unknown error\n";
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
std::cout << "GNSS-SDR Front-end calibration program ended.\n";
return 0;
}

View File

@@ -0,0 +1,80 @@
% Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es
% Antonio Ramos, 2018. antonio.ramos(at)cttc.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
close all;
clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs')
end
samplingFreq = 3000000; %[Hz]
plot_last_outputs=0;%1000;
channels = 1; % Number of channels
first_channel = 0; % Number of the first channel
path = '/home/javier/git/gnss-sdr/install/test_inta/'; %% CHANGE THIS PATH
for N=1:1:channels
tracking_log_path = [path 'tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch_ BY YOUR dump_filename
GNSS_tracking(N) = dll_pll_veml_read_tracking_dump(tracking_log_path);
end
% GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels
trackResults(N).status = 'T'; %fake track
if plot_last_outputs>0 && plot_last_outputs<length(GNSS_tracking(N).code_freq_hz)
start_sample=length(GNSS_tracking(N).code_freq_hz)-plot_last_outputs;
else
start_sample=1;
end
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz(start_sample:end).';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz(start_sample:end).';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error(start_sample:end).';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco(start_sample:end).';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error(start_sample:end).';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco(start_sample:end).';
trackResults(N).I_P = GNSS_tracking(N).P(start_sample:end).';
trackResults(N).Q_P = zeros(1,length(GNSS_tracking(N).P(start_sample:end)));
trackResults(N).I_VE = GNSS_tracking(N).VE(start_sample:end).';
trackResults(N).I_E = GNSS_tracking(N).E(start_sample:end).';
trackResults(N).I_L = GNSS_tracking(N).L(start_sample:end).';
trackResults(N).I_VL = GNSS_tracking(N).VL(start_sample:end).';
trackResults(N).Q_VE = zeros(1,length(GNSS_tracking(N).VE(start_sample:end)));
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E(start_sample:end)));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).L(start_sample:end)));
trackResults(N).Q_VL = zeros(1,length(GNSS_tracking(N).VL(start_sample:end)));
trackResults(N).data_I = GNSS_tracking(N).prompt_I(start_sample:end).';
trackResults(N).data_Q = GNSS_tracking(N).prompt_Q(start_sample:end).';
trackResults(N).PRN = GNSS_tracking(N).PRN(start_sample:end).';
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz(start_sample:end).';
trackResults(N).prn_start_time_s = GNSS_tracking(N).PRN_start_sample(start_sample:end)/samplingFreq;
% Use original MATLAB tracking plot function
settings.numberOfChannels = channels;
plotVEMLTracking(N, trackResults, settings)
end
%Doppler plot (optional)
% for N=1:1:channels
% figure;
% plot(trackResults(N).prn_start_time_s , GNSS_tracking(N).carrier_doppler_hz(start_sample:end).' / 1000);
% xlabel('Time(s)'); ylabel('Doppler(KHz)'); title(['Doppler frequency channel ' num2str(N)]);
% end

View File

@@ -0,0 +1,77 @@
% Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
close all;
clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs')
end
samplingFreq = 6625000; %[Hz]
channels = 8;
first_channel = 0;
code_period = 0.001;
path = '/archive/'; %% CHANGE THIS PATH
figpath = [path];
for N=1:1:channels
tracking_log_path = [path 'epl_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename
GNSS_tracking(N) = gps_l1_ca_kf_read_tracking_dump(tracking_log_path);
end
% GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels
trackResults(N).status = 'T'; %fake track
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).carrFreqRate = GNSS_tracking(N).carrier_dopplerrate_hz2.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P = GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.';
trackResults(N).I_E = GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).PRN = GNSS_tracking(N).PRN.';
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
kalmanResults(N).PRN = GNSS_tracking(N).PRN.';
kalmanResults(N).innovation = GNSS_tracking(N).carr_error.';
kalmanResults(N).state1 = GNSS_tracking(N).carr_nco.';
kalmanResults(N).state2 = GNSS_tracking(N).carrier_doppler_hz.';
kalmanResults(N).state3 = GNSS_tracking(N).carrier_dopplerrate_hz2.';
kalmanResults(N).r_noise_cov = GNSS_tracking(N).carr_noise_sigma2.';
kalmanResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
% Use original MATLAB tracking plot function
settings.numberOfChannels = channels;
settings.msToProcess = length(GNSS_tracking(N).E);
settings.codePeriod = code_period;
settings.timeStartInSeconds = 20;
%plotTracking(N, trackResults, settings)
plotKalman(N, kalmanResults, settings)
saveas(gcf, [figpath 'epl_tracking_ch_' num2str(N) '_PRN_' num2str(trackResults(N).PRN(end)) '.png'], 'png')
end

View File

@@ -0,0 +1,64 @@
% Reads GNSS-SDR PVT dump binary file using the provided
% function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
close all;
clear all;
% True position of the antenna in UTM system (if known). Otherwise enter
% all NaN's and mean position will be used as a reference .
settings.truePosition.E = nan;
settings.truePosition.N = nan;
settings.truePosition.U = nan;
settings.navSolPeriod=100; %[ms]
filename='/home/javier/workspace/gnss-sdr/trunk/install/PVT.dat';
navSolutions = gps_l1_ca_pvt_read_pvt_dump (filename);
% Reference position for Agilent cap2.dat (San Francisco static scenario)
% Scenario latitude is 37.8194388888889 N37 49 9.98
% Scenario longitude is -122.4784944 W122 28 42.58
% Scenario elevation is 35 meters.
lat=[37 49 9.98];
long=[-122 -28 -42.58];
lat_deg=dms2deg(lat);
long_deg=dms2deg(long);
h=35;
%Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
[X, Y, Z]=geo2cart(lat, long, h, 5); % geographical to cartesian conversion
%=== Convert to UTM coordinate system =============================
utmZone = findUtmZone(lat_deg, long_deg);
[settings.truePosition.E, ...
settings.truePosition.N, ...
settings.truePosition.U] = cart2utm(X, Y, Z, utmZone);
for k=1:1:length(navSolutions.X)
[navSolutions.E(k), ...
navSolutions.N(k), ...
navSolutions.U(k)]=cart2utm(navSolutions.X(k), navSolutions.Y(k), navSolutions.Z(k), utmZone);
end
plot_skyplot=0;
plotNavigation(navSolutions,settings,plot_skyplot);

View File

@@ -0,0 +1,19 @@
% Read PVG raw dump
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%clear all;
samplingFreq = 64e6/16; %[Hz]
channels=4;
path='/home/javier/workspace/gnss-sdr-ref/trunk/install/';
pvt_raw_log_path=[path 'PVT_raw.dat'];
GNSS_PVT_raw= gps_l1_ca_read_pvt_raw_dump(channels,pvt_raw_log_path);

View File

@@ -0,0 +1,51 @@
% Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
close all;clear;
samplingFreq = 10000000; %[Hz]
channels=[0:17];
path='/home/dmiralles/Documents/gnss-sdr/build/';
addpath('libs/');
clear PRN_absolute_sample_start;
for N=1:1:length(channels)
telemetry_log_path=[path 'telemetry' num2str(channels(N)) '.dat'];
GNSS_telemetry(N)= gps_l1_ca_read_telemetry_dump(telemetry_log_path);
end
%% Plotting values
%--- Configurations
chn_num_a = 11;
chn_num_b = 3;
%--- Plot results
figure;
plot(GNSS_telemetry(chn_num_a).tracking_sample_counter, ...
GNSS_telemetry(chn_num_a).tow_current_symbol_ms/1000, 'b+');
hold on;
grid on;
plot(GNSS_telemetry(chn_num_b).tracking_sample_counter, ...
GNSS_telemetry(chn_num_b).tow_current_symbol_ms, 'ro');
xlabel('TRK Sampling Counter');
ylabel('Current Symbol TOW');
legend(['CHN-',num2str(chn_num_a-1)], ['CHN-',num2str(chn_num_b-1)]);
figure;
plot(GNSS_telemetry(chn_num_a).tracking_sample_counter, ...
GNSS_telemetry(chn_num_a).tow, 'b+');
hold on;
grid on;
plot(GNSS_telemetry(chn_num_b).tracking_sample_counter, ...
GNSS_telemetry(chn_num_b).tow, 'ro');
xlabel('TRK Sampling Counter');
ylabel('Decoded Nav TOW');
legend(['CHN-',num2str(chn_num_a-1)], ['CHN-',num2str(chn_num_b-1)]);

View File

@@ -0,0 +1,44 @@
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%help script to compare GNSS-SDR Preambles starts
channel=3;
% From GNSS_SDR telemetry decoder
% 1 find preambles indexes
preambles_index=find(GNSS_telemetry(channel).Preamble_symbol_counter==0);
% 2 Get associated timestamp ms
preambles_timestamp_sdr_ms=GNSS_telemetry(channel).prn_delay_ms(preambles_index);
% From Matlab receiver
[firstSubFrame, activeChnList, javi_subFrameStart_sample] = findPreambles(trackResults_sdr,settings);
preambles_timestamp_matlab_ms=trackResults_sdr(channel).prn_delay_ms(javi_subFrameStart_sample(channel,1:6));
%Compare
common_start_index=max(find(abs(preambles_timestamp_sdr_ms-preambles_timestamp_matlab_ms(1))<2000));
error_ms=preambles_timestamp_sdr_ms(common_start_index:(common_start_index+length(preambles_timestamp_matlab_ms)-1))-preambles_timestamp_matlab_ms.'
% figure
% stem(tracking_loop_start+javi_subFrameStart_sample(channel,:),1000*trackResults_sdr(channel).absoluteSample(javi_subFrameStart_sample(channel,:))/settings.samplingFreq);
%
% hold on;
%
% plot(GNSS_observables.preamble_delay_ms(channel,:));
%
% plot(GNSS_observables.prn_delay_ms(channel,:),'r')

View File

@@ -0,0 +1,20 @@
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
% compare pseudoranges
close all;
% GNSS SDR
plot(GNSS_PVT_raw.tx_time(1,1:300).'-200/settings.samplingFreq,GNSS_PVT_raw.Pseudorange_m(1,1:300).')
% MATLAB
hold on;
plot(navSolutions.transmitTime,navSolutions.channel.rawP(1,:),'g')

View File

@@ -0,0 +1,125 @@
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% Read Hibrid Observables Dump
clearvars;
close all;
addpath('./libs');
samplingFreq = 25000000; %[Hz]
channels=10;
path='/home/dmiralles/Documents/gnss-sdr/';
observables_log_path=[path 'observables.dat'];
GNSS_observables= read_hybrid_observables_dump(channels,observables_log_path);
%% Plo data
%--- optional: search all channels having good satellite simultaneously
min_tow_idx=1;
obs_idx=1;
for n=1:1:channels
idx=find(GNSS_observables.valid(n,:)>0,1,'first');
if min_tow_idx<idx
min_tow_idx=idx;
obs_idx = n;
end
end
%--- plot observables from that index
figure;
plot(GNSS_observables.RX_time(obs_idx,min_tow_idx+1:end),GNSS_observables.Pseudorange_m(:,min_tow_idx+1:end)');
grid on;
xlabel('TOW [s]')
ylabel('Pseudorange [m]');
figure;
plot(GNSS_observables.RX_time(obs_idx,min_tow_idx+1:end),GNSS_observables.Carrier_phase_hz(:,min_tow_idx+1:end)');
xlabel('TOW [s]')
ylabel('Accumulated Carrier Phase [cycles]');
grid on;
figure;
plot(GNSS_observables.RX_time(obs_idx,min_tow_idx+1:end),GNSS_observables.Carrier_Doppler_hz(:,min_tow_idx+1:end)');
xlabel('TOW [s]');
ylabel('Doppler Frequency [Hz]');
grid on;
%% Deprecated Code
% %read true obs from simulator (optional)
% GPS_STARTOFFSET_s = 68.802e-3;
%
% true_observables_log_path='/home/javier/git/gnss-sdr/build/obs_out.bin';
% GNSS_true_observables= read_true_sim_observables_dump(true_observables_log_path);
%
% %correct the clock error using true values (it is not possible for a receiver to correct
% %the receiver clock offset error at the observables level because it is required the
% %decoding of the ephemeris data and solve the PVT equations)
%
% SPEED_OF_LIGHT_M_S = 299792458.0;
%
% %find the reference satellite
% [~,ref_sat_ch]=min(GNSS_observables.Pseudorange_m(:,min_idx+1));
% shift_time_s=GNSS_true_observables.Pseudorange_m(ref_sat_ch,:)/SPEED_OF_LIGHT_M_S-GPS_STARTOFFSET_s;
%
%
% %Compute deltas if required and interpolate to measurement time
% delta_true_psudorange_m=GNSS_true_observables.Pseudorange_m(1,:)-GNSS_true_observables.Pseudorange_m(2,:);
% delta_true_interp_psudorange_m=interp1(GNSS_true_observables.RX_time(1,:)-shift_time_s, ...
% delta_true_psudorange_m,GNSS_observables.RX_time(1,min_idx+1:end),'lineal','extrap');
% true_interp_acc_carrier_phase_ch1_hz=interp1(GNSS_true_observables.RX_time(1,:)-shift_time_s, ...
% GNSS_true_observables.Carrier_phase_hz(1,:),GNSS_observables.RX_time(1,min_idx+1:end),'lineal','extrap');
% true_interp_acc_carrier_phase_ch2_hz=interp1(GNSS_true_observables.RX_time(1,:)-shift_time_s, ...
% GNSS_true_observables.Carrier_phase_hz(2,:),GNSS_observables.RX_time(2,min_idx+1:end),'lineal','extrap');
%
% %Compute measurement errors
%
% delta_measured_psudorange_m=GNSS_observables.Pseudorange_m(1,min_idx+1:end)-GNSS_observables.Pseudorange_m(2,min_idx+1:end);
% psudorange_error_m=delta_measured_psudorange_m-delta_true_interp_psudorange_m;
% psudorange_rms_m=sqrt(sum(psudorange_error_m.^2)/length(psudorange_error_m))
%
% acc_carrier_error_ch1_hz=GNSS_observables.Carrier_phase_hz(1,min_idx+1:end)-true_interp_acc_carrier_phase_ch1_hz...
% -GNSS_observables.Carrier_phase_hz(1,min_idx+1)+true_interp_acc_carrier_phase_ch1_hz(1);
%
% acc_phase_rms_ch1_hz=sqrt(sum(acc_carrier_error_ch1_hz.^2)/length(acc_carrier_error_ch1_hz))
%
% acc_carrier_error_ch2_hz=GNSS_observables.Carrier_phase_hz(2,min_idx+1:end)-true_interp_acc_carrier_phase_ch2_hz...
% -GNSS_observables.Carrier_phase_hz(2,min_idx+1)+true_interp_acc_carrier_phase_ch2_hz(1);
% acc_phase_rms_ch2_hz=sqrt(sum(acc_carrier_error_ch2_hz.^2)/length(acc_carrier_error_ch2_hz))
%
%
% %plot results
% figure;
% plot(GNSS_true_observables.RX_time(1,:),delta_true_psudorange_m,'g');
% hold on;
% plot(GNSS_observables.RX_time(1,min_idx+1:end),delta_measured_psudorange_m,'b');
% title('TRUE vs. measured Pseudoranges [m]')
% xlabel('TOW [s]')
% ylabel('[m]');
%
% figure;
% plot(GNSS_observables.RX_time(1,min_idx+1:end),psudorange_error_m)
% title('Pseudoranges error [m]')
% xlabel('TOW [s]')
% ylabel('[m]');
%
% figure;
% plot(GNSS_observables.RX_time(1,min_idx+1:end),acc_carrier_error_ch1_hz)
% title('Accumulated carrier phase error CH1 [hz]')
% xlabel('TOW [s]')
% ylabel('[hz]');
%
% figure;
% plot(GNSS_observables.RX_time(1,min_idx+1:end),acc_carrier_error_ch2_hz)
% title('Accumulated carrier phase error CH2 [hz]')
% xlabel('TOW [s]')
% ylabel('[hz]');
%
%
%
%

View File

@@ -0,0 +1,146 @@
% Usage: dll_pll_veml_read_tracking_dump (filename, [count])
%
% Read GNSS-SDR Tracking dump binary file into MATLAB.
% Opens GNSS-SDR tracking binary log file .dat and returns the contents
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Luis Esteve, 2012. luis(at)epsilon-formacion.com
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [GNSS_tracking] = dll_pll_veml_read_tracking_dump (filename, count)
m = nargchk (1,2,nargin);
num_float_vars = 19;
num_unsigned_long_int_vars = 1;
num_double_vars = 1;
num_unsigned_int_vars = 1;
if(~isempty(strfind(computer('arch'), '64')))
% 64-bit computer
double_size_bytes = 8;
unsigned_long_int_size_bytes = 8;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
else
double_size_bytes = 8;
unsigned_long_int_size_bytes = 4;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
end
skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ...
double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes;
bytes_shift = 0;
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
if unsigned_long_int_size_bytes==8
v8 = fread (f, count, 'uint64', skip_bytes_each_read - unsigned_long_int_size_bytes);
else
v8 = fread (f, count, 'uint32', skip_bytes_each_read - unsigned_long_int_size_bytes);
end
bytes_shift = bytes_shift + unsigned_long_int_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next double
v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes);
bytes_shift = bytes_shift + double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next unsigned int
v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes);
fclose (f);
GNSS_tracking.VE = v1;
GNSS_tracking.E = v2;
GNSS_tracking.P = v3;
GNSS_tracking.L = v4;
GNSS_tracking.VL = v5;
GNSS_tracking.prompt_I = v6;
GNSS_tracking.prompt_Q = v7;
GNSS_tracking.PRN_start_sample = v8;
GNSS_tracking.acc_carrier_phase_rad = v9;
GNSS_tracking.carrier_doppler_hz = v10;
GNSS_tracking.carrier_doppler_rate_hz_s = v11;
GNSS_tracking.code_freq_hz = v12;
GNSS_tracking.code_freq_rate_hz_s = v13;
GNSS_tracking.carr_error = v14;
GNSS_tracking.carr_nco = v15;
GNSS_tracking.code_error = v16;
GNSS_tracking.code_nco = v17;
GNSS_tracking.CN0_SNV_dB_Hz = v18;
GNSS_tracking.carrier_lock_test = v19;
GNSS_tracking.var1 = v20;
GNSS_tracking.var2 = v21;
GNSS_tracking.PRN = v22;
end

View File

@@ -0,0 +1,60 @@
function [phi, lambda, h] = cart2geo(X, Y, Z, i)
% CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
% coordinates (phi, lambda, h) on a selected reference ellipsoid.
%
% [phi, lambda, h] = cart2geo(X, Y, Z, i);
%
% Choices i of Reference Ellipsoid for Geographical Coordinates
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
a = [6378388 6378160 6378135 6378137 6378137];
f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563];
lambda = atan2(Y,X);
ex2 = (2-f(i))*f(i)/((1-f(i))^2);
c = a(i)*sqrt(1+ex2);
phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i))));
h = 0.1; oldh = 0;
iterations = 0;
while abs(h-oldh) > 1.e-12
oldh = h;
N = c/sqrt(1+ex2*cos(phi)^2);
phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i))*f(i)*N/(N+h)))));
h = sqrt(X^2+Y^2)/cos(phi)-N;
iterations = iterations + 1;
if iterations > 100
fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh);
break;
end
end
phi = phi*180/pi;
% b = zeros(1,3);
% b(1,1) = fix(phi);
% b(2,1) = fix(rem(phi,b(1,1))*60);
% b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600;
lambda = lambda*180/pi;
% l = zeros(1,3);
% l(1,1) = fix(lambda);
% l(2,1) = fix(rem(lambda,l(1,1))*60);
% l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600;
%fprintf('\n phi =%3.0f %3.0f %8.5f',b(1),b(2),b(3))
%fprintf('\n lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3))
%fprintf('\n h =%14.3f\n',h)
%%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,176 @@
function [E, N, U] = cart2utm(X, Y, Z, zone)
% CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'.
%
% [E, N, U] = cart2utm(X, Y, Z, zone);
%
% Inputs:
% X,Y,Z - Cartesian coordinates. Coordinates are referenced
% with respect to the International Terrestrial Reference
% Frame 1996 (ITRF96)
% zone - UTM zone of the given position
%
% Outputs:
% E, N, U - UTM coordinates (Easting, Northing, Uping)
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
% This implementation is based upon
% O. Andersson & K. Poder (1981) Koordinattransformationer
% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
% Vol. 30: 552--571 and Vol. 31: 76
%
% An excellent, general reference (KW) is
% R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
% h\"oheren Geod\"asie und Kartographie.
% Erster Band, Springer Verlag
% Explanation of variables used:
% f flattening of ellipsoid
% a semi major axis in m
% m0 1 - scale at central meridian; for UTM 0.0004
% Q_n normalized meridian quadrant
% E0 Easting of central meridian
% L0 Longitude of central meridian
% bg constants for ellipsoidal geogr. to spherical geogr.
% gb constants for spherical geogr. to ellipsoidal geogr.
% gtu constants for ellipsoidal N, E to spherical N, E
% utg constants for spherical N, E to ellipoidal N, E
% tolutm tolerance for utm, 1.2E-10*meridian quadrant
% tolgeo tolerance for geographical, 0.00040 second of arc
% B, L refer to latitude and longitude. Southern latitude is negative
% International ellipsoid of 1924, valid for ED50
a = 6378388;
f = 1/297;
ex2 = (2-f)*f / ((1-f)^2);
c = a * sqrt(1+ex2);
vec = [X; Y; Z-4.5];
alpha = .756e-6;
R = [ 1 -alpha 0;
alpha 1 0;
0 0 1];
trans = [89.5; 93.8; 127.6];
scale = 0.9999988;
v = scale*R*vec + trans; % coordinate vector in ED50
L = atan2(v(2), v(1));
N1 = 6395000; % preliminary value
B = atan2(v(3)/((1-f)^2*N1), norm(v(1:2))/N1); % preliminary value
U = 0.1; oldU = 0;
iterations = 0;
while abs(U-oldU) > 1.e-4
oldU = U;
N1 = c/sqrt(1+ex2*(cos(B))^2);
B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) );
U = norm(v(1:2))/cos(B)-N1;
iterations = iterations + 1;
if iterations > 100
fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU);
break;
end
end
% Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
m0 = 0.0004;
n = f / (2-f);
m = n^2 * (1/4 + n*n/64);
w = (a*(-n-m0+m*(1-m0))) / (1+n);
Q_n = a + w;
% Easting and longitude of central meridian
E0 = 500000;
L0 = (zone-30)*6 - 3;
% Check tolerance for reverse transformation
tolutm = pi/2 * 1.2e-10 * Q_n;
tolgeo = 0.000040;
% Coefficients of trigonometric series
% ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52)
% bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45))));
% bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9)));
% bg[3] = n^3*(-26/15 + n*34/21);
% bg[4] = n^4*1237/630;
% spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62)
% gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45)));
% gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45)));
% gb[3] = n^3*(56/15 + n*(-136/35));
% gb[4] = n^4*4279/630;
% spherical to ellipsoidal N, E, KW p. 196, (69)
% gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180)));
% gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440));
% gtu[3] = n^3*(61/240 + n*(-103/140));
% gtu[4] = n^4*49561/161280;
% ellipsoidal to spherical N, E, KW p. 194, (65)
% utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360)));
% utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440));
% utg[3] = n^3*(-17/480 + n*37/840);
% utg[4] = n^4*(-4397/161280);
% With f = 1/297 we get
bg = [-3.37077907e-3;
4.73444769e-6;
-8.29914570e-9;
1.58785330e-11];
gb = [ 3.37077588e-3;
6.62769080e-6;
1.78718601e-8;
5.49266312e-11];
gtu = [ 8.41275991e-4;
7.67306686e-7;
1.21291230e-9;
2.48508228e-12];
utg = [-8.41276339e-4;
-5.95619298e-8;
-1.69485209e-10;
-2.20473896e-13];
% Ellipsoidal latitude, longitude to spherical latitude, longitude
neg_geo = 'FALSE';
if B < 0
neg_geo = 'TRUE ';
end
Bg_r = abs(B);
[res_clensin] = clsin(bg, 4, 2*Bg_r);
Bg_r = Bg_r + res_clensin;
L0 = L0*pi / 180;
Lg_r = L - L0;
% Spherical latitude, longitude to complementary spherical latitude
% i.e. spherical N, E
cos_BN = cos(Bg_r);
Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN);
Ep = atanh(sin(Lg_r) * cos_BN);
%Spherical normalized N, E to ellipsoidal N, E
Np = 2 * Np;
Ep = 2 * Ep;
[dN, dE] = clksin(gtu, 4, Np, Ep);
Np = Np/2;
Ep = Ep/2;
Np = Np + dN;
Ep = Ep + dE;
N = Q_n * Np;
E = Q_n*Ep + E0;
if neg_geo == 'TRUE '
N = -N + 20000000;
end;
%%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,29 @@
function corrTime = check_t(time)
% CHECK_T accounting for beginning or end of week crossover.
%
% corrTime = check_t(time);
%
% Inputs:
% time - time in seconds
%
% Outputs:
% corrTime - corrected time (seconds)
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
half_week = 302400; % seconds
corrTime = time;
if time > half_week
corrTime = time - 2*half_week;
elseif time < -half_week
corrTime = time + 2*half_week;
end
%%%%%%% end check_t.m %%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,36 @@
function [re, im] = clksin(ar, degree, arg_real, arg_imag)
% Clenshaw summation of sinus with complex argument
% [re, im] = clksin(ar, degree, arg_real, arg_imag);
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
sin_arg_r = sin(arg_real);
cos_arg_r = cos(arg_real);
sinh_arg_i = sinh(arg_imag);
cosh_arg_i = cosh(arg_imag);
r = 2 * cos_arg_r * cosh_arg_i;
i =-2 * sin_arg_r * sinh_arg_i;
hr1 = 0; hr = 0; hi1 = 0; hi = 0;
for t = degree : -1 : 1
hr2 = hr1;
hr1 = hr;
hi2 = hi1;
hi1 = hi;
z = ar(t) + r*hr1 - i*hi - hr2;
hi = i*hr1 + r*hi1 - hi2;
hr = z;
end
r = sin_arg_r * cosh_arg_i;
i = cos_arg_r * sinh_arg_i;
re = r*hr - i*hi;
im = r*hi + i*hr;

View File

@@ -0,0 +1,25 @@
function result = clsin(ar, degree, argument)
% Clenshaw summation of sinus of argument.
%
% result = clsin(ar, degree, argument);
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
cos_arg = 2 * cos(argument);
hr1 = 0;
hr = 0;
for t = degree : -1 : 1
hr2 = hr1;
hr1 = hr;
hr = ar(t) + cos_arg*hr1 - hr2;
end
result = hr * sin(argument);
%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,48 @@
function dmsOutput = deg2dms(deg)
% DEG2DMS Conversion of degrees to degrees, minutes, and seconds.
% The output format (dms format) is: (degrees*100 + minutes + seconds/100)
% February 7, 2001
% Updated by Darius Plausinaitis
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%%% Save the sign for later processing
neg_arg = false;
if deg < 0
% Only positive numbers should be used while splitting into deg/min/sec
deg = -deg;
neg_arg = true;
end
%%% Split degrees minutes and seconds
int_deg = floor(deg);
decimal = deg - int_deg;
min_part = decimal*60;
min = floor(min_part);
sec_part = min_part - floor(min_part);
sec = sec_part*60;
%%% Check for overflow
if sec == 60
min = min + 1;
sec = 0;
end
if min == 60
int_deg = int_deg + 1;
min = 0;
end
%%% Construct the output
dmsOutput = int_deg * 100 + min + sec/100;
%%% Correct the sign
if neg_arg == true
dmsOutput = -dmsOutput;
end
%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,15 @@
function deg = dms2deg(dms)
% DMS2DEG Conversion of degrees, minutes, and seconds to degrees.
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%if (dms(1)>=0)
deg=dms(1)+dms(2)/60+dms(3)/3600;
%else
%deg=dms(1)-dms(2)/60-dms(3)/3600;
%end

View File

@@ -0,0 +1,109 @@
function [dout,mout,sout] = dms2mat(dms,n)
% DMS2MAT Converts a dms vector format to a [deg min sec] matrix
%
% [d,m,s] = DMS2MAT(dms) converts a dms vector format to a
% deg:min:sec matrix. The vector format is dms = 100*deg + min + sec/100.
% This allows compressed dms data to be expanded to a d,m,s triple,
% for easier reporting and viewing of the data.
%
% [d,m,s] = DMS2MAT(dms,n) uses n digits in the accuracy of the
% seconds calculation. n = -2 uses accuracy in the hundredths position,
% n = 0 uses accuracy in the units position. Default is n = -5.
% For further discussion of the input n, see ROUNDN.
%
% mat = DMS2MAT(...) returns a single output argument of mat = [d m s].
% This is useful only if the input dms is a single column vector.
%
% See also MAT2DMS
% Written by: E. Byrns, E. Brown
% Revision: 1.10 $Date: 2002/03/20 21:25:06
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% SPDX-License-Identifier: GPL-3.0-or-later
if nargin == 0
error('Incorrect number of arguments')
elseif nargin == 1
n = -5;
end
% Test for empty arguments
if isempty(dms); dout = []; mout = []; sout = []; return; end
% Test for complex arguments
if ~isreal(dms)
warning('Imaginary parts of complex ANGLE argument ignored')
dms = real(dms);
end
% Don't let seconds be rounded beyond the tens place.
% If you did, then 55 seconds rounds to 100, which is not good.
if n == 2; n = 1; end
% Construct a sign vector which has +1 when dms >= 0 and -1 when dms < 0.
signvec = sign(dms);
signvec = signvec + (signvec == 0); % Ensure +1 when dms = 0
% Decompress the dms data vector
dms = abs(dms);
d = fix(dms/100); % Degrees
m = fix(dms) - abs(100*d); % Minutes
[s,msg] = roundn(100*rem(dms,1),n); % Seconds: Truncate to roundoff error
if ~isempty(msg); error(msg); end
% Adjust for 60 seconds or 60 minutes.
% Test for seconds > 60 to allow for round-off from roundn,
% Test for minutes > 60 as a ripple effect from seconds > 60
indx = find(s >= 60);
if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = s(indx) - 60; end
indx = find(m >= 60);
if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx) - 60; end
% Data consistency checks
if any(m > 59) | any (m < 0)
error('Minutes must be >= 0 and <= 59')
elseif any(s >= 60) | any( s < 0)
error('Seconds must be >= 0 and < 60')
end
% Determine where to store the sign of the angle. It should be
% associated with the largest nonzero component of d:m:s.
dsign = signvec .* (d~=0);
msign = signvec .* (d==0 & m~=0);
ssign = signvec .* (d==0 & m==0 & s~=0);
% In the application of signs below, the comparison with 0 is used so that
% the sign vector contains only +1 and -1. Any zero occurrences causes
% data to be lost when the sign has been applied to a higher component
% of d:m:s. Use fix function to eliminate potential round-off errors.
d = ((dsign==0) + dsign).*fix(d); % Apply signs to the degrees
m = ((msign==0) + msign).*fix(m); % Apply signs to minutes
s = ((ssign==0) + ssign).*s; % Apply signs to seconds
% Set the output arguments
if nargout <= 1
dout = [d m s];
elseif nargout == 3
dout = d; mout = m; sout = s;
else
error('Invalid number of output arguments')
end

View File

@@ -0,0 +1,34 @@
function X_sat_rot = e_r_corr(traveltime, X_sat)
% E_R_CORR Returns rotated satellite ECEF coordinates due to Earth
% rotation during signal travel time
%
% X_sat_rot = e_r_corr(traveltime, X_sat);
%
% Inputs:
% travelTime - signal travel time
% X_sat - satellite's ECEF coordinates
%
% Outputs:
% X_sat_rot - rotated satellite's coordinates (ECEF)
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
Omegae_dot = 7.292115147e-5; % rad/sec
%--- Find rotation angle --------------------------------------------------
omegatau = Omegae_dot * traveltime;
%--- Make a rotation matrix -----------------------------------------------
R3 = [ cos(omegatau) sin(omegatau) 0;
-sin(omegatau) cos(omegatau) 0;
0 0 1];
%--- Do the rotation ------------------------------------------------------
X_sat_rot = R3 * X_sat;
%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,59 @@
function utmZone = findUtmZone(latitude, longitude)
% Function finds the UTM zone number for given longitude and latitude.
% The longitude value must be between -180 (180 degree West) and 180 (180
% degree East) degree. The latitude must be within -80 (80 degree South) and
% 84 (84 degree North).
%
% utmZone = findUtmZone(latitude, longitude);
%
% Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
% 15 deg 30 min).
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Darius Plausinaitis
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
%% Check value bounds =====================================================
if ((longitude > 180) || (longitude < -180))
error('Longitude value exceeds limits (-180:180).');
end
if ((latitude > 84) || (latitude < -80))
error('Latitude value exceeds limits (-80:84).');
end
%% Find zone ==============================================================
% Start at 180 deg west = -180 deg
utmZone = fix((180 + longitude)/ 6) + 1;
%% Correct zone numbers for particular areas ==============================
if (latitude > 72)
% Corrections for zones 31 33 35 37
if ((longitude >= 0) && (longitude < 9))
utmZone = 31;
elseif ((longitude >= 9) && (longitude < 21))
utmZone = 33;
elseif ((longitude >= 21) && (longitude < 33))
utmZone = 35;
elseif ((longitude >= 33) && (longitude < 42))
utmZone = 37;
end
elseif ((latitude >= 56) && (latitude < 64))
% Correction for zone 32
if ((longitude >= 3) && (longitude < 12))
utmZone = 32;
end
end

View File

@@ -0,0 +1,50 @@
function [X, Y, Z] = geo2cart(phi, lambda, h, i)
% GEO2CART Conversion of geographical coordinates (phi, lambda, h) to
% Cartesian coordinates (X, Y, Z).
%
% [X, Y, Z] = geo2cart(phi, lambda, h, i);
%
% Format for phi and lambda: [degrees minutes seconds].
% h, X, Y, and Z are in meters.
%
% Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
%
% Inputs:
% phi - geocentric latitude (format [degrees minutes seconds])
% lambda - geocentric longitude (format [degrees minutes seconds])
% h - height
% i - reference ellipsoid type
%
% Outputs:
% X, Y, Z - Cartesian coordinates (meters)
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre, 1998
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
b = phi(1) + phi(2)/60 + phi(3)/3600;
b = b*pi / 180;
l = lambda(1) + lambda(2)/60 + lambda(3)/3600;
l = l*pi / 180;
a = [6378388 6378160 6378135 6378137 6378137];
f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563];
ex2 = (2-f(i))*f(i) / ((1-f(i))^2);
c = a(i) * sqrt(1+ex2);
N = c / sqrt(1 + ex2*cos(b)^2);
X = (N+h) * cos(b) * cos(l);
Y = (N+h) * cos(b) * sin(l);
Z = ((1-f(i))^2*N + h) * sin(b);
%%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,116 @@
function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings)
% Function calculates the Least Square Solution.
%
% [pos, el, az, dop] = leastSquarePos(satpos, obs, settings);
%
% Inputs:
% satpos - Satellites positions (in ECEF system: [X; Y; Z;] -
% one column per satellite)
% obs - Observations - the pseudorange measurements to each
% satellite:
% (e.g. [20000000 21000000 .... .... .... .... ....])
% settings - receiver settings
%
% Outputs:
% pos - receiver position and receiver clock error
% (in ECEF system: [X, Y, Z, dt])
% el - Satellites elevation angles (degrees)
% az - Satellites azimuth angles (degrees)
% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%--------------------------------------------------------------------------
% Based on Kai Borre
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
%=== Initialization =======================================================
nmbOfIterations = 7;
dtr = pi/180;
pos = zeros(4, 1);
X = satpos;
nmbOfSatellites = size(satpos, 2);
A = zeros(nmbOfSatellites, 4);
omc = zeros(nmbOfSatellites, 1);
az = zeros(1, nmbOfSatellites);
el = az;
%=== Iteratively find receiver position ===================================
for iter = 1:nmbOfIterations
for i = 1:nmbOfSatellites
if iter == 1
%--- Initialize variables at the first iteration --------------
Rot_X = X(:, i);
trop = 2;
else
%--- Update equations -----------------------------------------
rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ...
(X(3, i) - pos(3))^2;
traveltime = sqrt(rho2) / settings.c ;
%--- Correct satellite position (do to earth rotation) --------
Rot_X = e_r_corr(traveltime, X(:, i));
%--- Find the elevation angel of the satellite ----------------
[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
if (settings.useTropCorr == 1)
%--- Calculate tropospheric correction --------------------
trop = tropo(sin(el(i) * dtr), ...
0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
else
% Do not calculate or apply the tropospheric corrections
trop = 0;
end
end % if iter == 1 ... ... else
%--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
%--- Construct the A matrix ---------------------------------------
A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ...
(-(Rot_X(2) - pos(2))) / obs(i) ...
(-(Rot_X(3) - pos(3))) / obs(i) ...
1 ];
end % for i = 1:nmbOfSatellites
% These lines allow the code to exit gracefully in case of any errors
if rank(A) ~= 4
pos = zeros(1, 4);
return
end
%--- Find position update ---------------------------------------------
x = A \ omc;
%--- Apply position update --------------------------------------------
pos = pos + x;
end % for iter = 1:nmbOfIterations
pos = pos';
%=== Calculate Dilution Of Precision ======================================
if nargout == 4
%--- Initialize output ------------------------------------------------
dop = zeros(1, 5);
%--- Calculate DOP ----------------------------------------------------
Q = inv(A'*A);
dop(1) = sqrt(trace(Q)); % GDOP
dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP
dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP
dop(4) = sqrt(Q(3,3)); % VDOP
dop(5) = sqrt(Q(4,4)); % TDOP
end

View File

@@ -0,0 +1,129 @@
function dmsvec = mat2dms(d,m,s,n)
% MAT2DMS Converts a [deg min sec] matrix to vector format
%
% dms = MAT2DMS(d,m,s) converts a deg:min:sec matrix into a vector
% format. The vector format is dms = 100*deg + min + sec/100.
% This allows d,m,s triple to be compressed into a single value,
% which can then be employed similar to a degree or radian vector.
% The inputs d, m and s must be of equal size. Minutes and
% second must be between 0 and 60.
%
% dms = MAT2DMS(mat) assumes and input matrix of [d m s]. This is
% useful only for single column vectors for d, m and s.
%
% dms = MAT2DMS(d,m) and dms = MAT2DMS([d m]) assume that seconds
% are zero, s = 0.
%
% dms = MAT2DMS(d,m,s,n) uses n as the accuracy of the seconds
% calculation. n = -2 uses accuracy in the hundredths position,
% n = 0 uses accuracy in the units position. Default is n = -5.
% For further discussion of the input n, see ROUNDN.
%
% See also DMS2MAT
% Written by: E. Byrns, E. Brown
% Revision: 1.10 Date: 2002/03/20 21:25:51
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% SPDX-License-Identifier: GPL-3.0-or-later
if nargin == 0
error('Incorrect number of arguments')
elseif nargin==1
if size(d,2)== 3
s = d(:,3); m = d(:,2); d = d(:,1);
elseif size(d,2)== 2
m = d(:,2); d = d(:,1); s = zeros(size(d));
elseif size(d,2) == 0
d = []; m = []; s = [];
else
error('Single input matrices must be n-by-2 or n-by-3.');
end
n = -5;
elseif nargin == 2
s = zeros(size(d));
n = -5;
elseif nargin == 3
n = -5;
end
% Test for empty arguments
if isempty(d) & isempty(m) & isempty(s); dmsvec = []; return; end
% Don't let seconds be rounded beyond the tens place.
% If you did, then 55 seconds rounds to 100, which is not good.
if n == 2; n = 1; end
% Complex argument tests
if any([~isreal(d) ~isreal(m) ~isreal(s)])
warning('Imaginary parts of complex ANGLE argument ignored')
d = real(d); m = real(m); s = real(s);
end
% Dimension and value tests
if ~isequal(size(d),size(m),size(s))
error('Inconsistent dimensions for input arguments')
elseif any(rem(d(~isnan(d)),1) ~= 0 | rem(m(~isnan(m)),1) ~= 0)
error('Degrees and minutes must be integers')
end
if any(abs(m) > 60) | any (abs(m) < 0) % Actually algorithm allows for
error('Minutes must be >= 0 and < 60') % up to exactly 60 seconds or
% 60 minutes, but the error message
elseif any(abs(s) > 60) | any(abs(s) < 0) % doesn't reflect this so that angst
error('Seconds must be >= 0 and < 60') % is minimized in the user docs
end
% Ensure that only one negative sign is present and at the correct location
if any((s<0 & m<0) | (s<0 & d<0) | (m<0 & d<0) )
error('Multiple negative entries in a DMS specification')
elseif any((s<0 & (m~=0 | d~= 0)) | (m<0 & d~=0))
error('Incorrect negative DMS specification')
end
% Construct a sign vector which has +1 when
% angle >= 0 and -1 when angle < 0. Note that the sign of the
% angle is associated with the largest nonzero component of d:m:s
negvec = (d<0) | (m<0) | (s<0);
signvec = ~negvec - negvec;
% Convert to all positive numbers. Allows for easier
% adjusting at 60 seconds and 60 minutes
d = abs(d); m = abs(m); s = abs(s);
% Truncate seconds to a specified accuracy to eliminate round-off errors
[s,msg] = roundn(s,n);
if ~isempty(msg); error(msg); end
% Adjust for 60 seconds or 60 minutes. If s > 60, this can only be
% from round-off during roundn since s > 60 is already tested above.
% This round-off effect has happened though.
indx = find(s >= 60);
if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = 0; end
% The user can not put minutes > 60 as input. However, the line
% above may create minutes > 60 (since the user can put in m == 60),
% thus, the test below includes the greater than condition.
indx = find(m >= 60);
if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx)-60; end
% Construct the dms vector format
dmsvec = signvec .* (100*d + m + s/100);

View File

@@ -0,0 +1,51 @@
function [x,msg] = roundn(x,n)
% ROUNDN Rounds input data at specified power of 10
%
% y = ROUNDN(x) rounds the input data x to the nearest hundredth.
%
% y = ROUNDN(x,n) rounds the input data x at the specified power
% of tens position. For example, n = -2 rounds the input data to
% the 10E-2 (hundredths) position.
%
% [y,msg] = ROUNDN(...) returns the text of any error condition
% encountered in the output variable msg.
%
% See also ROUND
% Written by: E. Byrns, E. Brown
% Revision: 1.9 Date: 2002/03/20 21:26:19
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% SPDX-License-Identifier: GPL-3.0-or-later
msg = []; % Initialize output
if nargin == 0
error('Incorrect number of arguments')
elseif nargin == 1
n = -2;
end
% Test for scalar n
if max(size(n)) ~= 1
msg = 'Scalar accuracy required';
if nargout < 2; error(msg); end
return
elseif ~isreal(n)
warning('Imaginary part of complex N argument ignored')
n = real(n);
end
% Compute the exponential factors for rounding at specified
% power of 10. Ensure that n is an integer.
factors = 10 ^ (fix(-n));
% Set the significant digits for the input data
x = round(x * factors) / factors;

View File

@@ -0,0 +1,143 @@
function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ...
eph, settings)
% SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for
% given ephemeris EPH. Coordinates are computed for each satellite in the
% list PRNLIST.
%[ satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings);
%
% Inputs:
% transmitTime - transmission time
% prnList - list of PRN-s to be processed
% eph - ephemerides of satellites
% settings - receiver settings
%
% Outputs:
% satPositions - position of satellites (in ECEF system [X; Y; Z;])
% satClkCorr - correction of satellite clocks
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%--------------------------------------------------------------------------
% Based on Kai Borre 04-09-96
% Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%% Initialize constants ===================================================
numOfSatellites = size(prnList, 2);
% GPS constatns
gpsPi = 3.1415926535898; % Pi used in the GPS coordinate
% system
%--- Constants for satellite position calculation -------------------------
Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s]
GM = 3.986005e14; % Universal gravitational constant times
% the mass of the Earth, [m^3/s^2]
F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)]
%% Initialize results =====================================================
satClkCorr = zeros(1, numOfSatellites);
satPositions = zeros(3, numOfSatellites);
%% Process each satellite =================================================
for satNr = 1 : numOfSatellites
prn = prnList(satNr);
%% Find initial satellite clock correction --------------------------------
%--- Find time difference ---------------------------------------------
dt = check_t(transmitTime - eph(prn).t_oc);
%--- Calculate clock correction ---------------------------------------
satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ...
eph(prn).T_GD;
time = transmitTime - satClkCorr(satNr);
%% Find satellite's position ----------------------------------------------
%Restore semi-major axis
a = eph(prn).sqrtA * eph(prn).sqrtA;
%Time correction
tk = check_t(time - eph(prn).t_oe);
%Initial mean motion
n0 = sqrt(GM / a^3);
%Mean motion
n = n0 + eph(prn).deltan;
%Mean anomaly
M = eph(prn).M_0 + n * tk;
%Reduce mean anomaly to between 0 and 360 deg
M = rem(M + 2*gpsPi, 2*gpsPi);
%Initial guess of eccentric anomaly
E = M;
%--- Iteratively compute eccentric anomaly ----------------------------
for ii = 1:10
E_old = E;
E = M + eph(prn).e * sin(E);
dE = rem(E - E_old, 2*gpsPi);
if abs(dE) < 1.e-12
% Necessary precision is reached, exit from the loop
break;
end
end
%Reduce eccentric anomaly to between 0 and 360 deg
E = rem(E + 2*gpsPi, 2*gpsPi);
%Compute relativistic correction term
dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E);
%Calculate the true anomaly
nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e);
%Compute angle phi
phi = nu + eph(prn).omega;
%Reduce phi to between 0 and 360 deg
phi = rem(phi, 2*gpsPi);
%Correct argument of latitude
u = phi + ...
eph(prn).C_uc * cos(2*phi) + ...
eph(prn).C_us * sin(2*phi);
%Correct radius
r = a * (1 - eph(prn).e*cos(E)) + ...
eph(prn).C_rc * cos(2*phi) + ...
eph(prn).C_rs * sin(2*phi);
%Correct inclination
i = eph(prn).i_0 + eph(prn).iDot * tk + ...
eph(prn).C_ic * cos(2*phi) + ...
eph(prn).C_is * sin(2*phi);
%Compute the angle between the ascending node and the Greenwich meridian
Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ...
Omegae_dot * eph(prn).t_oe;
%Reduce to between 0 and 360 deg
Omega = rem(Omega + 2*gpsPi, 2*gpsPi);
%--- Compute satellite coordinates ------------------------------------
satPositions(1, satNr) = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega);
satPositions(2, satNr) = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega);
satPositions(3, satNr) = sin(u)*r * sin(i);
%% Include relativistic correction in clock correction --------------------
satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ...
eph(prn).T_GD + dtr;
end % for satNr = 1 : numOfSatellites

View File

@@ -0,0 +1,110 @@
function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z)
% TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude,
% height given Cartesian coordinates X,Y,Z, and reference ellipsoid
% values semi-major axis (a) and the inverse of flattening (finv).
%
% [dphi, dlambda, h] = togeod(a, finv, X, Y, Z);
%
% The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
% The output units of angular quantities will be in decimal degrees
% (15.5 degrees not 15 deg 30 min). The output units of h will be the
% same as the units of X,Y,Z,a.
%
% Inputs:
% a - semi-major axis of the reference ellipsoid
% finv - inverse of flattening of the reference ellipsoid
% X,Y,Z - Cartesian coordinates
%
% Outputs:
% dphi - latitude
% dlambda - longitude
% h - height above reference ellipsoid
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: 1987 C. Goad, 1996 Kai Borre
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
h = 0;
tolsq = 1.e-10;
maxit = 10;
% compute radians-to-degree factor
rtd = 180/pi;
% compute square of eccentricity
if finv < 1.e-20
esq = 0;
else
esq = (2 - 1/finv) / finv;
end
oneesq = 1 - esq;
% first guess
% P is distance from spin axis
P = sqrt(X^2+Y^2);
% direct calculation of longitude
if P > 1.e-20
dlambda = atan2(Y,X) * rtd;
else
dlambda = 0;
end
if (dlambda < 0)
dlambda = dlambda + 360;
end
% r is distance from origin (0,0,0)
r = sqrt(P^2 + Z^2);
if r > 1.e-20
sinphi = Z/r;
else
sinphi = 0;
end
dphi = asin(sinphi);
% initial value of height = distance from origin minus
% approximate distance from origin to surface of ellipsoid
if r < 1.e-20
h = 0;
return
end
h = r - a*(1-sinphi*sinphi/finv);
% iterate
for i = 1:maxit
sinphi = sin(dphi);
cosphi = cos(dphi);
% compute radius of curvature in prime vertical direction
N_phi = a/sqrt(1-esq*sinphi*sinphi);
% compute residuals in P and Z
dP = P - (N_phi + h) * cosphi;
dZ = Z - (N_phi*oneesq + h) * sinphi;
% update height and latitude
h = h + (sinphi*dZ + cosphi*dP);
dphi = dphi + (cosphi*dZ - sinphi*dP)/(N_phi + h);
% test for convergence
if (dP*dP + dZ*dZ < tolsq)
break;
end
% Not Converged--Warn user
if i == maxit
fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
' iterations\n'], i);
end
end % for i = 1:maxit
dphi = dphi * rtd;
%%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,58 @@
function [Az, El, D] = topocent(X, dx)
% TOPOCENT Transformation of vector dx into topocentric coordinate
% system with origin at X.
% Both parameters are 3 by 1 vectors.
%
% [Az, El, D] = topocent(X, dx);
%
% Inputs:
% X - vector origin coordinates (in ECEF system [X; Y; Z;])
% dx - vector ([dX; dY; dZ;]).
%
% Outputs:
% D - vector length. Units like units of the input
% Az - azimuth from north positive clockwise, degrees
% El - elevation angle, degrees
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre 11-24-96
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
dtr = pi/180;
[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
cl = cos(lambda * dtr);
sl = sin(lambda * dtr);
cb = cos(phi * dtr);
sb = sin(phi * dtr);
F = [-sl -sb*cl cb*cl;
cl -sb*sl cb*sl;
0 cb sb];
local_vector = F' * dx;
E = local_vector(1);
N = local_vector(2);
U = local_vector(3);
hor_dis = sqrt(E^2 + N^2);
if hor_dis < 1.e-20
Az = 0;
El = 90;
else
Az = atan2(E, N)/dtr;
El = atan2(U, hor_dis)/dtr;
end
if Az < 0
Az = Az + 360;
end
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%%

View File

@@ -0,0 +1,100 @@
function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum)
% TROPO Calculation of tropospheric correction.
% The range correction ddr in m is to be subtracted from
% pseudo-ranges and carrier phases
%
% ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum);
%
% Inputs:
% sinel - sin of elevation angle of satellite
% hsta - height of station in km
% p - atmospheric pressure in mb at height hp
% tkel - surface temperature in degrees Kelvin at height htkel
% hum - humidity in % at height hhum
% hp - height of pressure measurement in km
% htkel - height of temperature measurement in km
% hhum - height of humidity measurement in km
%
% Outputs:
% ddr - range correction (meters)
%
% Reference
% Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric
% Refraction Correction Model. Paper presented at the
% American Geophysical Union Annual Fall Meeting, San
% Francisco, December 12-17
% A Matlab reimplementation of a C code from driver.
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Kai Borre 06-28-95
% SPDX-License-Identifier: GPL-3.0-or-later
%==========================================================================
a_e = 6378.137; % semi-major axis of earth ellipsoid
b0 = 7.839257e-5;
tlapse = -6.5;
tkhum = tkel + tlapse*(hhum-htkel);
atkel = 7.5*(tkhum-273.15) / (237.3+tkhum-273.15);
e0 = 0.0611 * hum * 10^atkel;
tksea = tkel - tlapse*htkel;
em = -978.77 / (2.8704e6*tlapse*1.0e-5);
tkelh = tksea + tlapse*hhum;
e0sea = e0 * (tksea/tkelh)^(4*em);
tkelp = tksea + tlapse*hp;
psea = p * (tksea/tkelp)^em;
if sinel < 0
sinel = 0;
end
tropo = 0;
done = 'FALSE';
refsea = 77.624e-6 / tksea;
htop = 1.1385e-5 / refsea;
refsea = refsea * psea;
ref = refsea * ((htop-hsta)/htop)^4;
while 1
rtop = (a_e+htop)^2 - (a_e+hsta)^2*(1-sinel^2);
% check to see if geometry is crazy
if rtop < 0
rtop = 0;
end
rtop = sqrt(rtop) - (a_e+hsta)*sinel;
a = -sinel/(htop-hsta);
b = -b0*(1-sinel^2) / (htop-hsta);
rn = zeros(8,1);
for i = 1:8
rn(i) = rtop^(i+1);
end
alpha = [2*a, 2*a^2+4*b/3, a*(a^2+3*b),...
a^4/5+2.4*a^2*b+1.2*b^2, 2*a*b*(a^2+3*b)/3,...
b^2*(6*a^2+4*b)*1.428571e-1, 0, 0];
if b^2 > 1.0e-35
alpha(7) = a*b^3/2;
alpha(8) = b^4/9;
end
dr = rtop;
dr = dr + alpha*rn;
tropo = tropo + dr*ref*1000;
if done == 'TRUE '
ddr = tropo;
break;
end
done = 'TRUE ';
refsea = (371900.0e-6/tksea-12.92e-6)/tksea;
htop = 1.1385e-5 * (1255/tksea+0.05)/refsea;
ref = refsea * e0sea * ((htop-hsta)/htop)^4;
end;
%%%%%%%%% end tropo.m %%%%%%%%%%%%%%%%%%%

View File

@@ -0,0 +1,176 @@
% Usage: gps_l1_ca_dll_pll_read_tracking_dump (filename, [count])
%
% Read GNSS-SDR Tracking dump binary file into MATLAB.
% Opens GNSS-SDR tracking binary log file .dat and returns the contents
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [GNSS_tracking] = gps_l1_ca_dll_pll_read_tracking_dump (filename, count)
m = nargchk (1,2,nargin);
num_float_vars=5;
num_unsigned_long_int_vars=1;
num_double_vars=11;
num_unsigned_int_vars=1;
double_size_bytes=8;
unsigned_long_int_size_bytes=8;
float_size_bytes=4;
long_int_size_bytes=4;
skip_bytes_each_read=float_size_bytes*num_float_vars+unsigned_long_int_size_bytes*num_unsigned_long_int_vars+double_size_bytes*num_double_vars+long_int_size_bytes*num_unsigned_int_vars;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 2)
%count = Inf;
file_stats = dir(filename);
%round num bytes to read to integer number of samples (to protect the script from binary
%dump end file transitory)
count = (file_stats.bytes - mod(file_stats.bytes,skip_bytes_each_read))/skip_bytes_each_read;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
v1 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v2 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v3 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v4 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v5 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved unsigned_long_int
v6 = fread (f, count, 'uint64',skip_bytes_each_read-unsigned_long_int_size_bytes);
bytes_shift=bytes_shift+unsigned_long_int_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v7 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v8 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v9 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v10 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v11 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v12 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v13 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v14 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v15 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v16 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v17 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved double
v18 = fread (f, count, 'uint32',skip_bytes_each_read-double_size_bytes);
fclose (f);
%%%%%%%% output vars %%%%%%%%
% // EPR
% d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
% d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
% d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
% // PROMPT I and Q (to analyze navigation symbols)
% d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
% d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
% // PRN start sample stamp
% //tmp_float=(float)d_sample_counter;
% d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
% // accumulated carrier phase
% d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
%
% // carrier and code frequency
% d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
% d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
%
% //PLL commands
% d_dump_file.write(reinterpret_cast<char*>(&carr_phase_error_secs_Ti), sizeof(double));
% d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
%
% //DLL commands
% d_dump_file.write(reinterpret_cast<char*>(&code_error_chips_Ti), sizeof(double));
% d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
%
% // CN0 and carrier lock test
% d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
% d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
%
% // AUX vars (for debug purposes)
% tmp_double = d_rem_code_phase_samples;
% d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
% tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
% d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
% // PRN
% unsigned int prn_ = d_acquisition_gnss_synchro->PRN;
% d_dump_file.write(reinterpret_cast<char*>(&prn_), sizeof(unsigned int));
E=v1;
P=v2;
L=v3;
prompt_I=v4;
prompt_Q=v5;
PRN_start_sample=v6;
acc_carrier_phase_rad=v7;
carrier_doppler_hz=v8;
code_freq_hz=v9;
carr_error=v10;
carr_nco=v11;
code_error=v12;
code_nco=v13;
CN0_SNV_dB_Hz=v14;
carrier_lock_test=v15;
var1=v16;
var2=v17;
PRN=v18;
GNSS_tracking.E=E;
GNSS_tracking.P=P;
GNSS_tracking.L=L;
GNSS_tracking.prompt_I=prompt_I;
GNSS_tracking.prompt_Q=prompt_Q;
GNSS_tracking.PRN_start_sample=PRN_start_sample;
GNSS_tracking.acc_carrier_phase_rad=acc_carrier_phase_rad;
GNSS_tracking.carrier_doppler_hz=carrier_doppler_hz;
GNSS_tracking.code_freq_hz=code_freq_hz;
GNSS_tracking.carr_error=carr_error;
GNSS_tracking.carr_nco=carr_nco;
GNSS_tracking.code_error=code_error
GNSS_tracking.code_nco=code_nco;
GNSS_tracking.CN0_SNV_dB_Hz=CN0_SNV_dB_Hz;
GNSS_tracking.carrier_lock_test=carrier_lock_test;
GNSS_tracking.d_rem_code_phase_samples=var1;
GNSS_tracking.var2=var2;
GNSS_tracking.PRN=PRN;
end

View File

@@ -0,0 +1,143 @@
% Usage: gps_l1_ca_kf_read_tracking_dump (filename, [count])
%
% Read GNSS-SDR Tracking dump binary file into MATLAB.
% Opens GNSS-SDR tracking binary log file .dat and returns the contents
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [GNSS_tracking] = gps_l1_ca_kf_read_tracking_dump (filename, count)
m = nargchk (1,2,nargin);
num_float_vars = 19;
num_unsigned_long_int_vars = 1;
num_double_vars = 1;
num_unsigned_int_vars = 1;
if(~isempty(strfind(computer('arch'), '64')))
% 64-bit computer
double_size_bytes = 8;
unsigned_long_int_size_bytes = 8;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
else
double_size_bytes = 8;
unsigned_long_int_size_bytes = 4;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
end
skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ...
double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes;
bytes_shift = 0;
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes);
bytes_shift = bytes_shift + unsigned_long_int_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next double
v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes);
bytes_shift = bytes_shift + double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next unsigned int
v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes);
fclose (f);
GNSS_tracking.VE = v1;
GNSS_tracking.E = v2;
GNSS_tracking.P = v3;
GNSS_tracking.L = v4;
GNSS_tracking.VL = v5;
GNSS_tracking.prompt_I = v6;
GNSS_tracking.prompt_Q = v7;
GNSS_tracking.PRN_start_sample = v8;
GNSS_tracking.acc_carrier_phase_rad = v9;
GNSS_tracking.carrier_doppler_hz = v10;
GNSS_tracking.carrier_dopplerrate_hz2 = v11;
GNSS_tracking.code_freq_hz = v12;
GNSS_tracking.carr_error = v13;
GNSS_tracking.carr_noise_sigma2 = v14;
GNSS_tracking.carr_nco = v15;
GNSS_tracking.code_error = v16;
GNSS_tracking.code_nco = v17;
GNSS_tracking.CN0_SNV_dB_Hz = v18;
GNSS_tracking.carrier_lock_test = v19;
GNSS_tracking.var1 = v20;
GNSS_tracking.var2 = v21;
GNSS_tracking.PRN = v22;
end

View File

@@ -0,0 +1,97 @@
% Read GNSS-SDR PVT lib dump binary file into MATLAB. The resulting
% structure is compatible with the K.Borre MATLAB-based receiver.
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function [navSolutions] = gps_l1_ca_pvt_read_pvt_dump (filename, count)
%% usage: gps_l1_ca_pvt_read_pvt_dump (filename, [count])
%%
%% open GNSS-SDR PVT binary log file .dat and return the contents
%%
%
% // PVT GPS time
% tmp_double=GPS_current_time;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% // ECEF User Position East [m]
% tmp_double=mypos(0);
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% // ECEF User Position North [m]
% tmp_double=mypos(1);
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% // ECEF User Position Up [m]
% tmp_double=mypos(2);
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% // User clock offset [s]
% tmp_double=mypos(3);
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% // GEO user position Latitude [deg]
% tmp_double=d_latitude_d;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% // GEO user position Longitude [deg]
% tmp_double=d_longitude_d;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% // GEO user position Height [m]
% tmp_double=d_height_m;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
m = nargchk (1,2,nargin);
num_double_vars=8;
double_size_bytes=8;
skip_bytes_each_read=double_size_bytes*num_double_vars;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 3)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
GPS_current_time = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
ECEF_X = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
ECEF_Y = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
ECEF_Z = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Clock_Offset = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Lat = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Long = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Height = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
fclose (f);
end
navSolutions.X=ECEF_X.';
navSolutions.Y=ECEF_Y.';
navSolutions.Z=ECEF_Z.';
navSolutions.dt=Clock_Offset.';
navSolutions.latitude=Lat.';
navSolutions.longitude=Long.';
navSolutions.height=Height.';
navSolutions.TransmitTime=GPS_current_time.';

View File

@@ -0,0 +1,57 @@
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [pvt_raw] = gps_l1_ca_read_pvt_raw_dump (channels, filename, count)
%% usage: read_tracking_dat (filename, [count])
%%
%% open GNSS-SDR pvt binary log file .dat and return the contents
%%
m = nargchk (1,2,nargin);
num_double_vars=3;
double_size_bytes=8;
skip_bytes_each_read=double_size_bytes*num_double_vars*channels;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 3)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
for N=1:1:channels
pvt_raw.Pseudorange_m(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
pvt_raw.Pseudorange_symbol_shift(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
pvt_raw.tx_time(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
end
fclose (f);
%%%%%%%% output vars %%%%%%%%
% for (unsigned int i=0; i<d_nchannels ; i++)
% {
% tmp_double = in[i][0].Pseudorange_m;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = in[i][0].Pseudorange_symbol_shift;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% d_dump_file.write((char*)&d_tx_time, sizeof(double));
% }
end

View File

@@ -0,0 +1,54 @@
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [telemetry] = gps_l1_ca_read_telemetry_dump (filename, count)
%%
%% open GNSS-SDR tracking binary log file .dat and return the contents
%%
m = nargchk (1,2,nargin);
num_double_vars=3;
double_size_bytes=8;
num_int_vars=2;
int_size_bytes=4;
skip_bytes_each_read=double_size_bytes*num_double_vars+num_int_vars*int_size_bytes;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 3)
count = Inf;
end
f = fopen (filename, 'rb');
if (f < 0)
else
[x, loops_counter] = fread (f,skip_bytes_each_read);
fseek(f,0,-1);
for i=1:min(count, loops_counter),
telemetry(i).tow_current_symbol_ms = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
telemetry(i).tracking_sample_counter = fread (f, count, 'uint64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
telemetry(i).tow = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
telemetry(i).nav_simbols = fread (f, count, 'int32',skip_bytes_each_read-int_size_bytes);
bytes_shift=bytes_shift+int_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
telemetry(i).prn = fread (f, count, 'int32',skip_bytes_each_read-int_size_bytes);
bytes_shift=bytes_shift+int_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
end
fclose (f);
end

View File

@@ -0,0 +1,125 @@
function plotKalman(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list.
%
% plotTracking(channelList, trackResults, settings)
%
% Inputs:
% channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function.
% settings - receiver settings.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Darius Plausinaitis
% SPDX-License-Identifier: GPL-3.0-or-later
%--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ==============================================
for channelNr = channelList
%% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by
% this function.
figure(channelNr +200);
clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ...
num2str(trackResults(channelNr).PRN(end-1)), ...
') results']);
timeStart = settings.timeStartInSeconds;
%% Draw axes ==============================================================
% Row 1
handles(1, 1) = subplot(4, 2, 1);
handles(1, 2) = subplot(4, 2, 2);
% Row 2
handles(2, 1) = subplot(4, 2, 3);
handles(2, 2) = subplot(4, 2, 4);
% Row 3
handles(3, 1) = subplot(4, 2, [5 6]);
% Row 4
handles(4, 1) = subplot(4, 2, [7 8]);
%% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess)/1000;
%----- CNo for signal----------------------------------
plot (handles(1, 1), timeAxisInSeconds, ...
trackResults(channelNr).CNo(1:settings.msToProcess), 'b');
grid (handles(1, 1));
axis (handles(1, 1), 'tight');
xlabel(handles(1, 1), 'Time (s)');
ylabel(handles(1, 1), 'CNo (dB-Hz)');
title (handles(1, 1), 'Carrier to Noise Ratio');
%----- PLL discriminator filtered----------------------------------
plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).state1(1:settings.msToProcess), 'b');
grid (handles(1, 2));
axis (handles(1, 2), 'tight');
xlim (handles(1, 2), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(1, 2), 'Time (s)');
ylabel(handles(1, 2), 'Phase Amplitude');
title (handles(1, 2), 'Filtered Carrier Phase');
%----- Carrier Frequency --------------------------------
plot (handles(2, 1), timeAxisInSeconds(2:end), ...
trackResults(channelNr).state2(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(2, 1));
axis (handles(2, 1));
xlim (handles(2, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Freq (hz)');
title (handles(2, 1), 'Filtered Doppler Frequency');
%----- Carrier Frequency Rate --------------------------------
plot (handles(2, 2), timeAxisInSeconds(2:end), ...
trackResults(channelNr).state3(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(2, 2));
axis (handles(2, 2));
xlim (handles(2, 2), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(2, 2), 'Time (s)');
ylabel(handles(2, 2), 'Freq (hz)');
title (handles(2, 2), 'Filtered Doppler Frequency Rate');
%----- PLL discriminator unfiltered--------------------------------
plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).innovation, 'r');
grid (handles(3, 1));
axis (handles(3, 1), 'auto');
xlim (handles(3, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Raw PLL discriminator (Innovation)');
%----- PLL discriminator covariance --------------------------------
plot (handles(4, 1), timeAxisInSeconds, ...
trackResults(channelNr).r_noise_cov, 'r');
grid (handles(4, 1));
axis (handles(4, 1), 'auto');
xlim (handles(4, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(4, 1), 'Time (s)');
ylabel(handles(4, 1), 'Variance');
title (handles(4, 1), 'Estimated Noise Variance');
end % for channelNr = channelList

View File

@@ -0,0 +1,152 @@
% Function plots variations of coordinates over time and a 3D position
% plot. It plots receiver coordinates in UTM system or coordinate offsets if
% the true UTM receiver coordinates are provided.
%
% plotNavigation(navSolutions, settings)
%
% Inputs:
% navSolutions - Results from navigation solution function. It
% contains measured pseudoranges and receiver
% coordinates.
% settings - Receiver settings. The true receiver coordinates
% are contained in this structure.
% plot_skyplot - If ==1 then use satellite coordinates to plot the
% the satellite positions
% Darius Plausinaitis
% Modified by Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function plotNavigation(navSolutions, settings,plot_skyplot)
%% Plot results in the necessary data exists ==============================
if (~isempty(navSolutions))
%% If reference position is not provided, then set reference position
%% to the average position
if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ...
|| isnan(settings.truePosition.U)
%=== Compute mean values ==========================================
% Remove NaN-s or the output of the function MEAN will be NaN.
refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E)));
refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N)));
refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U)));
%Also convert geodetic coordinates to deg:min:sec vector format
meanLongitude = dms2mat(deg2dms(...
mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5);
meanLatitude = dms2mat(deg2dms(...
mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5);
LatLong_str=[num2str(meanLatitude(1)), '??', ...
num2str(meanLatitude(2)), '''', ...
num2str(meanLatitude(3)), '''''', ...
',', ...
num2str(meanLongitude(1)), '??', ...
num2str(meanLongitude(2)), '''', ...
num2str(meanLongitude(3)), '''''']
refPointLgText = ['Mean Position\newline Lat: ', ...
num2str(meanLatitude(1)), '{\circ}', ...
num2str(meanLatitude(2)), '{\prime}', ...
num2str(meanLatitude(3)), '{\prime}{\prime}', ...
'\newline Lng: ', ...
num2str(meanLongitude(1)), '{\circ}', ...
num2str(meanLongitude(2)), '{\prime}', ...
num2str(meanLongitude(3)), '{\prime}{\prime}', ...
'\newline Hgt: ', ...
num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')];
else
% compute the mean error for static receiver
mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E)));
mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N)));
mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U)));
refCoord.E = settings.truePosition.E;
refCoord.N = settings.truePosition.N;
refCoord.U = settings.truePosition.U;
error_meters=sqrt((mean_position.E-refCoord.E)^2+(mean_position.N-refCoord.N)^2+(mean_position.U-refCoord.U)^2);
refPointLgText = ['Reference Position, Mean 3D error = ' num2str(error_meters) ' [m]'];
end
figureNumber = 300;
% The 300 is chosen for more convenient handling of the open
% figure windows, when many figures are closed and reopened. Figures
% drawn or opened by the user, will not be "overwritten" by this
% function if the auto numbering is not used.
%=== Select (or create) and clear the figure ==========================
figure(figureNumber);
clf (figureNumber);
set (figureNumber, 'Name', 'Navigation solutions');
%--- Draw axes --------------------------------------------------------
handles(1, 1) = subplot(4, 2, 1 : 4);
handles(3, 1) = subplot(4, 2, [5, 7]);
handles(3, 2) = subplot(4, 2, [6, 8]);
%% Plot all figures =======================================================
%--- Coordinate differences in UTM system -----------------------------
plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ...
(navSolutions.N - refCoord.N)',...
(navSolutions.U - refCoord.U)']);
title (handles(1, 1), 'Coordinates variations in UTM system');
legend(handles(1, 1), 'E', 'N', 'U');
xlabel(handles(1, 1), ['Measurement period: ', ...
num2str(settings.navSolPeriod), 'ms']);
ylabel(handles(1, 1), 'Variations (m)');
grid (handles(1, 1));
axis (handles(1, 1), 'tight');
%--- Position plot in UTM system --------------------------------------
plot3 (handles(3, 1), navSolutions.E - refCoord.E, ...
navSolutions.N - refCoord.N, ...
navSolutions.U - refCoord.U, '+');
hold (handles(3, 1), 'on');
%Plot the reference point
plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10);
hold (handles(3, 1), 'off');
view (handles(3, 1), 0, 90);
axis (handles(3, 1), 'equal');
grid (handles(3, 1), 'minor');
legend(handles(3, 1), 'Measurements', refPointLgText);
title (handles(3, 1), 'Positions in UTM system (3D plot)');
xlabel(handles(3, 1), 'East (m)');
ylabel(handles(3, 1), 'North (m)');
zlabel(handles(3, 1), 'Upping (m)');
if (plot_skyplot==1)
%--- Satellite sky plot -----------------------------------------------
skyPlot(handles(3, 2), ...
navSolutions.channel.az, ...
navSolutions.channel.el, ...
navSolutions.channel.PRN(:, 1));
title (handles(3, 2), ['Sky plot (mean PDOP: ', ...
num2str(mean(navSolutions.DOP(2,:))), ')']);
end
else
disp('plotNavigation: No navigation data to plot.');
end % if (~isempty(navSolutions))

View File

@@ -0,0 +1,177 @@
function plotTracking(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list.
%
% plotTracking(channelList, trackResults, settings)
%
% Inputs:
% channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function.
% settings - receiver settings.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Darius Plausinaitis
% SPDX-License-Identifier: GPL-3.0-or-later
%--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ==============================================
for channelNr = channelList
%% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by
% this function.
figure(channelNr +200);
clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ...
num2str(trackResults(channelNr).PRN(end-1)), ...
') results']);
%% Draw axes ==============================================================
% Row 1
handles(1, 1) = subplot(4, 3, 1);
handles(1, 2) = subplot(4, 3, [2 3]);
% Row 2
handles(2, 1) = subplot(4, 3, 4);
handles(2, 2) = subplot(4, 3, [5 6]);
% Row 3
handles(3, 1) = subplot(4, 3, 7);
handles(3, 2) = subplot(4, 3, 8);
handles(3, 3) = subplot(4, 3, 9);
% Row 4
handles(4, 1) = subplot(4, 3, 10);
handles(4, 2) = subplot(4, 3, 11);
handles(4, 3) = subplot(4, 3, 12);
%% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess)/1000;
%----- Discrete-Time Scatter Plot ---------------------------------
plot(handles(1, 1), trackResults(channelNr).I_P,...
trackResults(channelNr).Q_P, ...
'.');
grid (handles(1, 1));
axis (handles(1, 1), 'equal');
title (handles(1, 1), 'Discrete-Time Scatter Plot');
xlabel(handles(1, 1), 'I prompt');
ylabel(handles(1, 1), 'Q prompt');
%----- Nav bits ---------------------------------------------------
plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).I_P);
grid (handles(1, 2));
title (handles(1, 2), 'Bits of the navigation message');
xlabel(handles(1, 2), 'Time (s)');
axis (handles(1, 2), 'tight');
%----- PLL discriminator unfiltered--------------------------------
plot (handles(2, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscr, 'r');
grid (handles(2, 1));
axis (handles(2, 1), 'tight');
xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Amplitude');
title (handles(2, 1), 'Raw PLL discriminator');
%----- Correlation ------------------------------------------------
plot(handles(2, 2), timeAxisInSeconds, ...
[sqrt(trackResults(channelNr).I_E.^2 + ...
trackResults(channelNr).Q_E.^2)', ...
sqrt(trackResults(channelNr).I_P.^2 + ...
trackResults(channelNr).Q_P.^2)', ...
sqrt(trackResults(channelNr).I_L.^2 + ...
trackResults(channelNr).Q_L.^2)'], ...
'-*');
grid (handles(2, 2));
title (handles(2, 2), 'Correlation results');
xlabel(handles(2, 2), 'Time (s)');
axis (handles(2, 2), 'tight');
hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ...
'$\sqrt{I_{P}^2 + Q_{P}^2}$', ...
'$\sqrt{I_{L}^2 + Q_{L}^2}$');
%set interpreter from tex to latex. This will draw \sqrt correctly
set(hLegend, 'Interpreter', 'Latex');
%----- PLL discriminator filtered----------------------------------
plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess), 'b');
grid (handles(3, 1));
axis (handles(3, 1), 'tight');
xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Filtered PLL discriminator');
%----- DLL discriminator unfiltered--------------------------------
plot (handles(3, 2), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscr, 'r');
grid (handles(3, 2));
axis (handles(3, 2), 'tight');
xlabel(handles(3, 2), 'Time (s)');
ylabel(handles(3, 2), 'Amplitude');
title (handles(3, 2), 'Raw DLL discriminator');
%----- DLL discriminator filtered----------------------------------
plot (handles(3, 3), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscrFilt, 'b');
grid (handles(3, 3));
axis (handles(3, 3), 'tight');
xlabel(handles(3, 3), 'Time (s)');
ylabel(handles(3, 3), 'Amplitude');
title (handles(3, 3), 'Filtered DLL discriminator');
%----- CNo for signal----------------------------------
plot (handles(4, 1), timeAxisInSeconds, ...
trackResults(channelNr).CNo(1:settings.msToProcess), 'b');
grid (handles(4, 1));
axis (handles(4, 1), 'tight');
xlabel(handles(4, 1), 'Time (s)');
ylabel(handles(4, 1), 'CNo (dB-Hz)');
title (handles(4, 1), 'Carrier to Noise Ratio');
%----- Carrier Frequency --------------------------------
plot (handles(4, 2), timeAxisInSeconds(2:end), ...
trackResults(channelNr).carrFreq(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(4, 2));
axis (handles(4, 2));
xlabel(handles(4, 2), 'Time (s)');
ylabel(handles(4, 2), 'Freq (hz)');
title (handles(4, 2), 'Carrier Freq');
%----- Code Frequency----------------------------------
%--- Skip sample 0 to help with results display
plot (handles(4, 3), timeAxisInSeconds(2:end), ...
trackResults(channelNr).codeFreq(2:settings.msToProcess), 'Color',[0.2 0.3 0.49]);
grid (handles(4, 3));
axis (handles(4, 3), 'tight');
xlabel(handles(4, 3), 'Time (s)');
ylabel(handles(4, 3), 'Freq (Hz)');
title (handles(4, 3), 'Code Freq');
end % for channelNr = channelList

View File

@@ -0,0 +1,152 @@
function plotVEMLTracking(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list.
%
% plotTracking(channelList, trackResults, settings)
%
% Inputs:
% channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function.
% settings - receiver settings.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Darius Plausinaitis
% SPDX-License-Identifier: GPL-3.0-or-later
%--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ==============================================
for channelNr = channelList
%% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by
% this function.
figure(channelNr +200);
clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ...
num2str(trackResults(channelNr).PRN(end-1)), ...
') results']);
%% Draw axes ==============================================================
% Row 1
handles(1, 1) = subplot(3, 3, 1);
handles(1, 2) = subplot(3, 3, [2 3]);
% Row 2
handles(2, 1) = subplot(3, 3, 4);
handles(2, 2) = subplot(3, 3, [5 6]);
% Row 3
handles(3, 1) = subplot(3, 3, 7);
handles(3, 2) = subplot(3, 3, 8);
handles(3, 3) = subplot(3, 3, 9);
%% Plot all figures =======================================================
if isfield(trackResults(channelNr), 'prn_start_time_s')
timeAxis=trackResults(channelNr).prn_start_time_s;
time_label='RX Time (s)';
else
timeAxis = (1:length(trackResults(channelNr).PRN));
time_label='Epoch';
end
%----- Discrete-Time Scatter Plot ---------------------------------
plot(handles(1, 1), trackResults(channelNr).data_I,...
trackResults(channelNr).data_Q, ...
'.');
grid (handles(1, 1));
axis (handles(1, 1), 'equal');
title (handles(1, 1), 'Discrete-Time Scatter Plot');
xlabel(handles(1, 1), 'I prompt');
ylabel(handles(1, 1), 'Q prompt');
%----- Nav bits ---------------------------------------------------
plot (handles(1, 2), timeAxis, ...
trackResults(channelNr).data_I);
grid (handles(1, 2));
title (handles(1, 2), 'Bits of the navigation message');
xlabel(handles(1, 2), time_label);
axis (handles(1, 2), 'tight');
%----- PLL discriminator unfiltered--------------------------------
plot (handles(2, 1), timeAxis, ...
trackResults(channelNr).pllDiscr, 'r');
grid (handles(2, 1));
axis (handles(2, 1), 'tight');
xlabel(handles(2, 1), time_label);
ylabel(handles(2, 1), 'Amplitude');
title (handles(2, 1), 'Raw PLL discriminator');
%----- Correlation ------------------------------------------------
plot(handles(2, 2), timeAxis, ...
[sqrt(trackResults(channelNr).I_VE.^2 + ...
trackResults(channelNr).Q_VE.^2)', ...
sqrt(trackResults(channelNr).I_E.^2 + ...
trackResults(channelNr).Q_E.^2)', ...
sqrt(trackResults(channelNr).I_P.^2 + ...
trackResults(channelNr).Q_P.^2)', ...
sqrt(trackResults(channelNr).I_L.^2 + ...
trackResults(channelNr).Q_L.^2)', ...
sqrt(trackResults(channelNr).I_VL.^2 + ...
trackResults(channelNr).Q_VL.^2)'], ...
'-*');
grid (handles(2, 2));
title (handles(2, 2), 'Correlation results');
xlabel(handles(2, 2), time_label);
axis (handles(2, 2), 'tight');
hLegend = legend(handles(2, 2), '$\sqrt{I_{VE}^2 + Q_{VE}^2}$', ...
'$\sqrt{I_{E}^2 + Q_{E}^2}$', ...
'$\sqrt{I_{P}^2 + Q_{P}^2}$', ...
'$\sqrt{I_{L}^2 + Q_{L}^2}$', ...
'$\sqrt{I_{VL}^2 + Q_{VL}^2}$');
%set interpreter from tex to latex. This will draw \sqrt correctly
set(hLegend, 'Interpreter', 'Latex');
%----- PLL discriminator filtered----------------------------------
plot (handles(3, 1), timeAxis, ...
trackResults(channelNr).pllDiscrFilt, 'b');
grid (handles(3, 1));
axis (handles(3, 1), 'tight');
xlabel(handles(3, 1), time_label);
ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Filtered PLL discriminator');
%----- DLL discriminator unfiltered--------------------------------
plot (handles(3, 2), timeAxis, ...
trackResults(channelNr).dllDiscr, 'r');
grid (handles(3, 2));
axis (handles(3, 2), 'tight');
xlabel(handles(3, 2), time_label);
ylabel(handles(3, 2), 'Amplitude');
title (handles(3, 2), 'Raw DLL discriminator');
%----- DLL discriminator filtered----------------------------------
plot (handles(3, 3), timeAxis, ...
trackResults(channelNr).dllDiscrFilt, 'b');
grid (handles(3, 3));
axis (handles(3, 3), 'tight');
xlabel(handles(3, 3), time_label);
ylabel(handles(3, 3), 'Amplitude');
title (handles(3, 3), 'Filtered DLL discriminator');
end % for channelNr = channelList

View File

@@ -0,0 +1,80 @@
% This function opens a binary file using the ibyte format, reads and
% quantizes the signal using the most significant nbits, and stores the
% quantized signal into an output file. The output file also uses the
% ibyte format.
%
% Usage: quantize_signal (infile, outfile, nbits)
%
% Inputs:
% infile - Input file name
% outfile - Output file name
% nbits - number of quantization bits
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Marc Majoral, 2020. mmajoral(at)cttc.es
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function quantize_signal (infile, outfile, nbits)
%% usage: quantize_signal (infile, outfile, nbits)
%%
%% open input signal file infile, quantize the signal using the most
%% significant nbits and write the quantized signal to outfile
%%
fileID = fopen(infile, 'rb');
fileID2 = fopen(outfile, 'wb');
% initialize loop
BlockSize = 20000000; % processing block size
NumBitsPerSample = 8; % num. bits per sample ibyte format.
NumBitsSHift = NumBitsPerSample - nbits;
DivVal = 2^NumBitsSHift;
Lim2sCompl = 2^(NumBitsPerSample - 1) - 1;
Base2sCompl = 2^NumBitsPerSample;
do = true;
data_bytes = fread(fileID, BlockSize);
while do
% 2's complement
for k=1:length(data_bytes)
if (data_bytes(k) > Lim2sCompl)
data_bytes(k) = -Base2sCompl + data_bytes(k);
end
end
% take the nbits most significant bits
data_bytes = floor(data_bytes/DivVal);
% quantization correction
data_bytes = data_bytes*2 + 1;
% 2's complement
for k=1:length(data_bytes)
if (data_bytes(k) < 0)
data_bytes(k) = Base2sCompl + data_bytes(k);
end
end
% write result
fwrite(fileID2, data_bytes);
if (size(data_bytes) < BlockSize)
do = false;
else
data_bytes = fread(fileID,BlockSize);
end
end
% close files
fclose(fileID);
fclose(fileID2);

View File

@@ -0,0 +1,47 @@
% Usage: read_complex_binary (filename, [count], [start_sample])
%
% Opens filename and returns the contents as a column vector,
% treating them as 32 bit complex numbers
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function v = read_complex_binary (filename, count, start_sample)
m = nargchk (1,2,nargin);
if (m)
%usage (m);
end
if (nargin < 2)
count = Inf;
start_sample=0;
end
if (nargin < 3)
start_sample=0;
end
f = fopen (filename, 'rb');
if (f < 0)
v = 0;
else
if (start_sample>0)
bytes_per_sample=4;
fseek(f,start_sample*bytes_per_sample,'bof');
end
t = fread (f, [2, count], 'float');
fclose (f);
v = t(1,:) + t(2,:)*i;
[r, c] = size (v);
v = reshape (v, c, r);
end

View File

@@ -0,0 +1,38 @@
% Usage: read_complex_binary (filename, [count])
%
% Opens filename and returns the contents as a column vector,
% treating them as 32 bit complex numbers
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function v = read_complex_char_binary (filename, count)
m = nargchk (1,2,nargin);
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
f = fopen (filename, 'rb');
if (f < 0)
v = 0;
else
t = fread (f, [2, count], 'int8');
fclose (f);
v = t(1,:) + t(2,:)*i;
[r, c] = size (v);
v = reshape (v, c, r);
end

View File

@@ -0,0 +1,38 @@
% Usage: read_complex_binary (filename, [count])
%
% Opens filename and returns the contents as a column vector,
% treating them as 32 bit complex numbers
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function v = read_complex_short_binary (filename, count)
m = nargchk (1,2,nargin);
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
f = fopen (filename, 'rb');
if (f < 0)
v = 0;
else
t = fread (f, [2, count], 'short');
fclose (f);
v = t(1,:) + t(2,:)*i;
[r, c] = size (v);
v = reshape (v, c, r);
end

View File

@@ -0,0 +1,77 @@
% Opens GNSS-SDR tracking binary log file .dat and returns the contents
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [observables] = read_hybrid_observables_dump (channels, filename, count)
m = nargchk (1,2,nargin);
num_double_vars=7;
double_size_bytes=8;
skip_bytes_each_read=double_size_bytes*num_double_vars*channels;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 3)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
for N=1:1:channels
observables.RX_time(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.d_TOW_at_current_symbol(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.Carrier_Doppler_hz(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.Carrier_phase_hz(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.Pseudorange_m(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.PRN(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.valid(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
end
fclose (f);
%%%%%%%% output vars %%%%%%%%
% double tmp_double;
% for (unsigned int i = 0; i < d_nchannels; i++)
% {
% tmp_double = current_gnss_synchro[i].RX_time;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_ms;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].Pseudorange_m;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].PRN;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double = current_gnss_synchro[i].Flag_valid_pseudorange;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% }
end

View File

@@ -0,0 +1,74 @@
% Usage: read_true_sim_observables_dump (filename, [count])
%
% Opens gnss-sdr-sim observables dump and reads all chennels
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas 2011
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
function [observables] = read_true_sim_observables_dump (filename, count)
m = nargchk (1,2,nargin);
channels=12; %Simulator always use 12 channels
num_double_vars=7;
double_size_bytes=8;
skip_bytes_each_read=double_size_bytes*num_double_vars*channels;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
for N=1:1:channels
observables.RX_time(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.Carrier_Doppler_hz(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.Carrier_phase_hz(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.Pseudorange_m(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.True_range_m(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.Carrier_phase_hz_v2(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
observables.PRN(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
end
fclose (f);
% %%%%%%%% output vars %%%%%%%%
% for(int i=0;i<12;i++)
% {
% d_dump_file.read((char *) &gps_time_sec[i], sizeof(double));
% d_dump_file.read((char *) &doppler_l1_hz, sizeof(double));
% d_dump_file.read((char *) &acc_carrier_phase_l1_cycles[i], sizeof(double));
% d_dump_file.read((char *) &dist_m[i], sizeof(double));
% d_dump_file.read((char *) &true_dist_m[i], sizeof(double));
% d_dump_file.read((char *) &carrier_phase_l1_cycles[i], sizeof(double));
% d_dump_file.read((char *) &prn[i], sizeof(double));
% }
end

View File

@@ -0,0 +1,140 @@
function plotTracking(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list.
%
% plotTracking(channelList, trackResults, settings)
%
% Inputs:
% channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function.
% settings - receiver settings.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Darius Plausinaitis
% SPDX-License-Identifier: GPL-3.0-or-later
%--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ==============================================
for channelNr = channelList
%% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by
% this function.
figure(channelNr +200);
clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ...
num2str(trackResults(channelNr).PRN), ...
') results']);
%% Draw axes ==============================================================
% Row 1
handles(1, 1) = subplot(3, 3, 1);
handles(1, 2) = subplot(3, 3, [2 3]);
% Row 2
handles(2, 1) = subplot(3, 3, 4);
handles(2, 2) = subplot(3, 3, [5 6]);
% Row 3
handles(3, 1) = subplot(3, 3, 7);
handles(3, 2) = subplot(3, 3, 8);
handles(3, 3) = subplot(3, 3, 9);
%% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess-1)/1000;
%----- Discrete-Time Scatter Plot ---------------------------------
plot(handles(1, 1), trackResults(channelNr).I_PN,...
trackResults(channelNr).Q_PN, ...
'.');
grid (handles(1, 1));
axis (handles(1, 1), 'equal');
title (handles(1, 1), 'Discrete-Time Scatter Plot');
xlabel(handles(1, 1), 'I prompt');
ylabel(handles(1, 1), 'Q prompt');
%----- Nav bits ---------------------------------------------------
plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).I_PN(1:settings.msToProcess-1));
grid (handles(1, 2));
title (handles(1, 2), 'Bits of the navigation message');
xlabel(handles(1, 2), 'Time (s)');
axis (handles(1, 2), 'tight');
%----- PLL discriminator unfiltered--------------------------------
plot (handles(2, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscr(1:settings.msToProcess-1), 'r');
grid (handles(2, 1));
axis (handles(2, 1), 'tight');
xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Amplitude');
title (handles(2, 1), 'Raw PLL discriminator');
%----- Correlation ------------------------------------------------
plot(handles(2, 2), timeAxisInSeconds, ...
[sqrt(trackResults(channelNr).I_E(1:settings.msToProcess-1).^2 + ...
trackResults(channelNr).Q_E(1:settings.msToProcess-1).^2)', ...
sqrt(trackResults(channelNr).I_P(1:settings.msToProcess-1).^2 + ...
trackResults(channelNr).Q_P(1:settings.msToProcess-1).^2)', ...
sqrt(trackResults(channelNr).I_L(1:settings.msToProcess-1).^2 + ...
trackResults(channelNr).Q_L(1:settings.msToProcess-1).^2)'], ...
'-*');
grid (handles(2, 2));
title (handles(2, 2), 'Correlation results');
xlabel(handles(2, 2), 'Time (s)');
axis (handles(2, 2), 'tight');
hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ...
'$\sqrt{I_{P}^2 + Q_{P}^2}$', ...
'$\sqrt{I_{L}^2 + Q_{L}^2}$');
%set interpreter from tex to latex. This will draw \sqrt correctly
set(hLegend, 'Interpreter', 'Latex');
%----- PLL discriminator filtered----------------------------------
plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscrFilt(1:settings.msToProcess-1), 'b');
grid (handles(3, 1));
axis (handles(3, 1), 'tight');
xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Filtered PLL discriminator');
%----- DLL discriminator unfiltered--------------------------------
plot (handles(3, 2), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscr(1:settings.msToProcess-1), 'r');
grid (handles(3, 2));
axis (handles(3, 2), 'tight');
xlabel(handles(3, 2), 'Time (s)');
ylabel(handles(3, 2), 'Amplitude');
title (handles(3, 2), 'Raw DLL discriminator');
%----- DLL discriminator filtered----------------------------------
plot (handles(3, 3), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscrFilt(1:settings.msToProcess-1), 'b');
grid (handles(3, 3));
axis (handles(3, 3), 'tight');
xlabel(handles(3, 3), 'Time (s)');
ylabel(handles(3, 3), 'Amplitude');
title (handles(3, 3), 'Filtered DLL discriminator');
end % for channelNr = channelList

View File

@@ -0,0 +1,125 @@
% Reads GNSS-SDR Acquisition dump .mat file using the provided
% function and plots acquisition grid of acquisition statistic of PRN sat
% Antonio Ramos, 2017. antonio.ramos(at)cttc.es
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
%% Configuration
path = '/home/dmiralles/Documents/gnss-sdr/';
file = 'bds_acq';
sat = 6;
channel = 0;
execution = 4;
% Signal:
% 1 GPS L1
% 2 GPS L2M
% 3 GPS L5
% 4 Gal. E1B
% 5 Gal. E5
% 6 Glo. 1G
% 7 Glo. 2G
% 8 BDS. B1
% 9 BDS. B3
% 10 BDS. B2a
signal_type = 8;
%%% True for light grid representation
lite_view = true;
%%% If lite_view, it sets the number of samples per chip in the graphical representation
n_samples_per_chip = 3;
d_samples_per_code = 25000;
%% Load data
switch(signal_type)
case 1
n_chips = 1023;
system = 'G';
signal = '1C';
case 2
n_chips = 10230;
system = 'G';
signal = '2S';
case 3
n_chips = 10230;
system = 'G';
signal = 'L5';
case 4
n_chips = 4092;
system = 'E';
signal = '1B';
case 5
n_chips = 10230;
system = 'E';
signal = '5X';
case 6
n_chips = 511;
system = 'R';
signal = '1G';
case 7
n_chips = 511;
system = 'R';
signal = '2G';
case 8
n_chips = 2048;
system = 'C';
signal = 'B1';
case 9
n_chips = 10230;
system = 'C';
signal = 'B3';
case 10
n_chips = 10230;
system = 'C';
signal = '5C';
end
filename = [path file '_' system '_' signal '_ch_' num2str(channel) '_' num2str(execution) '_sat_' num2str(sat) '.mat'];
load(filename);
[n_fft, n_dop_bins] = size(acq_grid);
[d_max, f_max] = find(acq_grid == max(max(acq_grid)));
freq = (0 : n_dop_bins - 1) * double(doppler_step) - double(doppler_max);
delay = (0 : n_fft - 1) / n_fft * n_chips;
%% Plot data
%--- Acquisition grid (3D)
figure(1)
if(lite_view == false)
surf(freq, delay, acq_grid, 'FaceColor', 'interp', 'LineStyle', 'none')
ylim([min(delay) max(delay)])
else
delay_interp = (0 : n_samples_per_chip * n_chips - 1) / n_samples_per_chip;
grid_interp = spline(delay, acq_grid', delay_interp)';
surf(freq, delay_interp, grid_interp, 'FaceColor', 'interp', 'LineStyle', 'none')
ylim([min(delay_interp) max(delay_interp)])
end
xlabel('Doppler shift (Hz)')
xlim([min(freq) max(freq)])
ylabel('Code delay (chips)')
zlabel('Test Statistics')
%--- Acquisition grid (2D)
figure(2)
subplot(2,1,1)
plot(freq, acq_grid(d_max, :))
xlim([min(freq) max(freq)])
xlabel('Doppler shift (Hz)')
ylabel('Test statistics')
title(['Fixed code delay to ' num2str((d_max - 1) / n_fft * n_chips) ' chips'])
subplot(2,1,2)
normalization = (d_samples_per_code^4) * input_power;
plot(delay, acq_grid(:, f_max)./normalization)
xlim([min(delay) max(delay)])
xlabel('Code delay (chips)')
ylabel('Test statistics')
title(['Doppler wipe-off = ' num2str((f_max - 1) * doppler_step - doppler_max) ' Hz'])

View File

@@ -0,0 +1,80 @@
% Reads GNSS-SDR Acquisition dump binary file using the provided
% function and plots acquisition grid of acquisition statistic of PRN sat
%
% This function analyzes a experiment performed by Luis Esteve in the framework
% of the Google Summer of Code (GSoC) 2012, with the collaboration of Javier Arribas
% and Carles Fern??ndez, related to the extension of GNSS-SDR to Galileo.
%
% Luis Esteve, 2012. luis(at)epsilon-formacion.com
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function plot_acq_grid_gsoc(sat)
file=['test_statistics_E_1C_sat_' num2str(sat) '_doppler_0.dat'];
sampling_freq_Hz=4E6
Doppler_max_Hz = 9875
Doppler_min_Hz = -10000
Doppler_step_Hz = 125
% read files
x=read_complex_binary (file);
l_y=length(x);
Doppler_axes=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz;
l_x=length(Doppler_axes);
acq_grid = zeros(l_x,l_y);
index=0;
for k=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz
index=index+1;
filename=['test_statistics_E_1C_sat_' num2str(sat) '_doppler_' num2str(k) '.dat'];
acq_grid(index,:)=abs(read_complex_binary (filename));
end
maximum_correlation_peak = max(max(acq_grid))
[fila,col]=find(acq_grid==max(max(acq_grid)));
delay_error_sps = col -1
Doppler_error_Hz = Doppler_axes(fila)
noise_grid=acq_grid;
delay_span=floor(3*sampling_freq_Hz/(1.023e6));
Doppler_span=floor(500/Doppler_step_Hz);
noise_grid(fila-Doppler_span:fila+Doppler_span,col-delay_span:col+delay_span)=0;
n=numel(noise_grid)-(2*delay_span+1)*(2*Doppler_span+1);
noise_floor= sum(sum(noise_grid))/n
Gain_dbs = 10*log10(maximum_correlation_peak/noise_floor)
%% Plot 3D FULL RESOLUTION
[X,Y] = meshgrid(Doppler_axes,1:1:l_y);
figure;
surf(X,Y,acq_grid');
xlabel('Doppler(Hz)');ylabel('Code Delay(samples)');title(['GLRT statistic of Galileo Parallel Code Phase Search Acquisition. Local replica: E1C cboc PRN ' num2str(sat)]);
end

View File

@@ -0,0 +1,121 @@
% Reads GNSS-SDR Acquisition dump binary file using the provided
% function and plot acquisition grid of acquisition statistic of PRN sat.
% CAF input must be 0 or 1 depending if the user desires to read the file
% that resolves doppler ambiguity or not.
%
% This function analyzes a experiment performed by Marc Sales in the framework
% of the Google Summer of Code (GSoC) 2014, with the collaboration of Luis Esteve, Javier Arribas
% and Carles Fernandez, related to the extension of GNSS-SDR to Galileo.
%
% Marc Sales marcsales92(at)gmail.com,
% Luis Esteve, 2014. luis(at)epsilon-formacion.com
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function plot_acq_grid_gsoc_e5(sat,CAF)
path='/home/marc/git/gnss-sdr/data/';
file=[path 'test_statistics_E5a_sat_' num2str(sat) '_doppler_0.dat'];
sampling_freq_Hz=32E6
%Doppler_max_Hz = 14875
%Doppler_min_Hz = -15000
%Doppler_step_Hz = 125
Doppler_max_Hz = 10000
Doppler_min_Hz = -10000
Doppler_step_Hz = 250
% read files
%x=read_complex_binary (file);
%x=load_complex_data(file); % complex
%l_y=length(x);
myFile = java.io.File(file);
flen = length(myFile);
l_y=flen/4;% float
Doppler_axes=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz;
l_x=length(Doppler_axes);
acq_grid = zeros(l_x,l_y);
index=0;
for k=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz
index=index+1;
filename=[path 'test_statistics_E5a_sat_' num2str(sat) '_doppler_' num2str(k) '.dat'];
fid=fopen(filename,'r');
xx=fread(fid,'float');%floats from squared correlation
%xx=load_complex_data (filename); %complex
acq_grid(index,:)=abs(xx);
end
[fila,col]=find(acq_grid==max(max(acq_grid)));
if (CAF > 0)
filename=[path 'test_statistics_E5a_sat_' num2str(sat) '_CAF.dat'];
fid=fopen(filename,'r');
xx=fread(fid,'float');%floats from squared correlation
acq_grid(:,col(1))=abs(xx);
Doppler_error_Hz = Doppler_axes(xx==max(xx))
maximum_correlation_peak = max(xx)
else
Doppler_error_Hz = Doppler_axes(fila)
maximum_correlation_peak = max(max(acq_grid))
end
delay_error_sps = col -1
noise_grid=acq_grid;
delay_span=floor(3*sampling_freq_Hz/(1.023e7));
Doppler_span=floor(500/Doppler_step_Hz);
noise_grid(fila-Doppler_span:fila+Doppler_span,col-delay_span:col+delay_span)=0;
n=numel(noise_grid)-(2*delay_span+1)*(2*Doppler_span+1);
noise_floor= sum(sum(noise_grid))/n
Gain_dbs = 10*log10(maximum_correlation_peak/noise_floor)
%% Plot 3D FULL RESOLUTION
[X,Y] = meshgrid(Doppler_axes,1:1:l_y);
figure;
surf(X,Y,acq_grid');
xlabel('Doppler(Hz)');ylabel('Code Delay(samples)');title(['GLRT statistic of Galileo Parallel Code Phase Search Acquisition. PRN ' num2str(sat)]);
end
function x=load_complex_data(file)
fid = fopen(file,'r');
%fid = fopen('signal_source.dat','r');
myFile = java.io.File(file);
flen = length(myFile);
num_samples=flen/8; % 8 bytes (2 single floats) per complex sample
for k=1:num_samples
a(1:2) = fread(fid, 2, 'float');
x(k) = a(1) + a(2)*1i;
k=k+1;
end
end

View File

@@ -0,0 +1,82 @@
% Reads GNSS-SDR Acquisition dump binary file using the provided
% function and plots acquisition grid of acquisition statistic of PRN sat
%
% This function analyzes a experiment performed by Luis Esteve in the framework
% of the Google Summer of Code (GSoC) 2012, with the collaboration of Javier Arribas
% and Carles Fernandez, related to the extension of GNSS-SDR to Galileo.
%
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
% SPDX-License-Identifier: GPL-3.0-or-later
%
% -------------------------------------------------------------------------
%
function plot_acq_grid_gsoc_glonass(sat)
file=['/archive/acquisition_R_1G_sat_' num2str(sat) '_doppler_0.dat'];
% sampling_freq_Hz=62316000
sampling_freq_Hz=6.625e6
Doppler_max_Hz = 10000
Doppler_min_Hz = -10000
Doppler_step_Hz = 250
% read files
x=read_complex_binary (file);
l_y=length(x);
Doppler_axes=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz;
l_x=length(Doppler_axes);
acq_grid = zeros(l_x,l_y);
index=0;
for k=Doppler_min_Hz:Doppler_step_Hz:Doppler_max_Hz
index=index+1;
filename=['acquisition_R_1G_sat_' num2str(sat) '_doppler_' num2str(k) '.dat'];
acq_grid(index,:)=abs(read_complex_binary (filename));
end
acq_grid = acq_grid.^2;
maximum_correlation_peak = max(max(acq_grid))
[fila,col]=find(acq_grid==max(max(acq_grid)));
delay_error_sps = col -1
Doppler_error_Hz = Doppler_axes(fila)
noise_grid=acq_grid;
delay_span=floor(3*sampling_freq_Hz/(0.511e6));
Doppler_span=floor(500/Doppler_step_Hz);
noise_grid(fila-Doppler_span:fila+Doppler_span,col-delay_span:col+delay_span)=0;
n=numel(noise_grid)-(2*delay_span+1)*(2*Doppler_span+1);
noise_floor= sum(sum(noise_grid))/n
Gain_dbs = 10*log10(maximum_correlation_peak/noise_floor)
%% Plot 3D FULL RESOLUTION
[X,Y] = meshgrid(Doppler_axes,1:1:l_y);
figure;
surf(X,Y,acq_grid');
xlabel('Doppler(Hz)');ylabel('Code Delay(samples)');title(['GLRT statistic of Glonass Parallel Code Phase Search Acquisition. Local replica: L1 cboc PRN ' num2str(sat)]);
end

View File

@@ -0,0 +1,25 @@
% plot tracking quality indicators
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-FileCopyrightText: Javier Arribas <jarribas@cttc.es>
% SPDX-License-Identifier: GPL-3.0-or-later
figure;
hold on;
title('Carrier lock test output for all the channels');
for n=1:1:length(GNSS_tracking)
plot(GNSS_tracking(n).carrier_lock_test)
plotnames{n}=['SV ' num2str(round(mean(GNSS_tracking(n).PRN)))];
end
legend(plotnames);
figure;
hold on;
title('Carrier CN0 output for all the channels');
for n=1:1:length(GNSS_tracking)
plot(GNSS_tracking(n).CN0_SNV_dB_Hz)
plotnames{n}=['SV ' num2str(round(mean(GNSS_tracking(n).PRN)))];
end
legend(plotnames);

View File

@@ -0,0 +1,57 @@
# GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
# This file is part of GNSS-SDR.
#
# SPDX-FileCopyrightText: 2021 C. Fernandez-Prades cfernandez(at)cttc.es
# SPDX-License-Identifier: BSD-3-Clause
cmake_minimum_required(VERSION 3.9...3.30)
project(nav-msg-listener CXX)
set(CMAKE_CXX_STANDARD 11)
set(NAVLISTENER_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}) # allows this to be a sub-project
set(NAVLISTENER_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR})
set(Boost_USE_STATIC_LIBS OFF)
find_package(Boost COMPONENTS system REQUIRED)
find_package(Protobuf REQUIRED)
if(${Protobuf_VERSION} VERSION_LESS "3.0.0")
message(FATAL_ERROR "Fatal error: Protocol Buffers >= v3.0.0 required.")
endif()
protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS ${NAVLISTENER_SOURCE_DIR}/nav_message.proto)
add_library(navmsg_lib ${NAVLISTENER_SOURCE_DIR}/nav_msg_udp_listener.cc ${PROTO_SRCS})
target_link_libraries(navmsg_lib
PUBLIC
Boost::boost
Boost::system
protobuf::libprotobuf
)
target_include_directories(navmsg_lib
PUBLIC
${NAVLISTENER_BINARY_DIR}
)
add_executable(nav_msg_listener ${NAVLISTENER_SOURCE_DIR}/main.cc)
target_link_libraries(nav_msg_listener PUBLIC navmsg_lib)
install(TARGETS nav_msg_listener
RUNTIME DESTINATION bin
COMPONENT "nav_msg_listener"
)
if(NOT TARGET uninstall)
configure_file(
"${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
"${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake"
IMMEDIATE @ONLY
)
add_custom_target(uninstall
COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake
)
endif()

View File

@@ -0,0 +1,103 @@
<!-- prettier-ignore-start -->
[comment]: # (
SPDX-License-Identifier: BSD-3-Clause
)
[comment]: # (
SPDX-FileCopyrightText: 2021 Carles Fernandez-Prades <carles.fernandez@cttc.es>
)
<!-- prettier-ignore-end -->
# nav_msg_listener
Simple application that retrieves decoded navigation messages produced by
GNSS-SDR and prints them in a terminal. This is only for demonstration purposes,
as a example on how to retrieve data using the `nav_message.proto` file.
# Build the software
This software requires [Boost](https://www.boost.org/) and
[Protocol Buffers](https://protobuf.dev/).
In a terminal, type:
```
$ mkdir build && cd build
$ cmake ..
$ make
```
Optionally, you can install it:
```
$ sudo make install
```
and uninstall it later with:
```
$ sudo make uninstall
```
## Usage
In order to tell GNSS-SDR to generate those messages, you need to include the
lines:
```
NavDataMonitor.enable_monitor=true
NavDataMonitor.client_addresses=127.0.0.1 ; destination IP
NavDataMonitor.port=1237 ; destination port
```
in your gnss-sdr configuration file. You can specify multiple destination
addresses, separated by underscores:
```
NavDataMonitor.client_addresses=79.154.253.31_79.154.253.32
```
Run gnss-sdr with your configuration, and at the same time, from the computer of
the client address (or another terminal from the same computer that is executing
gnss-sdr if you are using `127.0.0.1`), execute the binary as:
```
$ ./nav_msg_listener 1237
```
where `1237` needs to be the same port as in `NavDataMonitor.port`. As soon as
gnss-sdr starts to decode navigation messages, you will see them in your
terminal:
```
$ ./nav_msg_listener 1237
New Data received:
System: E
Signal: 1B
PRN: 11
TOW of last symbol [ms]: 75869044
Nav message: 000000001001010101010101010101010101010101010101010101010101010101010101010101010101010101010101010011100101110001000000
New Data received:
System: G
Signal: 1C
PRN: 16
TOW of last symbol [ms]: 75870000
Nav message: 100010111010101010101000101111000110001011001010010100011100010001000000000000000000000011010100000000101101100001000011000000000000000000000000111111101000010000110110011011000100000101111100000111100110110101000100110100100010011011101001001010011001011111111110000110000000000000000000000010001100
New Data received:
System: E
Signal: 5X
PRN: 18
TOW of last symbol [ms]: 75870260
Nav message: 0000100001111110010000010111110100011010010000100000000000000000000000000000000000000000000000000000000010101010000001001011010010100100100100100110101110110101010000100000000000000000111001011100010010100001010100001110101001001101111000000000
New Data received:
System: G
Signal: L5
PRN: 6
TOW of last symbol [ms]: 75871320
Nav message: 100010110001100011110001100010110010100111100001110100001000000110110101100101011100110111001101100001011001110110010100101110001000000010000000100000001000000010000000100000001000000010000000100000001000000010000000100000001000000010000000100000001000000010000000100000001000001010101010111110000000
```

View File

@@ -0,0 +1,35 @@
# GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
# This file is part of GNSS-SDR.
#
# SPDX-FileCopyrightText: 2011-2020 C. Fernandez-Prades cfernandez(at)cttc.es
# SPDX-License-Identifier: BSD-3-Clause
if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt")
message(FATAL_ERROR "Cannot find install manifest: \"@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt\"")
endif()
file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files)
string(REGEX REPLACE "\n" ";" files "${files}")
foreach(file ${files})
message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"")
if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}")
if(CMAKE_VERSION VERSION_LESS 3.17)
execute_process(
COMMAND @CMAKE_COMMAND@ -E remove "$ENV{DESTDIR}${file}"
OUTPUT_VARIABLE rm_out
RESULT_VARIABLE rm_retval
)
else()
execute_process(
COMMAND @CMAKE_COMMAND@ -E rm "$ENV{DESTDIR}${file}"
OUTPUT_VARIABLE rm_out
RESULT_VARIABLE rm_retval
)
endif()
if(NOT "${rm_retval}" STREQUAL 0)
message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"")
endif()
else()
message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.")
endif()
endforeach()

View File

@@ -0,0 +1,61 @@
/*!
* \file main.cc
* \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: BSD-3-Clause
*
* -----------------------------------------------------------------------------
*/
#include "nav_msg_udp_listener.h"
#include <boost/lexical_cast.hpp>
#include <iostream>
int main(int argc, char *argv[])
{
try
{
// Check command line arguments.
if (argc != 2)
{
// Print help.
std::cerr << "Usage: nav_msg_listener <port>\n";
return 1;
}
unsigned short port = boost::lexical_cast<unsigned short>(argv[1]);
Nav_Msg_Udp_Listener udp_listener(port);
std::cout << "Listening on port " << static_cast<int>(port) << ", press Control+C to exit ...\n";
while (true)
{
gnss_sdr::navMsg message;
if (udp_listener.receive_and_parse_nav_message(message))
{
udp_listener.print_message(message);
}
else
{
std::cout << "Error: the message cannot be parsed." << std::endl;
}
}
}
catch (boost::bad_lexical_cast &)
{
std::cerr << "Error: the argument " << argv[1] << " is not a valid port number.\n";
return 1;
}
catch (std::exception &e)
{
std::cerr << e.what() << '\n';
return 1;
}
return 0;
}

View File

@@ -0,0 +1,17 @@
// SPDX-License-Identifier: BSD-3-Clause
// SPDX-FileCopyrightText: 2021 Carles Fernandez-Prades <carles.fernandez@cttc.es>
syntax = "proto3";
package gnss_sdr;
message navMsg {
string system = 1; // GNSS constellation: "G" for GPS, "R" for Glonass, "E" for Galileo, and "C" for Beidou.
string signal = 2; // GNSS signal: "1C" for GPS L1 C/A, "1B" for Galileo E1b/c, "1G" for Glonass L1 C/A, "2S" for GPS L2 L2C(M), "2G" for Glonass L2 C/A, "L5" for GPS L5, "5X" for Galileo E5a, and "E6" for Galileo E6B.
int32 prn = 3; // SV ID.
int32 tow_at_current_symbol_ms = 4; // Time of week of the last symbol received, in ms
string nav_message = 5; // for Galileo I/NAV: decoded half page (even or odd), 120 bits, as described in OS SIS ICD 2.0, paragraph 4.3.2.3. I/NAV Page Part.
// for Galileo F/NAV: decoded word, 244 bits, as described in OS SIS ICD 2.0, paragraph 4.2.2. F/NAV Page Layout.
// for Galileo HAS: decoded full HAS message (header + body), variable length, as described in Galileo HAS SIS ICD.
// For GPS LNAV: decoded subframe, 300 bits, as described in IS-GPS-200M paragraph 20.3.2 Message Structure.
// For GPS CNAV: decoded subframe, 300 bits, as described in IS-GPS-200M paragraph 30.3.3 Message Content.
}

View File

@@ -0,0 +1,85 @@
/*!
* \file nav_msg_udp_listener.cc
* \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: BSD-3-Clause
*
* -----------------------------------------------------------------------------
*/
#include "nav_msg_udp_listener.h"
#include <iostream>
#include <sstream>
#include <string>
Nav_Msg_Udp_Listener::Nav_Msg_Udp_Listener(unsigned short port)
: socket{io_service}, endpoint{boost::asio::ip::udp::v4(), port}, connected_socket(true)
{
socket.open(endpoint.protocol(), error); // Open socket
if (error)
{
std::cerr << "Error opening socket: " << error.message() << '\n';
connected_socket = false;
}
if (connected_socket)
{
socket.bind(endpoint, error); // Bind the socket to the given local endpoint.
if (error)
{
std::cerr << "Error binding socket: " << error.message() << '\n';
connected_socket = false;
}
}
}
/*
* Blocking call to read nav_message from UDP port
* return true if message parsed succesfully, false ow
*/
bool Nav_Msg_Udp_Listener::receive_and_parse_nav_message(gnss_sdr::navMsg &message)
{
if (connected_socket)
{
char buff[8192]; // Buffer for storing the received data.
// This call will block until one or more bytes of data has been received.
int bytes = socket.receive(boost::asio::buffer(buff));
std::string data(&buff[0], bytes);
// Deserialize a stock of Nav_Msg objects from the binary string.
return message.ParseFromString(data);
}
return false;
}
/*
* Prints navigation message content
* param[in] message nav message to be printed
*/
void Nav_Msg_Udp_Listener::print_message(gnss_sdr::navMsg &message) const
{
if (connected_socket)
{
std::string system = message.system();
std::string signal = message.signal();
int prn = message.prn();
int tow_at_current_symbol_ms = message.tow_at_current_symbol_ms();
std::string nav_message = message.nav_message();
std::cout << "\nNew data received:\n";
std::cout << "System: " << system << '\n';
std::cout << "Signal: " << signal << '\n';
std::cout << "PRN: " << prn << '\n';
std::cout << "TOW of last symbol [ms]: "
<< tow_at_current_symbol_ms << '\n';
std::cout << "Nav message: " << nav_message << "\n\n";
}
}

View File

@@ -0,0 +1,37 @@
/*!
* \file nav_msg_udp_listener.h
* \author Carles Fernandez-Prades, 2021. cfernandez(at)cttc.es
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: BSD-3-Clause
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_NAV_MSG_UDP_LISTENER_H
#define GNSS_SDR_NAV_MSG_UDP_LISTENER_H
#include "nav_message.pb.h"
#include <boost/asio.hpp>
class Nav_Msg_Udp_Listener
{
public:
explicit Nav_Msg_Udp_Listener(unsigned short port);
void print_message(gnss_sdr::navMsg &message) const;
bool receive_and_parse_nav_message(gnss_sdr::navMsg &message);
private:
boost::asio::io_service io_service;
boost::asio::ip::udp::socket socket;
boost::system::error_code error;
boost::asio::ip::udp::endpoint endpoint;
bool connected_socket;
};
#endif

View File

@@ -0,0 +1,110 @@
"""
dll_pll_veml_plot_sample.py
Reads GNSS-SDR Tracking dump binary file using the provided function and
plots some internal variables
Irene Pérez Riega, 2023. iperrie@inta.es
Modifiable in the file:
sampling_freq - Sampling frequency [Hz]
plot_last_outputs - If 0 -> process everything / number of items processed
channels - Number of channels
first_channel - Number of the first channel
doppler_opt - = 1 -> Plot // = 0 -> No plot
path - Path to folder which contains raw files
fig_path - Path where doppler plots will be save
'trk_dump_ch' - Fixed part of the tracking dump files names
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import os
import numpy as np
import matplotlib.pyplot as plt
from lib.dll_pll_veml_read_tracking_dump import dll_pll_veml_read_tracking_dump
from lib.plotVEMLTracking import plotVEMLTracking
trackResults = []
settings = {}
GNSS_tracking = []
# ---------- CHANGE HERE:
sampling_freq = 3000000
plot_last_outputs = 0
channels = 5
first_channel = 0
doppler_opt = 1
settings['numberOfChannels'] = channels
path = '/home/labnav/Desktop/TEST_IRENE/tracking'
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/Doppler'
for N in range(1, channels+1):
tracking_log_path = os.path.join(path,
f'trk_dump_ch{N-1+first_channel}.dat')
GNSS_tracking.append(dll_pll_veml_read_tracking_dump(tracking_log_path))
# GNSS-SDR format conversion to Python GPS receiver
for N in range (1, channels+1):
if 0 < plot_last_outputs < len(GNSS_tracking[N - 1].get("code_freq_hz")):
start_sample = (len(GNSS_tracking[N-1].get("code_freq_hz")) -
plot_last_outputs)
else:
start_sample = 0
trackResult = {
'status': 'T', # fake track
'codeFreq': np.copy(GNSS_tracking[N-1]["code_freq_hz"][start_sample:]),
'carrFreq': np.copy(GNSS_tracking[N-1]["carrier_doppler_hz"][start_sample:]),
'dllDiscr': np.copy(GNSS_tracking[N-1]["code_error"][start_sample:]),
'dllDiscrFilt': np.copy(GNSS_tracking[N-1]["code_nco"][start_sample:]),
'pllDiscr': np.copy(GNSS_tracking[N-1]["carr_error"][start_sample:]),
'pllDiscrFilt': np.copy(GNSS_tracking[N-1]["carr_nco"][start_sample:]),
'I_P': np.copy(GNSS_tracking[N-1]["P"][start_sample:]),
'Q_P': np.zeros(len(GNSS_tracking[N-1]["P"][start_sample:])),
'I_VE': np.copy(GNSS_tracking[N-1]["VE"][start_sample:]),
'I_E': np.copy(GNSS_tracking[N-1]["E"][start_sample:]),
'I_L': np.copy(GNSS_tracking[N-1]["L"][start_sample:]),
'I_VL': np.copy(GNSS_tracking[N-1]["VL"][start_sample:]),
'Q_VE': np.zeros(len(GNSS_tracking[N-1]["VE"][start_sample:])),
'Q_E': np.zeros(len(GNSS_tracking[N-1]["E"][start_sample:])),
'Q_L': np.zeros(len(GNSS_tracking[N-1]["L"][start_sample:])),
'Q_VL': np.zeros(len(GNSS_tracking[N-1]["VL"][start_sample:])),
'data_I': np.copy(GNSS_tracking[N-1]["prompt_I"][start_sample:]),
'data_Q': np.copy(GNSS_tracking[N-1]["prompt_Q"][start_sample:]),
'PRN': np.copy(GNSS_tracking[N-1]["PRN"][start_sample:]),
'CNo': np.copy(GNSS_tracking[N-1]["CN0_SNV_dB_Hz"][start_sample:]),
'prn_start_time_s': np.copy(GNSS_tracking[N-1]["PRN_start_sample"]
[start_sample:]) / sampling_freq
}
trackResults.append(trackResult)
# Plot results:
plotVEMLTracking(N,trackResults,settings)
# Plot Doppler according to selected in doppler_opt variable:
if doppler_opt == 1:
if not os.path.exists(fig_path):
os.makedirs(fig_path)
plt.figure()
plt.plot(trackResults[N - 1]['prn_start_time_s'],
[x/1000 for x in GNSS_tracking[N - 1]['carrier_doppler_hz']
[start_sample:]])
plt.xlabel('Time(s)')
plt.ylabel('Doppler(KHz)')
plt.title('Doppler frequency channel ' + str(N))
plt.savefig(os.path.join(fig_path, f'Doppler_freq_ch_{N}.png'))
plt.show()

View File

@@ -0,0 +1,98 @@
"""
gps_l1_ca_kf_plot_sample.py
Reads GNSS-SDR Tracking dump binary file using the provided
function and plots some internal variables
Irene Pérez Riega, 2023. iperrie@inta.es
Modifiable in the file:
sampling_freq - Sampling frequency [Hz]
channels - Number of channels to check if they exist
first_channel - Number of the first channel
path - Path to folder which contains raw file
'trk_dump_ch' - Fixed part in tracking dump files names
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import os
import numpy as np
from lib.gps_l1_ca_kf_read_tracking_dump import gps_l1_ca_kf_read_tracking_dump
from lib.plotTracking import plotTracking
from lib.plotKalman import plotKalman
GNSS_tracking = []
trackResults = []
kalmanResults = []
# ---------- CHANGE HERE:
# Signal parameters:
samplingFreq = 6625000
channels = 5
first_channel = 0
code_period = 0.001
path = '/home/labnav/Desktop/TEST_IRENE/tracking'
for N in range(1, channels + 1):
tracking_log_path = os.path.join(path,
f'trk_dump_ch{N-1+first_channel}.dat')
GNSS_tracking.append(gps_l1_ca_kf_read_tracking_dump(tracking_log_path))
# todo lee lo mismo que dll_pll_velm_plot_sample y guarda diferente v13 y v14
# GNSS-SDR format conversion to Python GPS receiver
for N in range(1, channels + 1):
trackResult= {
'status': 'T', # fake track
'codeFreq': np.copy(GNSS_tracking[N - 1]["code_freq_hz"]),
'carrFreq': np.copy(GNSS_tracking[N - 1]["carrier_doppler_hz"]),
'carrFreqRate':
np.copy(GNSS_tracking[N - 1]["carrier_doppler_rate_hz2"]),#todo no se usa en dll, carrier_doppler_rate_hz_s segun dll
'dllDiscr': np.copy(GNSS_tracking[N - 1]["code_error"]),
'dllDiscrFilt': np.copy(GNSS_tracking[N - 1]["code_nco"]),
'pllDiscr': np.copy(GNSS_tracking[N - 1]["carr_error"]),#todo code_freq_rate_hz_s segun dll
'pllDiscrFilt': np.copy(GNSS_tracking[N - 1]["carr_nco"]),
'I_P': np.copy(GNSS_tracking[N - 1]["prompt_I"]),#todo distinto de dll
'Q_P': np.copy(GNSS_tracking[N - 1]["prompt_Q"]),#todo distinto de dll
'I_E': np.copy(GNSS_tracking[N - 1]["E"]),
'I_L': np.copy(GNSS_tracking[N - 1]["L"]),
'Q_E': np.zeros(len(GNSS_tracking[N - 1]["E"])),
'Q_L': np.zeros(len(GNSS_tracking[N - 1]["L"])),
'PRN': np.copy(GNSS_tracking[N - 1]["PRN"]),
'CNo': np.copy(GNSS_tracking[N - 1]["CN0_SNV_dB_Hz"])
}
kalmanResult= {
'PRN': np.copy(GNSS_tracking[N - 1]["PRN"]),
'innovation': np.copy(GNSS_tracking[N - 1]["carr_error"]),#todo code_freq_rate_hz_s segun dll
'state1': np.copy(GNSS_tracking[N - 1]["carr_nco"]),
'state2': np.copy(GNSS_tracking[N - 1]["carrier_doppler_hz"]),
'state3': GNSS_tracking[N - 1]["carrier_doppler_rate_hz2"],#todo segun el dll es carrier_doppler_rate_hz_s
'r_noise_cov': np.copy(GNSS_tracking[N - 1]["carr_noise_sigma2"]),#todo carr_error segun dll
'CNo': np.copy(GNSS_tracking[N - 1]["CN0_SNV_dB_Hz"])
}
trackResults.append(trackResult)
kalmanResults.append(kalmanResult)
settings = {
'numberOfChannels': channels,
'msToProcess': len(GNSS_tracking[N-1]['E']),
'codePeriod': code_period,
'timeStartInSeconds': 20
}
# Create and save graphics as PNG
plotTracking(N, trackResults, settings)
plotKalman(N, kalmanResults, settings)

View File

@@ -0,0 +1,101 @@
"""
gps_l1_ca_pvt_raw_plot_sample.py
Reads GNSS-SDR PVT raw dump binary file using the provided function and plots
some internal variables
Irene Pérez Riega, 2023. iperrie@inta.es
Modifiable in the file:
sampling_freq - Sampling frequency [Hz]
channels - Number of channels to check if they exist
path - Path to folder which contains raw file
pvt_raw_log_path - Completed path to PVT raw data file
nav_sol_period - Measurement period [ms]
plot_skyplot - = 1 -> Sky Plot (TO DO) // = 0 -> No Sky Plot
true_position - In settings, If not known enter all NaN's and mean
position will be used as a reference in UTM
coordinate system
plot_position - Optional function at the end
plot_oneVStime - Optional function at the end, select variable to plot
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import utm
import numpy as np
from lib.gps_l1_ca_read_pvt_dump import gps_l1_ca_read_pvt_dump
from lib.plotNavigation import plotNavigation
import pyproj
from lib.plotPosition import plot_position, plot_oneVStime
settings = {}
utm_e = []
utm_n = []
E_UTM = []
N_UTM = []
utm_zone = []
# ---------- CHANGE HERE:
samplingFreq = 64e6 / 16
channels = 5
path = '/home/labnav/Desktop/TEST_IRENE/'
pvt_raw_log_path = path + 'PVT.dat'
nav_sol_period = 10
plot_skyplot = 0
settings['true_position'] = {'E_UTM':np.nan,'N_UTM':np.nan,'U_UTM':np.nan}
settings['navSolPeriod'] = nav_sol_period
navSolutions = gps_l1_ca_read_pvt_dump(pvt_raw_log_path)
X, Y, Z = navSolutions['X'], navSolutions['Y'], navSolutions['Z']
utm_coords = []
for i in range(len(navSolutions['longitude'])):
utm_coords.append(utm.from_latlon(navSolutions['latitude'][i],
navSolutions['longitude'][i]))
for i in range(len(utm_coords)):
utm_e.append(utm_coords[i][0])
utm_n.append(utm_coords[i][1])
utm_zone.append(utm_coords[i][2])
# Transform from Lat Long degrees to UTM coordinate system
# It's supposed utm_zone and letter will not change during tracking
input_projection = pyproj.CRS.from_string("+proj=longlat "
"+datum=WGS84 +no_defs")
utm_e = []
utm_n = []
for i in range(len(navSolutions['longitude'])):
output_projection = pyproj.CRS (f"+proj=utm +zone={utm_zone[i]} "
f"+datum=WGS84 +units=m +no_defs")
transformer = pyproj.Transformer.from_crs(input_projection,
output_projection)
utm_e, utm_n = transformer.transform(navSolutions['longitude'][i],
navSolutions['latitude'][i])
E_UTM.append(utm_e)
N_UTM.append(utm_n)
navSolutions['E_UTM'] = E_UTM
navSolutions['N_UTM'] = N_UTM
navSolutions['U_UTM'] = navSolutions['Z']
plotNavigation(navSolutions,settings,plot_skyplot)
# OPTIONAL: Other plots ->
plot_position(navSolutions)
plot_oneVStime(navSolutions, 'X_vel')
plot_oneVStime(navSolutions, 'Tot_Vel')

View File

@@ -0,0 +1,99 @@
"""
gps_l1_ca_telemetry_plot_sample.py
Reads GNSS-SDR Tracking dump binary file using the provided function and
plots some internal variables
Irene Pérez Riega, 2023. iperrie@inta.es
Modifiable in the file:
sampling_freq - Sampling frequency [Hz]
channels - Number of channels to check if they exist
doppler_opt - = 1 -> Plot // = 0 -> No plot
path - Path to folder which contains raw file
fig_path - Path where plots will be save
chn_num_a / b - Channel which will be plotted
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import os
import matplotlib.pyplot as plt
from lib.gps_l1_ca_read_telemetry_dump import gps_l1_ca_read_telemetry_dump
GNSS_telemetry = []
# ---------- CHANGE HERE:
sampling_freq = 2000000
channels = list(range(18))
path = '/home/labnav/Desktop/TEST_IRENE/'
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/Telemetry'
if not os.path.exists(fig_path):
os.makedirs(fig_path)
i = 0
for N in channels:
try:
telemetry_log_path = os.path.join(path, f'telemetry{N}.dat')
telemetry_data = gps_l1_ca_read_telemetry_dump(telemetry_log_path)
GNSS_telemetry.append(telemetry_data)
i += 1
except:
pass
# ---------- CHANGE HERE:
chn_num_a = 0
chn_num_b = 5
# Plotting values
if chn_num_a in range(i) and chn_num_b in range(i):
# First Plot:
plt.figure()
plt.gcf().canvas.manager.set_window_title(f'Telem_Current_Simbol_TOW_'
f'{chn_num_a}_{chn_num_b}.png')
plt.plot(GNSS_telemetry[chn_num_a]['tracking_sample_counter'],
[x / 1000 for x in GNSS_telemetry[chn_num_a]
['tow_current_symbol_ms']], 'b')
plt.plot(GNSS_telemetry[chn_num_b]['tracking_sample_counter'],
GNSS_telemetry[chn_num_b]['tow_current_symbol_ms'], 'r')
plt.grid(True)
plt.xlabel('TRK Sampling Counter')
plt.ylabel('Current Symbol TOW')
plt.legend([f'CHN-{chn_num_a-1}', f'CHN-{chn_num_b-1}'])
plt.tight_layout()
plt.savefig(os.path.join(fig_path, f'Telem_Current_Simbol_TOW_{chn_num_a}'
f'_{chn_num_b}.png'))
plt.show()
# Second Plot:
plt.figure()
plt.gcf().canvas.manager.set_window_title(f'Telem_TRK_Sampling_Counter_'
f'{chn_num_a}_{chn_num_b}.png')
plt.plot(GNSS_telemetry[chn_num_a]['tracking_sample_counter'],
GNSS_telemetry[chn_num_a]['tow'], 'b')
plt.plot(GNSS_telemetry[chn_num_b]['tracking_sample_counter'],
GNSS_telemetry[chn_num_b]['tow'], 'r')
plt.grid(True)
plt.xlabel('TRK Sampling Counter')
plt.ylabel('Decoded Nav TOW')
plt.legend([f'CHN-{chn_num_a-1}', f'CHN-{chn_num_b-1}'])
plt.tight_layout()
plt.savefig(os.path.join(fig_path, f'Telem_TRK_Sampling_Counter_'
f'{chn_num_a}_{chn_num_b}.png'))
plt.show()

View File

@@ -0,0 +1,139 @@
"""
hybrid_observables_plot_sample.py
Reads GNSS-SDR observables raw dump binary file using the provided function
and plots some internal variables
Irene Pérez Riega, 2023. iperrie@inta.es
Modifiable in the file:
sampling_freq - Sampling frequency [Hz]
channels - Number of channels to check if they exist
path - Path to folder which contains raw file
fig_path - Path where plots will be save
observables_log_path - Completed path to observables raw data file
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import numpy as np
import matplotlib.pyplot as plt
from lib.read_hybrid_observables_dump import read_hybrid_observables_dump
import os
observables = {}
double_size_bytes = 8
bytes_shift = 0
# ---------- CHANGE HERE:
samplingFreq = 2000000
channels = 5
path = '/home/labnav/Desktop/TEST_IRENE/'
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/HybridObservables/'
observables_log_path = path + 'observables.dat'
if not os.path.exists(fig_path):
os.makedirs(fig_path)
GNSS_observables = read_hybrid_observables_dump(channels,observables_log_path)
# Plot data
# --- optional: plot since it detect the first satellite connected
min_tow_idx = 1
obs_idx = 1
for n in range(0, channels):
idx = np.where(np.array(GNSS_observables['valid'][n])>0)[0][0]
# Find the index from the first satellite connected
if n == 0:
min_tow_idx = idx
if min_tow_idx > idx:
min_tow_idx = idx
obs_idx = n
# Plot observables from that index
# First plot
plt.figure()
plt.title('Pseudorange')
for i in range(channels):
plt.scatter(GNSS_observables['RX_time'][i][min_tow_idx:],
GNSS_observables['Pseudorange_m'][i][min_tow_idx:],s=1,
label=f'Channel {i}')
plt.xlim(GNSS_observables['RX_time'][obs_idx][min_tow_idx]-100,
GNSS_observables['RX_time'][obs_idx][-1]+100)
plt.grid(True)
plt.xlabel('TOW [s]')
plt.ylabel('Pseudorange [m]')
plt.legend()
plt.gcf().canvas.manager.set_window_title('Pseudorange.png')
plt.tight_layout()
plt.savefig(os.path.join(fig_path, 'Pseudorange.png'))
plt.show()
# Second plot
plt.figure()
plt.title('Carrier Phase')
for i in range(channels):
plt.scatter(GNSS_observables['RX_time'][i][min_tow_idx:],
GNSS_observables['Carrier_phase_hz'][i][min_tow_idx:],s=1,
label=f'Channel {i}')
plt.xlim(GNSS_observables['RX_time'][obs_idx][min_tow_idx]-100,
GNSS_observables['RX_time'][obs_idx][-1]+100)
plt.xlabel('TOW [s]')
plt.ylabel('Accumulated Carrier Phase [cycles]')
plt.grid(True)
plt.legend()
plt.gcf().canvas.manager.set_window_title('AccumulatedCarrierPhase.png')
plt.tight_layout()
plt.savefig(os.path.join(fig_path, 'AccumulatedCarrierPhase.png'))
plt.show()
# Third plot
plt.figure()
plt.title('Doppler Effect')
for i in range(channels):
plt.scatter(GNSS_observables['RX_time'][i][min_tow_idx:],
GNSS_observables['Carrier_Doppler_hz'][i][min_tow_idx:],s=1,
label=f'Channel {i}')
plt.xlim(GNSS_observables['RX_time'][obs_idx][min_tow_idx]-100,
GNSS_observables['RX_time'][obs_idx][-1]+100)
plt.xlabel('TOW [s]')
plt.ylabel('Doppler Frequency [Hz]')
plt.grid(True)
plt.legend()
plt.gcf().canvas.manager.set_window_title('DopplerFrequency.png')
plt.tight_layout()
plt.savefig(os.path.join(fig_path, 'DopplerFrequency.png'))
plt.show()
# Fourth plot
plt.figure()
plt.title('GNSS Channels captured')
for i in range(channels):
lab = 0
a = 0
while lab == 0:
lab = int(GNSS_observables["PRN"][i][min_tow_idx+a])
a += 1
plt.scatter(GNSS_observables['RX_time'][i][min_tow_idx:],
GNSS_observables['PRN'][i][min_tow_idx:], s=1,
label=f'PRN {i} = {lab}')
plt.xlim(GNSS_observables['RX_time'][obs_idx][min_tow_idx]-100,
GNSS_observables['RX_time'][obs_idx][-1]+100)
plt.xlabel('TOW [s]')
plt.ylabel('PRN')
plt.grid(True)
plt.legend()
plt.gcf().canvas.manager.set_window_title('PRNs.png')
plt.tight_layout()
plt.savefig(os.path.join(fig_path, 'PRNs.png'))
plt.show()

View File

@@ -0,0 +1,222 @@
"""
dll_pll_veml_read_tracking_dump.py
dll_pll_veml_read_tracking_dump (filename)
Read GNSS-SDR Tracking dump binary file into Python.
Opens GNSS-SDR tracking binary log file .dat and returns the contents
Irene Pérez Riega, 2023. iperrie@inta.es
Args:
filename: path to file .dat with the raw data
Return:
GNSS_tracking: A dictionary with the processed data in lists
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import struct
import sys
def dll_pll_veml_read_tracking_dump (filename):
v1 = []
v2 = []
v3 = []
v4 = []
v5 = []
v6 = []
v7 = []
v8 = []
v9 = []
v10= []
v11 = []
v12 = []
v13 = []
v14 = []
v15 = []
v16 = []
v17 = []
v18 = []
v19 = []
v20 = []
v21 = []
v22 = []
GNSS_tracking = {}
bytes_shift = 0
if sys.maxsize > 2 ** 36: # 64 bits computer
float_size_bytes = 4
unsigned_long_int_size_bytes = 8
double_size_bytes = 8
unsigned_int_size_bytes = 4
else: # 32 bits
float_size_bytes = 4
unsigned_long_int_size_bytes = 4
double_size_bytes = 8
unsigned_int_size_bytes = 4
f = open(filename, 'rb')
if f is None:
return None
else:
while True:
f.seek(bytes_shift, 0)
# VE -> Magnitude of the Very Early correlator.
v1.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# E -> Magnitude of the Early correlator.
v2.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# P -> Magnitude of the Prompt correlator.
v3.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# L -> Magnitude of the Late correlator.
v4.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# VL -> Magnitude of the Very Late correlator.
v5.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# promp_I -> Value of the Prompt correlator in the In-phase
# component.
v6.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# promp_Q -> Value of the Prompt correlator in the Quadrature
# component.
v7.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# PRN_start_sample -> Sample counter from tracking start.
if unsigned_long_int_size_bytes == 8:
v8.append(struct.unpack(
'Q', f.read(unsigned_long_int_size_bytes))[0])
bytes_shift += unsigned_long_int_size_bytes
else:
v8.append(struct.unpack('I',
f.read(unsigned_int_size_bytes))[0])
bytes_shift += unsigned_int_size_bytes
f.seek(bytes_shift, 0)
# acc_carrier_phase_rad - > Accumulated carrier phase, in rad.
v9.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carrier doppler hz -> Doppler shift, in Hz.
v10.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carrier doppler rate hz s -> Doppler rate, in Hz/s.
v11.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code freq hz -> Code frequency, in chips/s.
v12.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code_freq_rate_hz_s -> Code frequency rate, in chips/s².
v13.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carr_error -> Raw carrier error (unfiltered) at the PLL
# output, in Hz.
v14.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carr_nco -> Carrier error at the output of the PLL
# filter, in Hz.
v15.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code error -> Raw code error (unfiltered) at the DLL
# output, in chips.
v16.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code nco -> Code error at the output of the DLL
# filter, in chips.
v17.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# CN0_SNV_dB_Hz -> C/N0 estimation, in dB-Hz.
v18.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carrier lock test -> Output of the carrier lock test.
v19.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# var 1 -> not used ?
v20.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# var 2 -> not used ?
v21.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# PRN -> Satellite ID.
v22.append(struct.unpack('I',
f.read(unsigned_int_size_bytes))[0])
bytes_shift += unsigned_int_size_bytes
f.seek(bytes_shift, 0)
# Check file
linea = f.readline()
if not linea:
break
f.close()
GNSS_tracking['VE'] = v1
GNSS_tracking['E'] = v2
GNSS_tracking['P'] = v3
GNSS_tracking['L'] = v4
GNSS_tracking['VL'] = v5
GNSS_tracking['prompt_I'] = v6
GNSS_tracking['prompt_Q'] = v7
GNSS_tracking['PRN_start_sample'] = v8
GNSS_tracking['acc_carrier_phase_rad'] = v9
GNSS_tracking['carrier_doppler_hz'] = v10
GNSS_tracking['carrier_doppler_rate_hz_s'] = v11
GNSS_tracking['code_freq_hz'] = v12
GNSS_tracking['code_freq_rate_hz_s'] = v13
GNSS_tracking['carr_error'] = v14
GNSS_tracking['carr_nco'] = v15
GNSS_tracking['code_error'] = v16
GNSS_tracking['code_nco'] = v17
GNSS_tracking['CN0_SNV_dB_Hz'] = v18
GNSS_tracking['carrier_lock_test'] = v19
GNSS_tracking['var1'] = v20
GNSS_tracking['var2'] = v21
GNSS_tracking['PRN'] = v22
return GNSS_tracking

View File

@@ -0,0 +1,225 @@
"""
gps_l1_ca_kf_read_tracking_dump.py
Read GNSS-SDR Tracking dump binary file into Python.
Opens GNSS-SDR tracking binary log file .dat and returns the contents in a dictionary
gps_l1_ca_kf_read_tracking_dump(filename)
Args:
filename - Path to file .dat with the raw data
Return:
GNSS_tracking - A dictionary with the processed data in lists
Irene Pérez Riega, 2023. iperrie@inta.es
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import struct
import sys
def gps_l1_ca_kf_read_tracking_dump(filename):
bytes_shift = 0
GNSS_tracking = {}
v1 = []
v2 = []
v3 = []
v4 = []
v5 = []
v6 = []
v7 = []
v8 = []
v9 = []
v10= []
v11 = []
v12 = []
v13 = []
v14 = []
v15 = []
v16 = []
v17 = []
v18 = []
v19 = []
v20 = []
v21 = []
v22 = []
if sys.maxsize > 2 ** 36: # 64 bits computer
float_size_bytes = 4
unsigned_long_int_size_bytes = 8
double_size_bytes = 8
unsigned_int_size_bytes = 4
else: # 32 bits
float_size_bytes = 4
unsigned_long_int_size_bytes = 4
double_size_bytes = 8
unsigned_int_size_bytes = 4
f = open(filename, 'rb')
if f is None:
help(gps_l1_ca_kf_read_tracking_dump)
return None
else:
while True:
f.seek(bytes_shift, 0)
# VE -> Magnitude of the Very Early correlator.
v1.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# E -> Magnitude of the Early correlator.
v2.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# P -> Magnitude of the Prompt correlator.
v3.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# L -> Magnitude of the Late correlator.
v4.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# VL -> Magnitude of the Very Late correlator.
v5.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# promp_I -> Value of the Prompt correlator in the
# In-phase component.
v6.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# promp_Q -> Value of the Prompt correlator in the
# Quadrature component.
v7.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# PRN_start_sample -> Sample counter from tracking start.
if unsigned_long_int_size_bytes == 8:
v8.append(struct.unpack(
'Q', f.read(unsigned_long_int_size_bytes))[0])
bytes_shift += unsigned_long_int_size_bytes
else:
v8.append(struct.unpack(
'I', f.read(unsigned_int_size_bytes))[0])
bytes_shift += unsigned_int_size_bytes
f.seek(bytes_shift, 0)
# acc_carrier_phase_rad - > Accumulated carrier phase, in rad.
v9.append(struct.unpack('f', f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carrier doppler hz -> Doppler shift, in Hz.
v10.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carrier doppler rate hz s -> Doppler rate, in Hz/s.
v11.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code freq hz -> Code frequency, in chips/s.
v12.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code_freq_rate_hz_s -> Code frequency rate, in chips/s².
#todo carr_error in gps_l1_ca_kf_read_tracking_dump.m
v13.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carr_error -> Raw carrier error (unfiltered) at the PLL
# output, in Hz.
#todo carr_noise_sigma2 in gps_l1_ca_kf_read_tracking_dump.m
v14.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carr_nco -> Carrier error at the output of the PLL
# filter, in Hz.
v15.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code error -> Raw code error (unfiltered) at the DLL
# output, in chips.
v16.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# code nco -> Code error at the output of the DLL
# filter, in chips.
v17.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# CN0_SNV_dB_Hz -> C/N0 estimation, in dB-Hz.
v18.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# carrier lock test -> Output of the carrier lock test.
v19.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# var 1 -> not used ?
v20.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# var 2 -> not used ?
v21.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# PRN -> Satellite ID.
v22.append(struct.unpack('I',
f.read(unsigned_int_size_bytes))[0])
bytes_shift += unsigned_int_size_bytes
f.seek(bytes_shift, 0)
linea = f.readline()
if not linea:
break
f.close()
GNSS_tracking['VE'] = v1
GNSS_tracking['E'] = v2
GNSS_tracking['P'] = v3
GNSS_tracking['L'] = v4
GNSS_tracking['VL'] = v5
GNSS_tracking['prompt_I'] = v6
GNSS_tracking['prompt_Q'] = v7
GNSS_tracking['PRN_start_sample'] = v8
GNSS_tracking['acc_carrier_phase_rad'] = v9
GNSS_tracking['carrier_doppler_hz'] = v10
GNSS_tracking['carrier_doppler_rate_hz2'] = v11 #todo segun el dll es carrier_doppler_rate_hz_s
GNSS_tracking['code_freq_hz'] = v12
GNSS_tracking['carr_error'] = v13 #todo code_freq_rate_hz_s segun dll
GNSS_tracking['carr_noise_sigma2'] = v14 #todo carr_error segun dll
GNSS_tracking['carr_nco'] = v15
GNSS_tracking['code_error'] = v16
GNSS_tracking['code_nco'] = v17
GNSS_tracking['CN0_SNV_dB_Hz'] = v18
GNSS_tracking['carrier_lock_test'] = v19
GNSS_tracking['var1'] = v20
GNSS_tracking['var2'] = v21
GNSS_tracking['PRN'] = v22
return GNSS_tracking

View File

@@ -0,0 +1,249 @@
"""
gps_l1_ca_read_pvt_dump.py
gps_l1_ca_read_pvt_dump (filename)
Open and read GNSS-SDR PVT binary log file (.dat) into Python, and
return the contents.
Irene Pérez Riega, 2023. iperrie@inta.es
Args:
filename: path to file PVT.dat with the raw data
Return:
nav_solutions: A dictionary with the processed data in lists
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import math
import struct
import numpy as np
def gps_l1_ca_read_pvt_dump(filename):
uint8_size_bytes = 1
uint32_size_bytes = 4
double_size_bytes = 8
float_size_bytes = 4
bytes_shift = 0
TOW = []
WEEK =[]
PVT_GPS_time = []
Clock_Offset = []
ECEF_X_POS = []
ECEF_Y_POS = []
ECEF_Z_POS = []
ECEF_X_VEL = []
ECEF_Y_VEL = []
ECEF_Z_VEL = []
C_XX = []
C_YY = []
C_ZZ = []
C_XY = []
C_YZ = []
C_ZX = []
Lat = []
Long = []
Height = []
num_valid_sats = []
RTKLIB_status = []
RTKLIB_type = []
AR_factor = []
AR_threshold = []
GDOP = []
PDOP = []
HDOP = []
VDOP = []
f = open(filename, 'rb')
if f is None:
return None
else:
while True:
f.seek(bytes_shift, 0)
# TOW -> (Time Of Week) [usually sec] uint32
TOW.append(struct.unpack('I',
f.read(uint32_size_bytes))[0])
bytes_shift += uint32_size_bytes
f.seek(bytes_shift, 0)
# WEEK -> uint32
WEEK.append(struct.unpack('I',
f.read(uint32_size_bytes))[0])
bytes_shift += uint32_size_bytes
f.seek(bytes_shift, 0)
# PVT_GPS_time -> double
PVT_GPS_time.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# User_clock_offset -> [s] double
Clock_Offset.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# ##### ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) ######
ECEF_X_POS.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
ECEF_Y_POS.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
ECEF_Z_POS.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
ECEF_X_VEL.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
ECEF_Y_VEL.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
ECEF_Z_VEL.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# #### Position variance/covariance [m²]
# {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) ######
C_XX.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
C_YY.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
C_ZZ.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
C_XY.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
C_YZ.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
C_ZX.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# GEO user position Latitude -> [deg] double
Lat.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# GEO user position Longitude -> [deg] double
Long.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# GEO user position Height -> [m] double
Height.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# NUMBER OF VALID SATS -> uint8
num_valid_sats.append(struct.unpack('B',
f.read(uint8_size_bytes))[0])
bytes_shift += uint8_size_bytes
f.seek(bytes_shift, 0)
# RTKLIB solution status (Real-Time Kinematic) -> uint8
RTKLIB_status.append(struct.unpack('B',
f.read(uint8_size_bytes))[0])
bytes_shift += uint8_size_bytes
f.seek(bytes_shift, 0)
# RTKLIB solution type (0:xyz-ecef,1:enu-baseline) -> uint8
RTKLIB_type.append(struct.unpack('B',
f.read(uint8_size_bytes))[0])
bytes_shift += uint8_size_bytes
f.seek(bytes_shift, 0)
# AR ratio factor for validation -> float
AR_factor.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# AR ratio threshold for validation -> float
AR_threshold.append(struct.unpack('f',
f.read(float_size_bytes))[0])
bytes_shift += float_size_bytes
f.seek(bytes_shift, 0)
# ##### GDOP / PDOP / HDOP / VDOP (4 * double) #####
GDOP.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
PDOP.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
HDOP.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
VDOP.append(struct.unpack('d',
f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
# Check file
linea = f.readline()
if not linea:
break
f.close()
# Creating a list with total velocities [m/s]
vel = []
for i in range(len(ECEF_X_VEL)):
vel.append(math.sqrt(ECEF_X_VEL[i]**2 + ECEF_Y_VEL[i]**2 +
ECEF_Z_VEL[i]**2))
navSolutions = {
'TOW': TOW,
'WEEK': WEEK,
'TransmitTime': PVT_GPS_time,
'dt': Clock_Offset,
'X': ECEF_X_POS,
'Y': ECEF_Y_POS,
'Z': ECEF_Z_POS,
'X_vel': ECEF_X_VEL,
'Y_vel': ECEF_Y_VEL,
'Z_vel': ECEF_Z_VEL,
'Tot_Vel': vel,
'C_XX': C_XX,
'C_YY': C_YY,
'C_ZZ': C_ZZ,
'C_XY': C_XY,
'C_YZ': C_YZ,
'C_ZX': C_ZX,
'latitude': Lat,
'longitude': Long,
'height': Height,
'SATS': num_valid_sats,
'RTK_status': RTKLIB_status,
'RTK_type': RTKLIB_type,
'AR_factor': AR_factor,
'AR_threshold': AR_threshold,
'GDOP': np.array(GDOP),
'PDOP': np.array(PDOP),
'HDOP': np.array(HDOP),
'VDOP': np.array(VDOP)
}
return navSolutions

View File

@@ -0,0 +1,86 @@
"""
gps_l1_ca_read_telemetry_dump.py
gps_l1_ca_read_telemetry_dump (filename)
Open and read GNSS-SDR telemetry binary log files (.dat) into Python, and
return the contents.
Irene Pérez Riega, 2023. iperrie@inta.es
Args:
filename - Path to file telemetry[N].dat with the raw data
Return:
telemetry - A dictionary with the processed data
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import struct
def gps_l1_ca_read_telemetry_dump(filename):
double_size_bytes = 8
int_size_bytes = 4
bytes_shift = 0
tow_current_symbol_ms = []
tracking_sample_counter = []
tow = []
nav_simbols = []
prn = []
f = open(filename, 'rb')
if f is None:
return None
else:
while True:
f.seek(bytes_shift, 0)
tow_current_symbol_ms.append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
tracking_sample_counter.append(struct.unpack(
'Q', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
tow.append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
nav_simbols.append(struct.unpack(
'I', f.read(int_size_bytes))[0])
bytes_shift += int_size_bytes
f.seek(bytes_shift, 0)
prn.append(struct.unpack('I', f.read(int_size_bytes))[0])
bytes_shift += int_size_bytes
f.seek(bytes_shift, 0)
# Check file
linea = f.readline()
if not linea:
break
telemetry = {
'tow_current_symbol_ms': tow_current_symbol_ms,
'tracking_sample_counter': tracking_sample_counter,
'tow': tow,
'nav_simbols': nav_simbols,
'prn': prn
}
return telemetry

View File

@@ -0,0 +1,140 @@
"""
plotKalman.py
plotKalman (channelNr, trackResults, settings)
This function plots the tracking results for the given channel list.
Irene Pérez Riega, 2023. iperrie@inta.es
Args:
channelList - list of channels to be plotted.
trackResults - tracking results from the tracking function.
settings - receiver settings.
Modifiable in the file:
fig_path - Path where doppler plots will be save
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import matplotlib.pyplot as plt
import numpy as np
import os
def plotKalman(channelNr, trackResults, settings):
# ---------- CHANGE HERE:
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/PlotKalman'
if not os.path.exists(fig_path):
os.makedirs(fig_path)
# Protection - if the list contains incorrect channel numbers
channelNr = np.intersect1d(channelNr,
np.arange(1, settings['numberOfChannels'] + 1))
for channelNr in channelNr:
time_start = settings['timeStartInSeconds']
time_axis_in_seconds = np.arange(1, settings['msToProcess']+1)/1000
# Plot all figures
plt.figure(figsize=(1920 / 100, 1080 / 100))
plt.clf()
plt.gcf().canvas.set_window_title(
f'Channel {channelNr} (PRN '
f'{str(trackResults[channelNr-1]["PRN"][-2])}) results')
plt.subplots_adjust(left=0.1, right=0.9, top=0.9, bottom=0.1,
hspace=0.4, wspace=0.4)
plt.tight_layout()
# Row 1
# ----- CNo for signal -----------------------------------------------
# Measure of the ratio between carrier signal power and noise power
plt.subplot(4, 2, 1)
plt.plot(time_axis_in_seconds,
trackResults[channelNr-1]['CNo'][:settings['msToProcess']],
'b')
plt.grid()
plt.axis('tight')
plt.xlabel('Time (s)')
plt.ylabel('CNo (dB-Hz)')
plt.title('Carrier to Noise Ratio', fontweight='bold')
# ----- PLL discriminator filtered -----------------------------------
plt.subplot(4, 2, 2)
plt.plot(time_axis_in_seconds,
trackResults[channelNr-1]['state1']
[:settings['msToProcess']], 'b')
plt.grid()
plt.axis('tight')
plt.xlim([time_start, time_axis_in_seconds[-1]])
plt.xlabel('Time (s)')
plt.ylabel('Phase Amplitude')
plt.title('Filtered Carrier Phase', fontweight='bold')
# Row 2
# ----- Carrier Frequency --------------------------------------------
# Filtered carrier frequency of (transmitted by a satellite)
# for a specific channel
plt.subplot(4, 2, 3)
plt.plot(time_axis_in_seconds[1:],
trackResults[channelNr-1]['state2']
[1:settings['msToProcess']], color=[0.42, 0.25, 0.39])
plt.grid()
plt.axis('auto')
plt.xlim(time_start, time_axis_in_seconds[-1])
plt.xlabel('Time (s)')
plt.ylabel('Freq (Hz)')
plt.title('Filtered Carrier Frequency', fontweight='bold')
# ----- Carrier Frequency Rate ---------------------------------------
plt.subplot(4, 2, 4)
plt.plot(time_axis_in_seconds[1:],
trackResults[channelNr-1]['state3']
[1:settings['msToProcess']], color=[0.42, 0.25, 0.39])
plt.grid()
plt.axis('auto')
plt.xlim(time_start, time_axis_in_seconds[-1])
plt.xlabel('Time (s)')
plt.ylabel('Freq (Hz)')
plt.title('Filtered Carrier Frequency Rate', fontweight='bold')
# Row 3
# ----- PLL discriminator unfiltered----------------------------------
plt.subplot(4, 2, (5,6))
plt.plot(time_axis_in_seconds,
trackResults[channelNr-1]['innovation'], 'r')
plt.grid()
plt.axis('auto')
plt.xlim(time_start, time_axis_in_seconds[-1])
plt.xlabel('Time (s)')
plt.ylabel('Amplitude')
plt.title('Raw PLL discriminator (Innovation)',fontweight='bold')
# Row 4
# ----- PLL discriminator covariance ---------------------------------
plt.subplot(4, 2, (7,8))
plt.plot(time_axis_in_seconds,
trackResults[channelNr-1]['r_noise_cov'], 'r')
plt.grid()
plt.axis('auto')
plt.xlim(time_start, time_axis_in_seconds[-1])
plt.xlabel('Time (s)')
plt.ylabel('Variance')
plt.title('Estimated Noise Variance', fontweight='bold')
plt.tight_layout()
plt.savefig(os.path.join(fig_path,
f'kalman_ch{channelNr}_PRN_'
f'{trackResults[channelNr - 1]["PRN"][-1]}'
f'.png'))
plt.show()

View File

@@ -0,0 +1,134 @@
"""
plotNavigation.py
Function plots variations of coordinates over time and a 3D position
plot. It plots receiver coordinates in UTM system or coordinate offsets if
the true UTM receiver coordinates are provided.
Irene Pérez Riega, 2023. iperrie@inta.es
plotNavigation(navSolutions, settings, plot_skyplot)
Args:
navSolutions - Results from navigation solution function. It
contains measured pseudoranges and receiver
coordinates.
settings - Receiver settings. The true receiver coordinates
are contained in this structure.
plot_skyplot - If == 1 then use satellite coordinates to plot the
satellite positions (not implemented yet TO DO)
Modifiable in the file:
fig_path - Path where plots will be save
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import numpy as np
import matplotlib.pyplot as plt
import os
def plotNavigation(navSolutions, settings, plot_skyplot=0):
# ---------- CHANGE HERE:
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/PlotNavigation'
if not os.path.exists(fig_path):
os.makedirs(fig_path)
if navSolutions:
if (np.isnan(settings['true_position']['E_UTM']) or
np.isnan(settings['true_position']['N_UTM']) or
np.isnan(settings['true_position']['U_UTM'])):
# Compute mean values
ref_coord = {
'E_UTM': np.nanmean(navSolutions['E_UTM']),
'N_UTM': np.nanmean(navSolutions['N_UTM']),
'U_UTM': np.nanmean(navSolutions['U_UTM'])
}
mean_latitude = np.nanmean(navSolutions['latitude'])
mean_longitude = np.nanmean(navSolutions['longitude'])
mean_height = np.nanmean(navSolutions['height'])
ref_point_lg_text = (f"Mean Position\nLat: {mean_latitude}º\n"
f"Long: {mean_longitude}º\n"
f"Hgt: {mean_height:+6.1f}")
else:
# Compute the mean error for static receiver
ref_coord = {
'E_UTM': settings.truePosition['E_UTM'],
'N_UTM': settings.truePosition['N_UTM'],
'U_UTM': settings.truePosition['U_UTM']
}
mean_position = {
'E_UTM': np.nanmean(navSolutions['E_UTM']),
'N_UTM': np.nanmean(navSolutions['N_UTM']),
'U_UTM': np.nanmean(navSolutions['U_UTM'])
}
error_meters = np.sqrt(
(mean_position['E_UTM'] - ref_coord['E_UTM']) ** 2 +
(mean_position['N_UTM'] - ref_coord['N_UTM']) ** 2 +
(mean_position['U_UTM'] - ref_coord['U_UTM']) ** 2)
ref_point_lg_text = (f"Reference Position, Mean 3D error = "
f"{error_meters} [m]")
#Create plot and subplots
plt.figure(figsize=(1920 / 120, 1080 / 120))
plt.clf()
plt.title('Navigation solutions',fontweight='bold')
ax1 = plt.subplot(4, 2, (1, 4))
ax2 = plt.subplot(4, 2, (5, 7), projection='3d')
ax3 = plt.subplot(4, 2, (6, 8), projection='3d')
# (ax1) Coordinate differences in UTM system from reference point
ax1.plot(np.vstack([navSolutions['E_UTM'] - ref_coord['E_UTM'],
navSolutions['N_UTM'] - ref_coord['N_UTM'],
navSolutions['U_UTM'] - ref_coord['U_UTM']]).T)
ax1.set_title('Coordinates variations in UTM system', fontweight='bold')
ax1.legend(['E_UTM', 'N_UTM', 'U_UTM'])
ax1.set_xlabel(f"Measurement period: {settings['navSolPeriod']} ms")
ax1.set_ylabel('Variations (m)')
ax1.grid(True)
ax1.axis('tight')
# (ax2) Satellite sky plot
if plot_skyplot: #todo posicion de los satelites
skyPlot(ax2, navSolutions['channel']['az'],
navSolutions['channel']['el'],
navSolutions['channel']['PRN'][:, 0])
ax2.set_title(f'Sky plot (mean PDOP: '
f'{np.nanmean(navSolutions["DOP"][1, :]):.1f})',
fontweight='bold')
# (ax3) Position plot in UTM system
ax3.scatter(navSolutions['E_UTM'] - ref_coord['E_UTM'],
navSolutions['N_UTM'] - ref_coord['N_UTM'],
navSolutions['U_UTM'] - ref_coord['U_UTM'], marker='+')
ax3.scatter([0], [0], [0], color='r', marker='+', linewidth=1.5)
ax3.view_init(0, 90)
ax3.set_box_aspect([1, 1, 1])
ax3.grid(True, which='minor')
ax3.legend(['Measurements', ref_point_lg_text])
ax3.set_title('Positions in UTM system (3D plot)',fontweight='bold')
ax3.set_xlabel('East (m)')
ax3.set_ylabel('North (m)')
ax3.set_zlabel('Upping (m)')
plt.tight_layout()
plt.savefig(os.path.join(fig_path, 'measures_UTM.png'))
plt.show()

View File

@@ -0,0 +1,208 @@
"""
plotPosition.py
plot_position(navSolutions)
Graph Latitude-Longitude and X-Y-X as a function of Transmit Time
Args:
navSolutions - A dictionary with the processed information in lists
plot_oneVStime(navSolutions, name)
Graph of a variable as a function of transmission time
Args:
navSolutions - A dictionary with the processed information in lists
name - navSolutions variable name that we want to plot
calcularCEFP(percentil, navSolutions, m_lat, m_long)
Calculate CEFP radio [m] for n percentil.
Args:
percentil - Number of measures that will be inside the circumference
navSolutions - A dictionary with the processed information in lists
m_lat - Mean latitude measures [º]
m_long - Mean longitude measures [º]
Modifiable in the file:
fig_path - Path where plots will be save
fig_path_maps - Path where the maps will be save
filename_map - Path where map will be save
filename_map_t - Path where terrain map will be save
Irene Pérez Riega, 2023. iperrie@inta.es
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import math
import os.path
import webbrowser
import numpy as np
import matplotlib.pyplot as plt
import folium
def plot_position(navSolutions):
# ---------- CHANGE HERE:
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/PlotPosition/'
fig_path_maps = fig_path + 'maps/'
filename_map = 'mapPlotPosition.html'
filename_map_t = 'mapTerrainPotPosition.html'
if not os.path.exists(fig_path_maps):
os.mkdir(fig_path_maps)
# Statics Positions:
m_lat = sum(navSolutions['latitude']) / len(navSolutions['latitude'])
m_long = sum(navSolutions['longitude']) / len(navSolutions['longitude'])
# CEFP_n -> Include the n% of the dots in the circle
r_CEFP_95 = calcularCEFP(95, navSolutions, m_lat, m_long)
r_CEFP_50 = calcularCEFP(50, navSolutions, m_lat, m_long)
# Generate and save html with the positions
m = folium.Map(location=[navSolutions['latitude'][0],
navSolutions['longitude'][0]], zoom_start=100)
c_CEFP95 = folium.Circle(location=[m_lat, m_long],
radius=r_CEFP_95, color='green', fill=True,
fill_color='green', fill_opacity=0.5)
c_CEFP50 = folium.Circle(location=[m_lat, m_long], radius=r_CEFP_50,
color='red', fill=True, fill_color='red',
fill_opacity=0.5)
# POP-UPs
popup95 = folium.Popup("(Green)CEFP95 diameter: {} "
"metres".format(2 * r_CEFP_95))
popup95.add_to(c_CEFP95)
popup50 = folium.Popup("(Red)CEFP50 diameter: {} "
"metres".format(2 * r_CEFP_50))
popup50.add_to(c_CEFP50)
c_CEFP95.add_to(m)
c_CEFP50.add_to(m)
# Optional: Plot each point ->
"""
for i in range(len(navSolutions['latitude'])):
folium.Marker(location=[navSolutions['latitude'][i],
navSolutions['longitude'][i]],
icon=folium.Icon(color='red')).add_to(m)
"""
m.save(fig_path_maps + filename_map)
webbrowser.open(fig_path_maps + filename_map)
# Optional: with terrain ->
"""
n = folium.Map(location=[navSolutions['latitude'][0],
navSolutions['longitude'][0]], zoom_start=100,
tiles='Stamen Terrain')
c_CEFP95.add_to(n)
c_CEFP50.add_to(n)
n.save(fig_path_maps + filename_map_t)
webbrowser.open(fig_path_maps + filename_map_t)
"""
# Plot ->
time = []
for i in range(len(navSolutions['TransmitTime'])):
time.append(round(navSolutions['TransmitTime'][i] -
min(navSolutions['TransmitTime']), 3))
plt.figure(figsize=(1920 / 120, 1080 / 120))
plt.clf()
plt.suptitle(f'Plot file PVT process data results')
# Latitude and Longitude
plt.subplot(1, 2, 1)
scatter = plt.scatter(navSolutions['latitude'], navSolutions['longitude'],
c=time, marker='.')
plt.grid()
plt.ticklabel_format(style='plain', axis='both', useOffset=False)
plt.title('Positions latitud-longitud')
plt.xlabel('Latitude º')
plt.ylabel('Longitude º')
plt.axis('tight')
# Colors
cmap = plt.get_cmap('viridis')
norm = plt.Normalize(vmin=min(time), vmax=max(time))
scatter.set_cmap(cmap)
scatter.set_norm(norm)
colors = plt.colorbar(scatter)
colors.set_label('TransmitTime [s]')
# X, Y, Z
ax = plt.subplot(1, 2, 2, projection='3d')
plt.ticklabel_format(style='plain', axis='both', useOffset=False)
ax.scatter(navSolutions['X'], navSolutions['Y'], navSolutions['Z'],
c=time, marker='.')
ax.set_xlabel('Eje X [m]')
ax.set_ylabel('Eje Y [m]')
ax.set_zlabel('Eje Z [m]')
ax.set_title('Positions x-y-z')
plt.tight_layout()
plt.savefig(os.path.join(fig_path, f'PVT_ProcessDataResults.png'))
plt.show()
def plot_oneVStime(navSolutions, name):
# ---------- CHANGE HERE:
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/PlotPosition/'
if not os.path.exists(fig_path):
os.mkdir(fig_path)
time = []
for i in range(len(navSolutions['TransmitTime'])):
time.append(round(navSolutions['TransmitTime'][i] -
min(navSolutions['TransmitTime']), 3))
plt.clf()
plt.scatter(time, navSolutions[name], marker='.')
plt.grid()
plt.title(f'{name} vs Time')
plt.xlabel('Time [s]')
plt.ylabel(name)
plt.axis('tight')
plt.ticklabel_format(style='plain', axis='both', useOffset=False)
plt.tight_layout()
plt.savefig(os.path.join(fig_path, f'{name}VSTime.png'))
plt.show()
def calcularCEFP(percentil, navSolutions, m_lat, m_long):
r_earth = 6371000
lat = []
long = []
dlat = []
dlong = []
dist = []
m_lat = math.radians(m_lat)
m_long = math.radians(m_long)
for i in range(len(navSolutions['latitude'])):
lat.append(math.radians(navSolutions['latitude'][i]))
long.append(math.radians(navSolutions['longitude'][i]))
for i in range(len(lat)):
dlat.append(m_lat - lat[i])
dlong.append(m_long - long[i])
# Haversine:
a = (math.sin(dlat[i] / 2) ** 2 +
math.cos(lat[i]) * math.cos(m_lat) * math.sin(dlong[i] / 2) ** 2)
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
dist.append(r_earth * c)
# Radio CEFP
radio_CEFP_p = np.percentile(dist, percentil)
return radio_CEFP_p

View File

@@ -0,0 +1,190 @@
"""
plotTracking.py
This function plots the tracking results for the given channel list.
Irene Pérez Riega, 2023. iperrie@inta.es
plotTracking(channelList, trackResults, settings)
Args:
channelList - list of channels to be plotted.
trackResults - tracking results from the tracking function.
settings - receiver settings.
Modifiable in the file:
fig_path - Path where plots will be save
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import numpy as np
import os
import matplotlib.pyplot as plt
def plotTracking(channelNr, trackResults, settings):
# ---------- CHANGE HERE:
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/PlotTracking'
if not os.path.exists(fig_path):
os.makedirs(fig_path)
# Protection - if the list contains incorrect channel numbers
if channelNr in list(range(1,settings["numberOfChannels"]+1)):
plt.figure(figsize=(1920 / 120, 1080 / 120))
plt.clf()
plt.gcf().canvas.set_window_title(
f'Channel {channelNr} (PRN '
f'{trackResults[channelNr-1]["PRN"][0]}) results')
plt.subplots_adjust(left=0.1, right=0.9, top=0.9, bottom=0.1,
hspace=0.4, wspace=0.4)
plt.tight_layout()
# Extract timeAxis and time_label
if 'prn_start_time_s' in trackResults[channelNr-1]:
timeAxis = trackResults[channelNr-1]['prn_start_time_s']
time_label = 'RX Time (s)'
else:
timeAxis = np.arange(1, len(trackResults[channelNr-1]['PRN']) + 1)
time_label = 'Epoch'
# Row 1 ==============================================================
# Discrete-Time Scatter Plot
plt.subplot(4, 3, 1)
plt.plot(trackResults[channelNr-1]['I_P'],
trackResults[channelNr-1]['Q_P'], marker='.', markersize=1,
linestyle=' ')
plt.grid()
plt.axis('equal')
plt.title('Discrete-Time Scatter Plot', fontweight='bold')
plt.xlabel('I prompt')
plt.ylabel('Q prompt')
# Nav bits
plt.subplot(4, 3, (2, 3))
plt.plot(timeAxis, trackResults[channelNr-1]['I_P'], linewidth=1)
plt.grid()
plt.title('Bits of the navigation message', fontweight='bold')
plt.xlabel(time_label)
plt.axis('tight')
# Row 2 ==============================================================
# Raw PLL discriminator unfiltered
plt.subplot(4, 3, 4)
plt.plot(timeAxis, trackResults[channelNr-1]['pllDiscr'],
color='r', linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Raw PLL discriminator', fontweight='bold')
# Correlation results
plt.subplot(4, 3, (5, 6))
corr_data = [
np.sqrt(trackResults[channelNr-1]['I_E'] ** 2 +
trackResults[channelNr-1]['Q_E'] ** 2),
np.sqrt(trackResults[channelNr-1]['I_P'] ** 2 +
trackResults[channelNr-1]['Q_P'] ** 2),
np.sqrt(trackResults[channelNr-1]['I_L'] ** 2 +
trackResults[channelNr - 1]['Q_L'] ** 2)
]
line = []
colors = ['b', '#FF6600', '#FFD700', 'purple', 'g']
for i, data in enumerate(corr_data):
line.append(plt.plot(timeAxis, data,
label=f'Data {i+1}', color=colors[i],
marker='*', linestyle=' ', linewidth=1))
plt.grid()
plt.title('Correlation results', fontweight='bold')
plt.xlabel(time_label)
plt.axis('tight')
plt.legend([r'$\sqrt{I_{VE}^2 + Q_{VE}^2}$',
r'$\sqrt{I_{E}^2 + Q_{E}^2}$',
r'$\sqrt{I_{P}^2 + Q_{P}^2}$',
r'$\sqrt{I_{L}^2 + Q_{L}^2}$',
r'$\sqrt{I_{VL}^2 + Q_{VL}^2}$'], loc='best')
# Row 3 ==============================================================
# Filtered PLL discriminator
plt.subplot(4, 3, 7)
plt.plot(timeAxis, trackResults[channelNr-1]['pllDiscrFilt'],
'b', linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Filtered PLL discriminator', fontweight='bold')
# Raw DLL discriminator unfiltered
plt.subplot(4, 3, 8)
plt.plot(timeAxis, trackResults[channelNr-1]['dllDiscr'],
'r', linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Raw DLL discriminator', fontweight='bold')
# Filtered DLL discriminator
plt.subplot(4, 3, 9)
plt.plot(timeAxis, trackResults[channelNr-1]['dllDiscrFilt'],
'b', linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Filtered DLL discriminator', fontweight='bold')
# Row 4 ==============================================================
# CNo for signal
plt.subplot(4, 3, 10)
plt.plot(timeAxis, trackResults[channelNr-1]['CNo'], 'b',
linewidth=1)
plt.grid()
plt.axis('equal')
plt.xlabel('Time (s)')
plt.ylabel('CNo (dB-Hz)')
plt.title('Carrier to Noise Ratio', fontweight='bold')
# Carrier Frequency
plt.subplot(4, 3, 11)
plt.plot(timeAxis, trackResults[channelNr-1]['carrFreq'],
marker='.', markersize=1, linestyle=' ')
plt.grid()
plt.axis('equal')
plt.xlabel('Time (s)')
plt.ylabel('Freq (hz)')
plt.title('Carrier Frequency', fontweight='bold')
# Code Frequency
# Skip sample 0 to help with results display
plt.subplot(4, 3, 12)
plt.plot(timeAxis, trackResults[channelNr-1]['codeFreq'],
marker='.', markersize=1, linestyle=' ')
plt.grid()
plt.axis('equal')
plt.xlabel('Time (s)')
plt.ylabel('Freq (Hz)')
plt.title('Code Frequency',fontweight='bold')
plt.tight_layout()
plt.savefig(os.path.join(fig_path,
f'trk_dump_ch{channelNr}_PRN_'
f'{trackResults[channelNr - 1]["PRN"][-1]}'
f'.png'))
plt.show()

View File

@@ -0,0 +1,171 @@
"""
plotVEMLTracking.py
This function plots the tracking results for the given channel list.
Irene Pérez Riega, 2023. iperrie@inta.es
plotVEMLTracking(channelNr, trackResults, settings)
Args:
channelList - list of channels to be plotted.
trackResults - tracking results from the tracking function.
settings - receiver settings.
Modifiable in the file:
fig_path - Path where plots will be save
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import matplotlib.pyplot as plt
import numpy as np
import os
def plotVEMLTracking(channelNr, trackResults, settings):
# ---------- CHANGE HERE:
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/VEMLTracking'
if not os.path.exists(fig_path):
os.makedirs(fig_path)
# Protection - if the list contains incorrect channel numbers
if channelNr in list(range(1,settings["numberOfChannels"]+1)):
plt.figure(figsize=(1920 / 120, 1080 / 120))
plt.clf()
plt.gcf().canvas.set_window_title(
f'Channel {channelNr} (PRN '
f'{trackResults[channelNr-1]["PRN"][0]}) results')
plt.subplots_adjust(left=0.1, right=0.9, top=0.9, bottom=0.1,
hspace=0.4, wspace=0.4)
# Extract timeAxis and time_label
if 'prn_start_time_s' in trackResults[channelNr-1]:
timeAxis = trackResults[channelNr-1]['prn_start_time_s']
time_label = 'RX Time (s)'
else:
timeAxis = np.arange(1, len(trackResults[channelNr-1]['PRN']) + 1)
time_label = 'Epoch'
len_dataI = len (trackResults[channelNr-1]["data_I"])
len_dataQ = len (trackResults[channelNr-1]["data_Q"])
if len_dataI < len_dataQ:
dif = len_dataQ - len_dataI
trackResults[channelNr-1]["data_I"] = np.pad(
trackResults[channelNr-1]["data_I"], pad_width=(0,dif),
mode="constant", constant_values=0)
elif len_dataQ < len_dataI:
dif = len_dataI - len_dataQ
trackResults[channelNr-1]["data_Q"] = np.pad(
trackResults[channelNr-1]["data_Q"], pad_width=(0,dif),
mode="constant", constant_values=0 )
# Discrete-Time Scatter Plot
plt.subplot(3, 3, 1)
plt.plot(trackResults[channelNr-1]['data_I'],
trackResults[channelNr-1]['data_Q'], marker='.',
markersize=1, linestyle=' ')
plt.grid()
plt.axis('equal')
plt.title('Discrete-Time Scatter Plot', fontweight='bold')
plt.xlabel('I prompt')
plt.ylabel('Q prompt')
# Nav bits
plt.subplot(3, 3, (2, 3))
plt.plot(timeAxis, trackResults[channelNr-1]['data_I'],
linewidth=1)
plt.grid()
plt.title('Bits of the navigation message', fontweight='bold')
plt.xlabel(time_label)
plt.axis('tight')
# Raw PLL discriminator unfiltered
plt.subplot(3, 3, 4)
plt.plot(timeAxis, trackResults[channelNr-1]['pllDiscr'],
color='r', linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Raw PLL discriminator', fontweight='bold')
# Correlation results
plt.subplot(3, 3, (5, 6))
corr_data = [
np.sqrt(trackResults[channelNr-1]['I_VE'] ** 2 +
trackResults[channelNr-1]['Q_VE'] ** 2),
np.sqrt(trackResults[channelNr-1]['I_E'] ** 2 +
trackResults[channelNr-1]['Q_E'] ** 2),
np.sqrt(trackResults[channelNr-1]['I_P'] ** 2 +
trackResults[channelNr-1]['Q_P'] ** 2),
np.sqrt(trackResults[channelNr-1]['I_L'] ** 2 +
trackResults[channelNr-1]['Q_L'] ** 2),
np.sqrt(trackResults[channelNr-1]['I_VL'] ** 2 +
trackResults[channelNr-1]['Q_VL'] ** 2)
]
line = []
colors = ['b','#FF6600','#FFD700','purple','g']
for i, data in enumerate(corr_data):
line.append(plt.plot(timeAxis, data, label=f'Data {i+1}',
color=colors[i], marker='*', linestyle=' ',
linewidth=1))
plt.grid()
plt.title('Correlation results',fontweight='bold')
plt.xlabel(time_label)
plt.axis('tight')
plt.legend([r'$\sqrt{I_{VE}^2 + Q_{VE}^2}$',
r'$\sqrt{I_{E}^2 + Q_{E}^2}$',
r'$\sqrt{I_{P}^2 + Q_{P}^2}$',
r'$\sqrt{I_{L}^2 + Q_{L}^2}$',
r'$\sqrt{I_{VL}^2 + Q_{VL}^2}$'], loc='best')
# Filtered PLL discriminator
plt.subplot(3, 3, 7)
plt.plot(timeAxis, trackResults[channelNr-1]['pllDiscrFilt'],
'b', linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Filtered PLL discriminator', fontweight='bold')
# Raw DLL discriminator unfiltered
plt.subplot(3, 3, 8)
plt.plot(timeAxis, trackResults[channelNr-1]['dllDiscr'], 'r',
linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Raw DLL discriminator',fontweight='bold')
# Filtered DLL discriminator
plt.subplot(3, 3, 9)
plt.plot(timeAxis, trackResults[channelNr-1]['dllDiscrFilt'],
'b', linewidth=1)
plt.grid()
plt.axis('tight')
plt.xlabel(time_label)
plt.ylabel('Amplitude')
plt.title('Filtered DLL discriminator',fontweight='bold')
plt.savefig(os.path.join(fig_path,
f'Ch{channelNr}_PRN'
f'{trackResults[channelNr-1]["PRN"][0]}'
f'_results'))
plt.show()

View File

@@ -0,0 +1,112 @@
"""
read_hybrid_observables_dump.py
This function plots the tracking results for the given channel list.
Irene Pérez Riega, 2023. iperrie@inta.es
read_hybrid_observables_dump(channels, filename)
Args:
channels - list of channels to be processed
filename - path to the observables file
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import struct
def read_hybrid_observables_dump(channels, filename):
double_size_bytes = 8
bytes_shift = 0
RX_time = [[] for _ in range(channels+1)]
d_TOW_at_current_symbol = [[] for _ in range(channels+1)]
Carrier_Doppler_hz = [[] for _ in range(channels+1)]
Carrier_phase_hz = [[] for _ in range(channels+1)]
Pseudorange_m = [[] for _ in range(channels+1)]
PRN = [[] for _ in range(channels+1)]
valid = [[] for _ in range(channels+1)]
f = open(filename, 'rb')
if f is None:
return None
else:
while True:
try:
# There is an empty channel at the end (Channel-6)
for N in range(0, channels+1):
f.seek(bytes_shift, 0)
RX_time[N].append(struct.unpack(
'd',f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
d_TOW_at_current_symbol[N].append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
Carrier_Doppler_hz[N].append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
Carrier_phase_hz[N].append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
Pseudorange_m[N].append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
PRN[N].append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
valid[N].append(struct.unpack(
'd', f.read(double_size_bytes))[0])
bytes_shift += double_size_bytes
f.seek(bytes_shift, 0)
except:
break
# Delete last Channel:
RX_time = [row for i, row in enumerate(RX_time) if i != 5]
d_TOW_at_current_symbol = [row for i, row in enumerate(
d_TOW_at_current_symbol)if i != 5]
Carrier_Doppler_hz = [row for i, row in enumerate(
Carrier_Doppler_hz) if i != 5]
Carrier_phase_hz = [row for i, row in enumerate(
Carrier_phase_hz) if i != 5]
Pseudorange_m = [row for i, row in enumerate(Pseudorange_m) if i != 5]
PRN = [row for i, row in enumerate(PRN) if i != 5]
valid = [row for i, row in enumerate(valid) if i != 5]
observables = {
'RX_time': RX_time,
'd_TOW_at_current_symbol': d_TOW_at_current_symbol,
'Carrier_Doppler_hz': Carrier_Doppler_hz,
'Carrier_phase_hz': Carrier_phase_hz,
'Pseudorange_m': Pseudorange_m,
'PRN': PRN,
'valid':valid
}
f.close()
return observables

View File

@@ -0,0 +1,262 @@
"""
plot_acq_grid.py
Reads GNSS-SDR Acquisition dump .mat file using the provided function and
plots acquisition grid of acquisition statistic of PRN sat
Irene Pérez Riega, 2023. iperrie@inta.es
Modifiable in the file:
sampling_freq - Sampling frequency [Hz]
channels - Number of channels to check if they exist
path - Path to folder which contains raw file
fig_path - Path where plots will be save
plot_all_files - Plot all the files in a folder (True/False)
----
file - Fixed part in files names. In our case: acq_dump
sat - Satellite. In our case: 1
channel - Channel. In our case: 1
execution - In our case: 0
signal_type - In our case: 1
----
lite_view - True for light grid representation
File format:
{path}/{file}_ch_{system}_{signal}_ch_{channel}_{execution}_sat_{sat}.mat
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import os
import sys
import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline
import h5py
# ---------- CHANGE HERE:
path = '/home/labnav/Desktop/TEST_IRENE/acquisition/'
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/Acquisition/'
plot_all_files = False
if not os.path.exists(fig_path):
os.makedirs(fig_path)
if not plot_all_files:
# ---------- CHANGE HERE:
file = 'acq_dump'
sat = 1
channel = 0
execution = 1
signal_type = 1
lite_view = True
# If lite_view -> sets the number of samples per chip in the graphical
# representation
n_samples_per_chip = 3
d_samples_per_code = 25000
signal_types = {
1: ('G', '1C', 1023), # GPS L1
2: ('G', '2S', 10230), # GPS L2M
3: ('G', 'L5', 10230), # GPS L5
4: ('E', '1B', 4092), # Galileo E1B
5: ('E', '5X', 10230), # Galileo E5
6: ('R', '1G', 511), # Glonass 1G
7: ('R', '2G', 511), # Glonass 2G
8: ('C', 'B1', 2048), # Beidou B1
9: ('C', 'B3', 10230), # Beidou B3
10: ('C', '5C', 10230) # Beidou B2a
}
system, signal, n_chips = signal_types.get(signal_type)
# Load data
filename = (f'{path}{file}_ch_{system}_{signal}_ch_{channel}_{execution}'
f'_sat_{sat}.mat')
img_name_root = (f'{fig_path}{file}_ch_{system}_{signal}_ch_{channel}_'
f'{execution}_sat_{sat}')
with h5py.File(filename, 'r') as data:
acq_grid = data['acq_grid'][:]
n_fft, n_dop_bins = acq_grid.shape
d_max, f_max = np.unravel_index(np.argmax(acq_grid), acq_grid.shape)
doppler_step = data['doppler_step'][0]
doppler_max = data['doppler_max'][0]
freq = np.arange(n_dop_bins) * doppler_step - doppler_max
delay = np.arange(n_fft) / n_fft * n_chips
# Plot data
# --- Acquisition grid (3D)
fig = plt.figure()
plt.gcf().canvas.manager.set_window_title(filename)
if not lite_view:
ax = fig.add_subplot(111, projection='3d')
X, Y = np.meshgrid(freq, delay)
ax.plot_surface(X, Y, acq_grid, cmap='viridis')
ax.set_ylim([min(delay), max(delay)])
else:
delay_interp = (np.arange(n_samples_per_chip * n_chips)
/ n_samples_per_chip)
spline = CubicSpline(delay, acq_grid)
grid_interp = spline(delay_interp)
ax = fig.add_subplot(111, projection='3d')
X, Y = np.meshgrid(freq, delay_interp)
ax.plot_surface(X, Y, grid_interp, cmap='inferno')
ax.set_ylim([min(delay_interp), max(delay_interp)])
ax.set_xlabel('Doppler shift (Hz)')
ax.set_xlim([min(freq), max(freq)])
ax.set_ylabel('Code delay (chips)')
ax.set_zlabel('Test Statistics')
plt.tight_layout()
plt.savefig(img_name_root + '_sample_3D.png')
plt.show()
# --- Acquisition grid (2D)
input_power = 100 # Change Test statistics in Doppler wipe-off plot
fig2, axes = plt.subplots(2, 1, figsize=(8, 6))
plt.gcf().canvas.manager.set_window_title(filename)
axes[0].plot(freq, acq_grid[d_max, :])
axes[0].set_xlim([min(freq), max(freq)])
axes[0].set_xlabel('Doppler shift (Hz)')
axes[0].set_ylabel('Test statistics')
axes[0].set_title(f'Fixed code delay to {(d_max - 1) / n_fft * n_chips} '
f'chips')
normalization = (d_samples_per_code**4) * input_power
axes[1].plot(delay, acq_grid[:, f_max] / normalization)
axes[1].set_xlim([min(delay), max(delay)])
axes[1].set_xlabel('Code delay (chips)')
axes[1].set_ylabel('Test statistics')
axes[1].set_title(f'Doppler wipe-off = '
f'{str((f_max-1) * doppler_step - doppler_max)} Hz')
plt.tight_layout()
plt.savefig(img_name_root + '_sample_2D.png')
plt.show()
else:
# ---------- CHANGE HERE:
lite_view = True
# If lite_view -> sets the number of samples per chip in the graphical
# representation
n_samples_per_chip = 3
d_samples_per_code = 25000
filenames = os.listdir(path)
for filename in filenames:
sat = 1
channel = 0
execution = 1
system = filename[12]
signal = filename[14:16]
if system == "G":
if signal == "1C":
n_chips = 1023
elif signal == "2S" or "L5":
n_chips = 10230
else:
print("Incorrect files format. Change the code or the "
"filenames.")
sys.exit()
elif system == "E":
if signal == "1B":
n_chips = 4092
elif signal == "5X":
n_chips = 10230
else:
print("Incorrect files format. Change the code or the "
"filenames.")
sys.exit()
elif system == "R":
if signal == "1G" or "2G":
n_chips = 511
else:
print("Incorrect files format. Change the code or the "
"filenames.")
sys.exit()
elif system == "C":
if signal == "B1":
n_chips = 2048
elif signal == "B3" or "5C":
n_chips = 10230
else:
print("Incorrect files format. Change the code or the "
"filenames.")
sys.exit()
complete_path = path + filename
with h5py.File(complete_path, 'r') as data:
acq_grid = data['acq_grid'][:]
n_fft, n_dop_bins = acq_grid.shape
d_max, f_max = np.unravel_index(np.argmax(acq_grid),
acq_grid.shape)
doppler_step = data['doppler_step'][0]
doppler_max = data['doppler_max'][0]
freq = np.arange(n_dop_bins) * doppler_step - doppler_max
delay = np.arange(n_fft) / n_fft * n_chips
# Plot data
# --- Acquisition grid (3D)
fig = plt.figure()
plt.gcf().canvas.manager.set_window_title(filename)
if not lite_view:
ax = fig.add_subplot(111, projection='3d')
X, Y = np.meshgrid(freq, delay)
ax.plot_surface(X, Y, acq_grid, cmap='viridis')
ax.set_ylim([min(delay), max(delay)])
else:
delay_interp = (np.arange(n_samples_per_chip * n_chips)
/ n_samples_per_chip)
spline = CubicSpline(delay, acq_grid)
grid_interp = spline(delay_interp)
ax = fig.add_subplot(111, projection='3d')
X, Y = np.meshgrid(freq, delay_interp)
ax.plot_surface(X, Y, grid_interp, cmap='inferno')
ax.set_ylim([min(delay_interp), max(delay_interp)])
ax.set_xlabel('Doppler shift (Hz)')
ax.set_xlim([min(freq), max(freq)])
ax.set_ylabel('Code delay (chips)')
ax.set_zlabel('Test Statistics')
plt.savefig(os.path.join(fig_path, filename[:-4]) + '_3D.png')
plt.close()
# --- Acquisition grid (2D)
input_power = 100 # Change Test statistics in Doppler wipe-off plot
fig2, axes = plt.subplots(2, 1, figsize=(8, 6))
plt.gcf().canvas.manager.set_window_title(filename)
axes[0].plot(freq, acq_grid[d_max, :])
axes[0].set_xlim([min(freq), max(freq)])
axes[0].set_xlabel('Doppler shift (Hz)')
axes[0].set_ylabel('Test statistics')
axes[0].set_title(f'Fixed code delay to '
f'{(d_max - 1) / n_fft * n_chips} chips')
normalization = (d_samples_per_code ** 4) * input_power
axes[1].plot(delay, acq_grid[:, f_max] / normalization)
axes[1].set_xlim([min(delay), max(delay)])
axes[1].set_xlabel('Code delay (chips)')
axes[1].set_ylabel('Test statistics')
axes[1].set_title(f'Doppler wipe-off = '
f'{str((f_max - 1) * doppler_step - doppler_max)} '
f'Hz')
plt.tight_layout()
plt.savefig(os.path.join(fig_path, filename[:-4]) + '_2D.png')
# plt.show()
plt.close()

View File

@@ -0,0 +1,76 @@
"""
plot_tracking_quality_indicators.py
Irene Pérez Riega, 2023. iperrie@inta.es
Modifiable in the file:
channels - Number of channels
firs_channel - Number of the first channel
path - Path to folder which contains raw files
fig_path - Path where doppler plots will be save
'trk_dump_ch' - Fixed part of the tracking dump files names
-----------------------------------------------------------------------------
GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
This file is part of GNSS-SDR.
Copyright (C) 2022 (see AUTHORS file for a list of contributors)
SPDX-License-Identifier: GPL-3.0-or-later
-----------------------------------------------------------------------------
"""
import matplotlib.pyplot as plt
import numpy as np
import os
from lib.dll_pll_veml_read_tracking_dump import dll_pll_veml_read_tracking_dump
GNSS_tracking = []
plot_names = []
# ---------- CHANGE HERE:
channels = 5
first_channel = 0
path = '/home/labnav/Desktop/TEST_IRENE/tracking'
fig_path = '/home/labnav/Desktop/TEST_IRENE/PLOTS/TrackingQualityIndicator'
for N in range(1, channels + 1):
tracking_log_path = os.path.join(path,
f'trk_dump_ch{N-1+first_channel}.dat')
GNSS_tracking.append(dll_pll_veml_read_tracking_dump(tracking_log_path))
if not os.path.exists(fig_path):
os.makedirs(fig_path)
# Plot tracking quality indicators
# First plot
plt.figure()
plt.gcf().canvas.manager.set_window_title('Carrier lock test output for all '
'the channels')
plt.title('Carrier lock test output for all the channels')
for n in range(len(GNSS_tracking)):
plt.plot(GNSS_tracking[n]['carrier_lock_test'])
plot_names.append(f'SV {str(round(np.mean(GNSS_tracking[n]["PRN"])))}')
plt.legend(plot_names)
plt.savefig(os.path.join(fig_path,
f'carrier_lock_test '
f'{str(round(np.mean(GNSS_tracking[n]["PRN"])))}'))
plt.show()
# Second plot
plt.figure()
plt.gcf().canvas.manager.set_window_title('Carrier CN0 output for all the '
'channels')
plt.title('Carrier CN0 output for all the channels')
for n in range(len(GNSS_tracking)):
plt.plot(GNSS_tracking[n]['CN0_SNV_dB_Hz'])
plot_names.append(f'CN0_SNV_dB_Hz '
f'{str(round(np.mean(GNSS_tracking[n]["PRN"])))}')
plt.legend(plot_names)
plt.savefig(os.path.join(
fig_path, f'SV {str(round(np.mean(GNSS_tracking[n]["PRN"])))}'))
plt.show()

View File

@@ -0,0 +1,160 @@
[GNSS-SDR]
; SPDX-FileCopyrightText: Carles Fernandez-Prades, 2018 carles.fernandez@cttc.es
; SPDX-License-Identifier: GPL-3.0-or-later
;######### GLOBAL OPTIONS ##################
;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second].
GNSS-SDR.internal_fs_sps=3000000
;######### SIGNAL_SOURCE CONFIG ############
;#implementation: Use [File_Signal_Source] or [UHD_Signal_Source]
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=./data/L125_III1b_210s_L2_3Msps.bin ; <- Available at https://zenodo.org/record/1184601
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=ibyte
;#sampling_frequency: Original Signal sampling frequency in [Hz]
SignalSource.sampling_frequency=3000000
;#samples: Number of samples to be processed. Notice that 0 indicates the entire file.
SignalSource.samples=0
;#repeat: Repeat the processing file. Disable this option in this version
SignalSource.repeat=false
;#dump: Dump the Signal source data to a file. Disable this option in this version
SignalSource.dump=false
;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing.
; it helps to not overload the CPU, but the processing time will be longer.
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
;######### DATA_TYPE_ADAPTER CONFIG ############
DataTypeAdapter.implementation=Ibyte_To_Complex
;######### INPUT_FILTER CONFIG ############
InputFilter.implementation=Pass_Through
;######### RESAMPLER CONFIG ############
Resampler.implementation=Pass_Through
Resampler.item_type=gr_complex
;######### CHANNELS GLOBAL CONFIG ############
Channels_2S.count=10
;Channels.in_acquisition=1
Channel0.signal=2S
Channel1.signal=2S
Channel2.signal=2S
Channel3.signal=2S
Channel4.signal=2S
Channel5.signal=2S
Channel6.signal=2S
Channel7.signal=2S
Channel8.signal=2S
Channel9.signal=2S
;######### SPECIFIC CHANNELS CONFIG ######
Channel0.satellite=19
;######### ACQUISITION GLOBAL CONFIG ############
Acquisition_2S.implementation=GPS_L2_M_PCPS_Acquisition
Acquisition_2S.item_type=gr_complex
Acquisition_2S.doppler_max=4500
Acquisition_2S.doppler_step=125
Acquisition_2S.use_CFAR_algorithm=false
Acquisition_2S.threshold=10
Acquisition_2S.blocking=true
;######### TRACKING GLOBAL CONFIG ############
Tracking_2S.implementation=GPS_L2_M_DLL_PLL_Tracking
Tracking_2S.item_type=gr_complex
Tracking_2S.pll_bw_hz=4.0;
Tracking_2S.dll_bw_hz=1;
Tracking_2S.early_late_space_chips=0.5;
Tracking_2S.dump=true
Tracking_2S.dump_filename=./data/track_ch_
;######### TELEMETRY DECODER CONFIG ############
TelemetryDecoder_2S.implementation=GPS_L2C_Telemetry_Decoder
;######### OBSERVABLES CONFIG ############
Observables.implementation=Hybrid_Observables
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=Single; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.iono_model=OFF; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=OFF; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time [ms]
PVT.output_rate_ms=100
;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms.
PVT.display_rate_ms=500
;# KML, GeoJSON, NMEA and RTCM output configuration
;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump.
PVT.dump_filename=./data/access18
;#nmea_dump_filename: NMEA log path and filename
PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea
;#flag_nmea_tty_port: Enable or disable the NMEA log to a serial TTY port (Can be used with real hardware or virtual one)
PVT.flag_nmea_tty_port=false
;#nmea_dump_devname: serial device descriptor for NMEA logging
PVT.nmea_dump_devname=/dev/pts/4
PVT.flag_rtcm_server=false
PVT.rtcm_tcp_port=2101
PVT.rtcm_station_id=1234
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=true
PVT.elevation_mask=5

View File

@@ -0,0 +1,72 @@
## Continuous Reproducibility in GNSS Signal Processing
<!-- prettier-ignore-start -->
[comment]: # (
SPDX-License-Identifier: GPL-3.0-or-later
)
[comment]: # (
SPDX-FileCopyrightText: 2018 Carles Fernandez-Prades <carles.fernandez@cttc.es>
)
<!-- prettier-ignore-end -->
This folder contains files required for the reproduction of the experiment
proposed in:
C. Fern&aacute;ndez-Prades, J. Vil&agrave;-Valls, J. Arribas and A. Ramos,
[_Continuous Reproducibility in GNSS Signal Processing_](https://ieeexplore.ieee.org/document/8331069/),
IEEE Access, Vol. 6, No. 1, pp. 20451-20463, April 2018. DOI:
[10.1109/ACCESS.2018.2822835](https://doi.org/10.1109/ACCESS.2018.2822835)
The data set used in this paper is available at
https://zenodo.org/records/1184601
The sample format is `ibyte`: Interleaved (I&Q) stream of samples of type signed
integer, 8-bit twos complement number ranging from -128 to 127. The sampling
rate is 3 MSps.
The figure appearing in that paper can be automatically generated with the
pipeline available at https://gitlab.com/gnss-sdr/gnss-sdr/pipelines
After the **Build** stage, which compiles the source code in several versions of
the most popular GNU/Linux distributions, and the **Test** stage, which executes
GNSS-SDRs QA code, the **Deploy** stage creates and publishes an image of a
software container ready to execute the experiment. This container is available
by doing:
```
$ docker pull carlesfernandez/docker-gnsssdr:access18
```
Then, in the **Experiment** stage, a job installs the image created in the
previous step, grabs the data file, executes the experiment and produces a
figure with the obtained results.
The steps to reproduce the experiment in your own machine (with
[Docker](https://www.docker.com) already installed and running) are:
```
$ docker pull carlesfernandez/docker-gnsssdr:access18
$ docker run -it -v $PWD/access18:/home/access18 carlesfernandez/docker-gnsssdr:access18
$ git clone https://github.com/gnss-sdr/gnss-sdr
$ cd gnss-sdr
$ git checkout next
$ mkdir -p exp-access18/data
$ cd exp-access18/data
$ curl https://zenodo.org/records/1184601/files/L2_signal_samples.tar.xz --output L2_signal_samples.tar.xz
$ tar xvfJ L2_signal_samples.tar.xz
$ echo "3a04c1eeb970776bb77f5e3b7eaff2df L2_signal_samples.tar.xz" > data.md5
$ md5sum -c data.md5
$ cd ..
$ cp ../utils/reproducibility/ieee-access18/L2-access18.conf .
$ cp ../utils/reproducibility/ieee-access18/plot_dump.m .
$ cp -r ../utils/matlab/libs/geoFunctions .
$ gnss-sdr --c=L2-access18.conf
$ octave --no-gui plot_dump.m
$ epspdf Figure2.eps Figure2.pdf
$ cp Figure2.pdf /home/access18/
$ exit
```
You will find the file `Figure2.pdf` in a newly created folder called
`access18`.

View File

@@ -0,0 +1,219 @@
% -------------------------------------------------------------------------
%
% GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
% This file is part of GNSS-SDR.
%
% SPDX-License-Identifier: GPL-3.0-or-later
% SPDX-FileCopyrightText: Antonio Ramos, 2018. antonio.ramos(at)cttc.es
%
% -------------------------------------------------------------------------
clear all;
clc;
n_channel = 0;
symbol_period = 20e-3;
filename = 'track_ch_';
fontsize = 12;
addpath('./data') % Path to gnss-sdr dump files (Tracking and PVT)
addpath('./geoFunctions')
load([filename int2str(n_channel) '.mat']);
t = (0 : length(abs_P) - 1) * symbol_period;
hf = figure('visible', 'off');
set(hf, 'paperorientation', 'landscape');
subplot(3, 3, [1,3])
plot(t, abs_E, t, abs_P, t, abs_L)
xlabel('Time [s]','fontname','Times','fontsize', fontsize)
ylabel('Correlation result','fontname','Times','fontsize', fontsize)
legend('Early', 'Prompt', 'Late')
grid on
subplot(3, 3, 7)
plot(Prompt_I./1000, Prompt_Q./1000, 'linestyle', 'none', 'marker', '.')
xlabel('I','fontname','Times','fontsize', fontsize)
ylabel('Q','fontname','Times','fontsize', fontsize)
axis equal
grid on
subplot(3, 3, [4,6])
plot(t, Prompt_I)
xlabel('Time [s]','fontname','Times','fontsize', fontsize)
ylabel('Navigation data bits','fontname','Times','fontsize', fontsize)
grid on
fileID = fopen('data/access18.dat', 'r');
dinfo = dir('data/access18.dat');
filesize = dinfo.bytes;
aux = 1;
while ne(ftell(fileID), filesize)
navsol.TOW_at_current_symbol_ms(aux) = fread(fileID, 1, 'uint32');
navsol.week(aux) = fread(fileID, 1, 'uint32');
navsol.RX_time(aux) = fread(fileID, 1, 'double');
navsol.user_clock_offset(aux) = fread(fileID, 1, 'double');
navsol.X(aux) = fread(fileID, 1, 'double');
navsol.Y(aux) = fread(fileID, 1, 'double');
navsol.Z(aux) = fread(fileID, 1, 'double');
navsol.VX(aux) = fread(fileID, 1, 'double');
navsol.VY(aux) = fread(fileID, 1, 'double');
navsol.VZ(aux) = fread(fileID, 1, 'double');
navsol.varXX(aux) = fread(fileID, 1, 'double');
navsol.varYY(aux) = fread(fileID, 1, 'double');
navsol.varZZ(aux) = fread(fileID, 1, 'double');
navsol.varXY(aux) = fread(fileID, 1, 'double');
navsol.varYZ(aux) = fread(fileID, 1, 'double');
navsol.varZX(aux) = fread(fileID, 1, 'double');
navsol.latitude(aux) = fread(fileID, 1, 'double');
navsol.longitude(aux) = fread(fileID, 1, 'double');
navsol.height(aux) = fread(fileID, 1, 'double');
navsol.number_sats(aux) = fread(fileID, 1, 'uint8');
navsol.solution_status(aux) = fread(fileID, 1, 'uint8');
navsol.solution_type(aux) = fread(fileID, 1, 'uint8');
navsol.AR_ratio_factor(aux) = fread(fileID, 1, 'float');
navsol.AR_ratio_threshold(aux) = fread(fileID, 1, 'float');
navsol.GDOP(aux) = fread(fileID, 1, 'double');
navsol.PDOP(aux) = fread(fileID, 1, 'double');
navsol.HDOP(aux) = fread(fileID, 1, 'double');
navsol.VDOP(aux) = fread(fileID, 1, 'double');
aux = aux + 1;
end
fclose(fileID);
mean_Latitude = mean(navsol.latitude);
mean_Longitude = mean(navsol.longitude);
mean_h = mean(navsol.height);
utmZone = findUtmZone(mean_Latitude, mean_Longitude);
[ref_X_cart, ref_Y_cart, ref_Z_cart] = geo2cart(dms2mat(deg2dms(mean_Latitude)), dms2mat(deg2dms(mean_Longitude)), mean_h, 5);
[mean_utm_X, mean_utm_Y, mean_utm_Z] = cart2utm(ref_X_cart, ref_Y_cart, ref_Z_cart, utmZone);
numPoints = length(navsol.X);
aux = 0;
for n = 1:numPoints
aux = aux+1;
[E(aux), N(aux), U(aux)] = cart2utm(navsol.X(n), navsol.Y(n), navsol.Z(n), utmZone);
end
v_2d = [E;N].'; % 2D East Nort position vectors
v_3d = [E;N;U].'; % 2D East Nort position vectors
%% ACCURACY
% 2D -------------------
sigma_E_accuracy = sqrt((1/(numPoints-1)) * sum((v_2d(:,1) - mean_utm_X).^2));
sigma_N_accuracy = sqrt((1/(numPoints-1)) * sum((v_2d(:,2) - mean_utm_Y).^2));
sigma_ratio_2d_accuracy = sigma_N_accuracy / sigma_E_accuracy
% if sigma_ratio=1 -> Prob in circle with r=DRMS -> 65%
DRMS_accuracy = sqrt(sigma_E_accuracy^2 + sigma_N_accuracy^2)
% if sigma_ratio=1 -> Prob in circle with r=2DRMS -> 95%
TWO_DRMS_accuracy = 2 * DRMS_accuracy
% if sigma_ratio>0.3 -> Prob in circle with r=CEP -> 50%
CEP_accuracy = 0.62 * sigma_E_accuracy + 0.56 * sigma_N_accuracy
% 3D -------------------
sigma_U_accuracy = sqrt((1/(numPoints-1)) * sum((v_3d(:,3) - mean_utm_Z).^2));
% if sigma_ratio=1 -> Prob in circle with r=DRMS -> 50%
SEP_accuracy = 0.51 * sqrt(sigma_E_accuracy^2 + sigma_N_accuracy^2 + sigma_U_accuracy^2)
% if sigma_ratio=1 -> Prob in circle with r=DRMS -> 61%
MRSE_accuracy = sqrt(sigma_E_accuracy^2 + sigma_N_accuracy^2 + sigma_U_accuracy^2)
% if sigma_ratio=1 -> Prob in circle with r=2DRMS -> 95%
TWO_MRSE_accuracy=2 * MRSE_accuracy
%% PRECISION
% 2D Mean and Variance
mean_2d = [mean(v_2d(:,1)) ; mean(v_2d(:,2))];
sigma_2d = [sqrt(var(v_2d(:,1))) ; sqrt(var(v_2d(:,2)))];
sigma_ratio_2d = sigma_2d(2) / sigma_2d(1)
% if sigma_ratio=1 -> Prob in circle with r=DRMS -> 65%
DRMS = sqrt(sigma_2d(1)^2 + sigma_2d(2)^2)
% if sigma_ratio=1 -> Prob in circle with r=2DRMS -> 95%
TWO_DRMS = 2 * DRMS
% if sigma_ratio>0.3 -> Prob in circle with r=CEP -> 50%
CEP = 0.62 * sigma_2d(1) + 0.56 * sigma_2d(2)
% 3D Mean and Variance
mean_3d = [mean(v_3d(:,1)) ; mean(v_3d(:,2)) ; mean(v_3d(:,3))];
sigma_3d = [sqrt(var(v_3d(:,1))) ; sqrt(var(v_3d(:,2))) ; sqrt(var(v_3d(:,3)))];
% absolute mean error
error_2D_vec = [mean_utm_X-mean_2d(1) mean_utm_Y-mean_2d(2)];
error_2D_m = norm(error_2D_vec)
error_3D_vec = [mean_utm_X-mean_3d(1) mean_utm_Y-mean_3d(2) mean_utm_Z-mean_3d(3)];
error_3D_m = norm(error_3D_vec)
RMSE_X = sqrt(mean((v_3d(:,1)-mean_utm_X).^2))
RMSE_Y = sqrt(mean((v_3d(:,2)-mean_utm_Y).^2))
RMSE_Z = sqrt(mean((v_3d(:,3)-mean_utm_Z).^2))
RMSE_2D = sqrt(mean((v_2d(:,1)-mean_utm_X).^2 + (v_2d(:,2)-mean_utm_Y).^2))
RMSE_3D = sqrt(mean((v_3d(:,1)-mean_utm_X).^2 + (v_3d(:,2)-mean_utm_Y).^2 + (v_3d(:,3)-mean_utm_Z).^2))
% if sigma_ratio=1 -> Prob in circle with r=DRMS -> 50%
SEP = 0.51 * sqrt(sigma_3d(1)^2 + sigma_3d(2)^2 + sigma_3d(3)^2)
% if sigma_ratio=1 -> Prob in circle with r=DRMS -> 61%
MRSE = sqrt(sigma_3d(1)^2 + sigma_3d(2)^2 + sigma_3d(3)^2)
% if sigma_ratio=1 -> Prob in circle with r=2DRMS -> 95%
TWO_MRSE = 2 * MRSE
%% SCATTER PLOT 2D
subplot(3,3,8)
scatter(v_2d(:,1)-mean_2d(1), v_2d(:,2)-mean_2d(2));
hold on;
plot(0, 0, 'k*');
[x,y,z] = cylinder([TWO_DRMS TWO_DRMS], 200);
plot(x(1,:), y(1,:), 'Color', [0 0.6 0]);
str = strcat('2DRMS=', num2str(TWO_DRMS), ' m');
text(cosd(65)*TWO_DRMS, sind(65)*TWO_DRMS, str, 'Color', [0 0.6 0]);
[x,y,z] = cylinder([CEP CEP], 200);
plot(x(1,:), y(1,:), 'r--');
str = strcat('CEP=', num2str(CEP), ' m');
text(cosd(80)*CEP, sind(80)*CEP, str, 'Color','r');
grid on
axis equal;
xlabel('North [m]','fontname','Times','fontsize', fontsize)
ylabel('East [m]','fontname','Times','fontsize', fontsize)
%% SCATTER PLOT 3D
subplot(3,3,9)
scatter3(v_3d(:,1)-mean_3d(1), v_3d(:,2)-mean_3d(2), v_3d(:,3)-mean_3d(3));
hold on;
[x,y,z] = sphere();
hSurface = surf(MRSE*x, MRSE*y, MRSE*z); % sphere centered at origin
set(hSurface, 'facecolor', 'none', 'edgecolor', [0 0.6 0], 'edgealpha', 1, 'facealpha', 1);
xlabel('North [m]', 'fontname', 'Times', 'fontsize', fontsize-2)
ylabel('East [m]', 'fontname', 'Times', 'fontsize', fontsize-2)
zlabel('Up [m]', 'fontname', 'Times', 'fontsize', fontsize-2)
str = strcat('MRSE=', num2str(MRSE), ' m')
text(cosd(45)*MRSE, sind(45)*MRSE, 20, str, 'Color', [0 0.6 0]);
a = gca;
set(a, 'fontsize', fontsize-6)
hh = findall(hf, '-property', 'FontName');
set(hh, 'FontName', 'Times');
print(hf, 'Figure2.eps', '-depsc')
close(hf);

View File

@@ -0,0 +1,139 @@
# GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
# This file is part of GNSS-SDR.
#
# SPDX-FileCopyrightText: 2010-2020 C. Fernandez-Prades cfernandez(at)cttc.es
# SPDX-License-Identifier: BSD-3-Clause
if("${ARMADILLO_VERSION_STRING}" VERSION_GREATER "9.800" OR (NOT ARMADILLO_FOUND) OR ENABLE_OWN_ARMADILLO) # requires back(), introduced in Armadillo 9.800
message(STATUS "The obsdiff utility tool will be built when doing '${CMAKE_MAKE_PROGRAM_PRETTY_NAME}'")
if(NOT GNSSTK_FOUND AND NOT ENABLE_OWN_GNSSTK)
find_package(GNSSTK)
endif()
if(NOT GNSSTK_FOUND OR ENABLE_OWN_GNSSTK)
include(GNUInstallDirs)
if(GNSSTK_USES_GPSTK_NAMESPACE)
set(GNSSTK_LIBRARY ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/${CMAKE_INSTALL_LIBDIR}/${CMAKE_FIND_LIBRARY_PREFIXES}gpstk${CMAKE_STATIC_LIBRARY_SUFFIX})
set(GNSSTK_INCLUDE_DIR ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/include)
else()
set(GNSSTK_LIBRARY ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/${CMAKE_INSTALL_LIBDIR}/${CMAKE_FIND_LIBRARY_PREFIXES}gnsstk${CMAKE_STATIC_LIBRARY_SUFFIX})
set(GNSSTK_INCLUDE_DIR ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/include)
endif()
endif()
if(USE_CMAKE_TARGET_SOURCES)
add_executable(obsdiff)
target_sources(obsdiff
PRIVATE
obsdiff.cc
obsdiff_flags.h
)
else()
source_group(Headers FILES obsdiff_flags.h)
add_executable(obsdiff ${CMAKE_CURRENT_SOURCE_DIR}/obsdiff.cc obsdiff_flags.h)
endif()
target_include_directories(obsdiff PUBLIC ${GNSSSDR_SOURCE_DIR}/tests/common-files)
if(GNSSTK_VERSION AND "${GNSSTK_VERSION}" VERSION_LESS 3.0.1)
set_property(TARGET obsdiff PROPERTY CXX_STANDARD 14) # Required by GPSTk v3.0.0
endif()
# Do not show warnings raised by GPSTk v3.0.0
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
target_compile_options(obsdiff
PRIVATE
-Wno-deprecated -Wno-unused-parameter -Wno-sign-compare -Wno-reorder -Wno-deprecated-copy -Wno-extra -Wno-unused-but-set-variable -Wno-unknown-pragmas
)
endif()
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
target_compile_options(obsdiff
PRIVATE
-Wno-deprecated -Wno-unused-parameter -Wno-sign-compare -Wno-reorder
)
endif()
if(NOT ARMADILLO_FOUND OR ENABLE_OWN_ARMADILLO)
add_dependencies(obsdiff armadillo-${armadillo_RELEASE})
endif()
if(ENABLE_GLOG_AND_GFLAGS)
if(NOT GFLAGS_FOUND)
add_dependencies(obsdiff gflags-${GNSSSDR_GFLAGS_LOCAL_VERSION})
endif()
endif()
if(NOT GNSSTK_FOUND OR ENABLE_OWN_GNSSTK)
add_dependencies(obsdiff gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION})
endif()
if(NOT MATIO_FOUND OR MATIO_VERSION_STRING VERSION_LESS ${GNSSSDR_MATIO_MIN_VERSION})
add_dependencies(obsdiff matio-${GNSSSDR_MATIO_LOCAL_VERSION})
endif()
if(GNSSTK_USES_GPSTK_NAMESPACE)
target_compile_definitions(obsdiff PRIVATE -DGNSSTK_USES_GPSTK_NAMESPACE=1)
endif()
if(GNSSTK_OLDER_THAN_8)
target_compile_definitions(obsdiff PRIVATE -DOLD_GPSTK=1)
endif()
if(GNSSTK_OLDER_THAN_9)
target_compile_definitions(obsdiff PRIVATE -DGNSSTK_OLDER_THAN_9=1)
endif()
if(GNSSTK_OLDER_THAN_13)
target_compile_definitions(obsdiff PRIVATE -DGNSSTK_OLDER_THAN_13=1)
endif()
if(NOT TARGET Gnsstk::gnsstk)
if(GNSSTK_USES_GPSTK_NAMESPACE)
file(MAKE_DIRECTORY ${GNSSTK_INCLUDE_DIR}/gpstk)
add_library(Gnsstk::gnsstk STATIC IMPORTED)
add_dependencies(Gnsstk::gnsstk gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION})
set_target_properties(Gnsstk::gnsstk PROPERTIES
IMPORTED_LINK_INTERFACE_LANGUAGES "CXX"
IMPORTED_LOCATION "${GNSSTK_LIBRARY}"
INTERFACE_INCLUDE_DIRECTORIES "${GNSSTK_INCLUDE_DIR};${GNSSTK_INCLUDE_DIR}/gpstk"
INTERFACE_LINK_LIBRARIES "${GNSSTK_LIBRARY}"
)
else()
file(MAKE_DIRECTORY ${GNSSTK_INCLUDE_DIR}/gnsstk)
add_library(Gnsstk::gnsstk STATIC IMPORTED)
add_dependencies(Gnsstk::gnsstk gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION})
set_target_properties(Gnsstk::gnsstk PROPERTIES
IMPORTED_LINK_INTERFACE_LANGUAGES "CXX"
IMPORTED_LOCATION "${GNSSTK_LIBRARY}"
INTERFACE_INCLUDE_DIRECTORIES "${GNSSTK_INCLUDE_DIR};${GNSSTK_INCLUDE_DIR}/gnsstk"
INTERFACE_LINK_LIBRARIES "${GNSSTK_LIBRARY}"
)
endif()
endif()
target_link_libraries(obsdiff
PRIVATE
Armadillo::armadillo
Threads::Threads
Matio::matio
Gnsstk::gnsstk
)
if(ENABLE_GLOG_AND_GFLAGS)
target_link_libraries(obsdiff PRIVATE Gflags::gflags)
target_compile_definitions(obsdiff PRIVATE -DUSE_GLOG_AND_GFLAGS=1)
else()
target_link_libraries(obsdiff PUBLIC absl::flags absl::flags_parse)
endif()
if(ENABLE_STRIP)
set_target_properties(obsdiff PROPERTIES LINK_FLAGS "-s")
endif()
include(XcodeRemoveWarningDuplicates)
xcode_remove_warning_duplicates(obsdiff)
add_custom_command(TARGET obsdiff POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:obsdiff>
${LOCAL_INSTALL_BASE_DIR}/install/$<TARGET_FILE_NAME:obsdiff>
)
install(TARGETS obsdiff
RUNTIME DESTINATION bin
COMPONENT "obsdiff"
)
else()
message(STATUS "The Armadillo library version found (${ARMADILLO_VERSION_STRING}) is older than 9.800.")
message(STATUS " The obsdiff utility tool will not be built.")
message(STATUS " You could build it by setting -DENABLE_OWN_ARMADILLO=ON")
endif()

142
utils/rinex-tools/README.md Normal file
View File

@@ -0,0 +1,142 @@
## obsdiff
<!-- prettier-ignore-start -->
[comment]: # (
SPDX-License-Identifier: GPL-3.0-or-later
)
[comment]: # (
SPDX-FileCopyrightText: 2020 Carles Fernandez-Prades <carles.fernandez@cttc.es>
)
<!-- prettier-ignore-end -->
This program computes single-differences and double-differences from RINEX
observation files.
### Building
Requirements:
- [Armadillo](https://arma.sourceforge.net/): A C++ library for linear algebra
and scientific computing. This program requires version 9.800 or higher. If
your installed Armadillo version is older, see below.
- [Gflags](https://github.com/gflags/gflags): A C++ library that implements
command-line flags processing. If not found in your system, the latest version
will be downloaded, built and linked for you at building time.
- [GNSSTK](https://github.com/SGL-UT/gnsstk): The GNSSTk C++ Library, used for
RINEX files reading. If not found in your system, the latest version will be
downloaded, built and linked for you at building time.
- [Matio](https://github.com/tbeu/matio): A MATLAB MAT File I/O Library,
version >= 1.5.3. If it is not found, or an older version is found, CMake will
download, build and link a recent version for you at building time.
Optional:
- [Gnuplot](http://www.gnuplot.info/): a portable command-line driven graphing
utility.
This program is built along with GNSS-SDR if the options
`ENABLE_UNIT_TESTING_EXTRA` or `ENABLE_SYSTEM_TESTING_EXTRA` are set to `ON`
when calling CMake:
```
$ cmake -DENABLE_SYSTEM_TESTING_EXTRA=ON ..
$ make obsdiff
$ sudo make install
```
The last step is optional. Without it, you still will get the executable at
`../install/obsdiff`.
This program requires Armadillo 9.800 or higher. If the available Armadillo
version is older, this program will not be built. If your local Armadillo
installed version is older than 9.800, you can force CMake to download, build
and link a recent one:
```
$ cmake -DENABLE_SYSTEM_TESTING_EXTRA=ON -DENABLE_OWN_ARMADILLO=ON ..
$ make obsdiff
$ sudo make install
```
This later option requires [BLAS](http://www.netlib.org/blas/),
[LAPACK](http://www.netlib.org/lapack/) and
[GFortran](https://gcc.gnu.org/fortran/) already installed in your system.
### Usage
Double differences (Pseudorange, Carrier Phase and Carrier Doppler) within Base
and Rover receivers:
```
$ obsdiff --base_rinex_obs=base.20o --rover_rinex_obs=rover.20o
```
Double differences with receiver clock correction (Pseudorange, Carrier Phase
and Carrier Doppler) within Base and Rover receivers:
```
$ obsdiff --base_rinex_obs=base.20o --rover_rinex_obs=rover.20o --rinex_nav=base.nav --remove_rx_clock_error=true
```
Single difference (Pseudorange, Carrier Phase and Carrier Doppler) with Base
receiver only and a special duplicated satellites simulated scenario:
```
$ obsdiff --rover_rinex_obs=rover.20o --single_diff=true --dupli_sat=true --dupli_sat_prns=1,2,3,4
```
Where the list of duplicated satellites PRN pairs must be specified by
`--dupli_sat_prns` flag (_i.e.,_ `1,2,3,4` indicates that the PRNs 1,2 share the
same orbit. The same applies for PRNs 3,4)
Single difference of Pseudorange Rate vs. Carrier Phase rate for each satellite:
```
$ obsdiff --rover_rinex_obs=rover.20o --single_diff=true
```
There is some flexibility in how command-line flags may be specified. The
following examples are equivalent:
```
$ obsdiff -base_rinex_obs=reference.20o
$ obsdiff --base_rinex_obs=reference.20o
$ obsdiff -base_rinex_obs reference.20o
$ obsdiff --base_rinex_obs reference.20o
```
For boolean flags, the possibilities are slightly different:
```
$ obsdiff --single_diffs
$ obsdiff --nosingle_diffs
$ obsdiff --single_diffs=true
$ obsdiff --single_diffs=false
```
(as well as the single-dash variant on all of these).
Despite this flexibility, we recommend using only a single form:
`--variable=value` for non-boolean flags, and `--variable/--novariable` for
boolean flags.
Available command-line flags:
<!-- prettier-ignore-start -->
| **Command-line flag** | **Default value** | **Description** |
|:-------------------------:|:-----------------:|:-----------------|
| `--skip_obs_transitory_s` | `30.0` | Skip the initial observable outputs to avoid transitory results [s]. |
| `--skip_obs_ends_s` | `5.0` | Skip the lasts observable outputs to avoid transitory results [s]. |
| `--single_diffs` | `false` | [`true`, `false`]: If `true`, the program also computes the single difference errors for [Carrier Phase](https://gnss-sdr.org/docs/sp-blocks/observables/#carrier-phase-measurement) and [Doppler](https://gnss-sdr.org/docs/sp-blocks/observables/#doppler-shift-measurement) measurements (requires LO synchronization between receivers). |
| `--compare_with_5X` | `false` | [`true`, `false`]: If `true`, the program compares the E5a Doppler and Carrier Phases with the E5 full Bw in RINEX (expect discrepancy due to the center frequencies difference). |
| `--dupli_sat` | `false` | [`true`, `false`]: If `true`, this flag enables special observable test mode where the scenario contains duplicated satellite orbits. |
| `--dupli_sat_prns` | `1,2,3,4` | List of duplicated satellites PRN pairs (_i.e._, `1,2,3,4` indicates that the PRNs 1,2 share the same orbit. The same applies for PRNs 3,4). |
| `--base_rinex_obs` | `base.obs` | Filename of reference RINEX observation file. |
| `--rover_rinex_obs` | `rover.obs` | Filename of tested RINEX observation file. |
| `--remove_rx_clock_error` | `false` | Compute and remove the receivers clock error prior to compute observable differences (requires a valid RINEX nav file for both receivers) |
| `--rinex_nav` | `base.nav` | Filename of reference RINEX navigation file. Only needed if `remove_rx_clock_error` is set to `true`. |
| `--system` | `G` | GNSS satellite system: `G` for GPS, `E` for Galileo. |
| `--signal` | `1C` | GNSS signal: `1C` for GPS L1 CA, `1B` for Galileo E1. |
| `--show_plots` | `true` | [`true`, `false`]: If `true`, and if [gnuplot](http://www.gnuplot.info/) is found on the system, displays results plots on screen. Please set it to `false` for non-interactive testing. |
<!-- prettier-ignore-end -->

1873
utils/rinex-tools/obsdiff.cc Normal file

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,57 @@
/*!
* \file obsdiff_flags.h
* \brief Helper file for unit testing
* \author Javier Arribas, 2020. jarribas(at)cttc.es
*
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_OBSDIFF_FLAGS_H
#define GNSS_SDR_OBSDIFF_FLAGS_H
#if USE_GLOG_AND_GFLAGS
#include <gflags/gflags.h>
DEFINE_double(skip_obs_transitory_s, 30.0, "Skip the initial observable outputs to avoid transitory results [s]");
DEFINE_double(skip_obs_ends_s, 5.0, "Skip the lasts observable outputs to avoid transitory results [s]");
DEFINE_bool(single_diffs, false, "Compute also the single difference errors for Accumulated Carrier Phase and Carrier Doppler (requires LO synchronization between receivers)");
DEFINE_bool(compare_with_5X, false, "Compare the E5a Doppler and Carrier Phases with the E5 full bw in RINEX (expect discrepancy due to the center frequencies difference)");
DEFINE_bool(dupli_sat, false, "Enable special observable test mode where the scenario contains duplicated satellite orbits");
DEFINE_bool(single_diff, false, "Enable special observable test mode using only rover observables");
DEFINE_string(dupli_sat_prns, "1,2,3,4", "List of duplicated satellites PRN pairs (i.e. 1,2,3,4 indicates that the PRNs 1,2 share the same orbit. The same applies for PRNs 3,4)");
DEFINE_string(base_rinex_obs, "base.obs", "Filename of reference RINEX observation file");
DEFINE_string(rinex_nav, "base.nav", "Filename of reference RINEX navigation file");
DEFINE_string(rover_rinex_obs, "base.obs", "Filename of test RINEX observation file");
DEFINE_string(system, "G", "GNSS satellite system: G for GPS, E for Galileo");
DEFINE_string(signal, "1C", "GNSS signal: 1C for GPS L1 CA, 1B for Galileo E1");
DEFINE_bool(remove_rx_clock_error, false, "Compute and remove the receivers clock error prior to compute observable differences (requires a valid RINEX nav file for both receivers)");
#else
#include <absl/flags/flag.h>
#include <cstdint>
#include <string>
ABSL_FLAG(double, skip_obs_transitory_s, 30.0, "Skip the initial observable outputs to avoid transitory results [s]");
ABSL_FLAG(double, skip_obs_ends_s, 5.0, "Skip the lasts observable outputs to avoid transitory results [s]");
ABSL_FLAG(bool, single_diffs, false, "Compute also the single difference errors for Accumulated Carrier Phase and Carrier Doppler (requires LO synchronization between receivers)");
ABSL_FLAG(bool, compare_with_5X, false, "Compare the E5a Doppler and Carrier Phases with the E5 full bw in RINEX (expect discrepancy due to the center frequencies difference)");
ABSL_FLAG(bool, dupli_sat, false, "Enable special observable test mode where the scenario contains duplicated satellite orbits");
ABSL_FLAG(bool, single_diff, false, "Enable special observable test mode using only rover observables");
ABSL_FLAG(std::string, dupli_sat_prns, "1,2,3,4", "List of duplicated satellites PRN pairs (i.e. 1,2,3,4 indicates that the PRNs 1,2 share the same orbit. The same applies for PRNs 3,4)");
ABSL_FLAG(std::string, base_rinex_obs, "base.obs", "Filename of reference RINEX observation file");
ABSL_FLAG(std::string, rinex_nav, "base.nav", "Filename of reference RINEX navigation file");
ABSL_FLAG(std::string, rover_rinex_obs, "base.obs", "Filename of test RINEX observation file");
ABSL_FLAG(std::string, system, "G", "GNSS satellite system: G for GPS, E for Galileo");
ABSL_FLAG(std::string, signal, "1C", "GNSS signal: 1C for GPS L1 CA, 1B for Galileo E1");
ABSL_FLAG(bool, remove_rx_clock_error, false, "Compute and remove the receivers clock error prior to compute observable differences (requires a valid RINEX nav file for both receivers)");
#endif
#endif

View File

@@ -0,0 +1,164 @@
# GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
# This file is part of GNSS-SDR.
#
# SPDX-FileCopyrightText: 2010-2020 C. Fernandez-Prades cfernandez(at)cttc.es
# SPDX-License-Identifier: BSD-3-Clause
if(NOT GNSSTK_FOUND AND NOT ENABLE_OWN_GNSSTK)
find_package(GNSSTK)
endif()
if(NOT GNSSTK_FOUND OR ENABLE_OWN_GNSSTK)
include(GNUInstallDirs)
if(GNSSTK_USES_GPSTK_NAMESPACE)
set(GNSSTK_LIBRARY ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/${CMAKE_INSTALL_LIBDIR}/${CMAKE_FIND_LIBRARY_PREFIXES}gpstk${CMAKE_STATIC_LIBRARY_SUFFIX})
set(GNSSTK_INCLUDE_DIR ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/include)
else()
set(GNSSTK_LIBRARY ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/${CMAKE_INSTALL_LIBDIR}/${CMAKE_FIND_LIBRARY_PREFIXES}gnsstk${CMAKE_STATIC_LIBRARY_SUFFIX})
set(GNSSTK_INCLUDE_DIR ${GNSSSDR_BINARY_DIR}/gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION}/install/include)
endif()
endif()
if(CMAKE_VERSION VERSION_LESS 3.30)
find_package(Boost COMPONENTS iostreams serialization QUIET)
else()
find_package(Boost COMPONENTS iostreams serialization)
if(NOT TARGET Boost::iostreams)
message(STATUS "Trying deprecated FindBoost Module ...")
if(POLICY CMP0167)
cmake_policy(SET CMP0167 OLD)
find_package(Boost COMPONENTS iostreams serialization)
endif()
endif()
endif()
if(CMAKE_VERSION VERSION_LESS 3.5)
if(NOT TARGET Boost::iostreams)
add_library(Boost::iostreams IMPORTED SHARED)
set_property(TARGET Boost::iostreams PROPERTY
INTERFACE_INCLUDE_DIRECTORIES ${Boost_INCLUDE_DIR})
set_property(TARGET Boost::iostreams PROPERTY
INTERFACE_LINK_LIBRARIES ${Boost_IOSTREAMS_LIBRARIES})
set_property(TARGET Boost::iostreams PROPERTY
IMPORTED_LOCATION ${Boost_IOSTREAMS_LIBRARIES})
endif()
if(NOT TARGET Boost::serialization)
add_library(Boost::serialization IMPORTED SHARED)
set_property(TARGET Boost::serialization PROPERTY
INTERFACE_INCLUDE_DIRECTORIES ${Boost_INCLUDE_DIR})
set_property(TARGET Boost::serialization PROPERTY
INTERFACE_LINK_LIBRARIES ${Boost_SERIALIZARION_LIBRARIES})
set_property(TARGET Boost::serialization PROPERTY
IMPORTED_LOCATION ${Boost_SERIALIZARION_LIBRARIES})
endif()
endif()
find_program(UNCOMPRESS_EXECUTABLE uncompress
PATHS /bin
/usr/bin
/usr/sbin
)
if(TARGET Boost::iostreams AND TARGET Boost::serialization)
message(STATUS "The rinex2assist utility tool will be built when doing '${CMAKE_MAKE_PROGRAM_PRETTY_NAME}'")
if(USE_CMAKE_TARGET_SOURCES)
add_executable(rinex2assist)
target_sources(rinex2assist
PRIVATE
main.cc
)
else()
add_executable(rinex2assist ${CMAKE_CURRENT_SOURCE_DIR}/main.cc)
endif()
if(GNSSTK_VERSION AND "${GNSSTK_VERSION}" VERSION_LESS 3.0.1)
set_property(TARGET rinex2assist PROPERTY CXX_STANDARD 14) # Required by GPSTk v3.0.0
endif()
# Do not show warnings raised by GPSTk v3.0.0
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
target_compile_options(rinex2assist
PRIVATE
-Wno-deprecated -Wno-unused-parameter -Wno-sign-compare -Wno-type-limits -Wno-unused-but-set-variable -Wno-deprecated-copy -Wno-extra
)
endif()
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
target_compile_options(rinex2assist
PRIVATE
-Wno-deprecated -Wno-unused-parameter -Wno-sign-compare -Wno-switch -Wno-inconsistent-missing-override
)
endif()
target_link_libraries(rinex2assist
PRIVATE
Boost::iostreams
Boost::serialization
${GNSSTK_LIBRARY}
Threads::Threads
core_system_parameters
)
if(ENABLE_GLOG_AND_GFLAGS)
target_link_libraries(rinex2assist PRIVATE Gflags::gflags)
target_compile_definitions(rinex2assist PRIVATE -DUSE_GLOG_AND_GFLAGS=1)
else()
target_link_libraries(rinex2assist PRIVATE absl::flags absl::flags_parse)
endif()
if(GNSSTK_USES_GPSTK_NAMESPACE)
target_compile_definitions(rinex2assist PUBLIC -DGNSSTK_USES_GPSTK_NAMESPACE=1)
target_include_directories(rinex2assist
PRIVATE
${GNSSTK_INCLUDE_DIR}/gpstk
${GNSSTK_INCLUDE_DIR}
)
else()
target_include_directories(rinex2assist
PRIVATE
${GNSSTK_INCLUDE_DIR}/gnsstk
${GNSSTK_INCLUDE_DIR}
)
endif()
if(GNSSTK_OLDER_THAN_9)
target_compile_definitions(rinex2assist PRIVATE -DGNSSTK_OLDER_THAN_9=1)
endif()
if(NOT UNCOMPRESS_EXECUTABLE-NOTFOUND)
target_compile_definitions(rinex2assist PRIVATE -DUNCOMPRESS_EXECUTABLE="${UNCOMPRESS_EXECUTABLE}")
else()
target_compile_definitions(rinex2assist PRIVATE -DUNCOMPRESS_EXECUTABLE="")
endif()
if(NOT GNSSTK_FOUND OR ENABLE_OWN_GNSSTK)
add_dependencies(rinex2assist gnsstk-${GNSSSDR_GNSSTK_LOCAL_VERSION})
endif()
if(ENABLE_STRIP)
set_target_properties(rinex2assist PROPERTIES LINK_FLAGS "-s")
endif()
add_custom_command(TARGET rinex2assist POST_BUILD
COMMAND ${CMAKE_COMMAND} -E copy $<TARGET_FILE:rinex2assist>
${LOCAL_INSTALL_BASE_DIR}/install/$<TARGET_FILE_NAME:rinex2assist>
)
install(TARGETS rinex2assist
RUNTIME DESTINATION bin
COMPONENT "rinex2assist"
)
else()
message(STATUS "Boost Iostreams library not found.")
message(STATUS " The rinex2assist utility tool will not be built.")
if(${CMAKE_SYSTEM_NAME} MATCHES "Linux|kFreeBSD|GNU")
message(STATUS " You can install the Boost Iostreams library by doing:")
if(${LINUX_DISTRIBUTION} MATCHES "Fedora" OR ${LINUX_DISTRIBUTION} MATCHES "Red Hat")
message(STATUS " sudo yum install boost-iostreams")
elseif(${LINUX_DISTRIBUTION} MATCHES "openSUSE")
message(STATUS " sudo zypper install libboost_iostreams-devel")
else()
message(STATUS " sudo apt-get install libboost-iostreams-dev")
endif()
endif()
endif()
set(Boost_FOUND TRUE) # trick for summary report

View File

@@ -0,0 +1,109 @@
## Rinex2assist
<!-- prettier-ignore-start -->
[comment]: # (
SPDX-License-Identifier: GPL-3.0-or-later
)
[comment]: # (
SPDX-FileCopyrightText: 2019-2020 Carles Fernandez-Prades <carles.fernandez@cttc.es>
)
<!-- prettier-ignore-end -->
This program reads data from RINEX navigation files and generates XML files that
can be read by GNSS-SDR as Assisted GNSS data.
### Building
This program is built along with GNSS-SDR if the options
`ENABLE_UNIT_TESTING_EXTRA` or `ENABLE_SYSTEM_TESTING_EXTRA` are set to `ON`
when calling CMake:
```
$ cmake -DENABLE_SYSTEM_TESTING_EXTRA=ON ..
$ make
$ sudo make install
```
The last step is optional. Without it, you will get the executable at
`../install/rinex2assist`.
The building requires two extra dependencies: the Boost Iostreams library and
the program `uncompress`:
- The Boost Iostreams library can be installed through a package:
- In Debian / Ubuntu: `sudo apt-get install libboost-iostreams-dev`
- In Fedora / CentOS: `sudo yum install boost-iostreams`
- In OpenSUSE: `sudo zypper install libboost_iostreams-devel`
- In Arch Linux: included in `boost-libs` package.
- In macOS: included in Macports / Homebrew `boost` package.
- The program `uncompress` is available by default in most UNIX and GNU/Linux
systems.
- In Fedora / CentOS: `sudo yum install ncompress`
- In OpenSUSE: `sudo zypper install ncompress`
### Usage
The usage is as follows:
```
$ rinex2assist /path/to/RINEX_nav_file
```
The argument is mandatory (the name of the RINEX navigation file). The name
`gps_ephemeris.xml` is given to the output if GPS NAV data is fould. If the
RINEX file contains Galileo data, the corresponding `gal_ephemeris.xml` file
will be generated. The program is also able to extract parameters of the UTC and
the Ionospheric models from the RINEX header, if available. They will be called
`gps_utc_model.xml`, `gps_iono.xml`, `gal_utc_model.xml` and `gal_iono.xml`.
There are some servers available for downloading recent RINEX navigation files.
For instance:
- NASA:
[ftp://cddis.gsfc.nasa.gov/pub/gnss/data/hourly/](ftp://gssc.esa.int/gnss/data/hourly/)
- ESA:
[ftp://gssc.esa.int/gnss/data/hourly/](ftp://gssc.esa.int/gnss/data/hourly/)
- UNAVCO:
[ftp://data-out.unavco.org/pub/hourly/rinex/](ftp://data-out.unavco.org/pub/hourly/rinex/)
Just make sure to pick up a recent file from a
[station near you](http://www.igs.org/network).
The program accepts either versions 2.xx or 3.xx for the RINEX navigation data
file, as well as compressed files (ending in `.gz` or `.Z`).
Examples:
```
$ rinex2assist EBRE00ESP_R_20183290400_01H_GN.rnx.gz
Generated file: gps_ephemeris.xml
Generated file: gps_utc_model.xml
Generated file: gps_iono.xml
```
and
```
$ rinex2assist EBRE00ESP_R_20183290000_01H_EN.rnx.gz
Generated file: gal_ephemeris.xml
Generated file: gal_utc_model.xml
Generated file: gal_iono.xml
```
An example of GNSS-SDR configuration using ephemeris, UTC and ionospheric model
parameters for GPS L1 and Galileo signals is shown below:
```
GNSS-SDR.AGNSS_XML_enabled=true
GNSS-SDR.AGNSS_ref_location=41.39,2.31
GNSS-SDR.AGNSS_gps_ephemeris_xml=gps_ephemeris.xml
GNSS-SDR.AGNSS_gps_iono_xml=gps_iono.xml
GNSS-SDR.AGNSS_gps_utc_model_xml=gps_utc_model.xml
GNSS-SDR.AGNSS_gal_ephemeris_xml=gal_ephemeris.xml
GNSS-SDR.AGNSS_gal_iono_xml=gal_iono.xml
GNSS-SDR.AGNSS_gal_utc_model_xml=gal_utc_model.xml
```
More info about the usage of AGNSS data
[here](https://gnss-sdr.org/docs/sp-blocks/global-parameters/#assisted-gnss-with-xml-files).

495
utils/rinex2assist/main.cc Normal file
View File

@@ -0,0 +1,495 @@
/*!
* \file main.cc
* \brief converts navigation RINEX files into XML files for Assisted GNSS.
* \author Carles Fernandez-Prades, 2018. cfernandez(at)cttc.cat
*
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "galileo_ephemeris.h" // IWYU pragma: keep
#include "galileo_iono.h"
#include "galileo_utc_model.h"
#include "gps_ephemeris.h"
#include "gps_iono.h"
#include "gps_utc_model.h"
#include <boost/archive/xml_oarchive.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/serialization/map.hpp>
#include <cstddef> // for size_t
#include <cstdlib>
#include <iostream>
#if GNSSTK_USES_GPSTK_NAMESPACE
#include <gpstk/GPSWeekSecond.hpp>
#include <gpstk/Rinex3NavData.hpp>
#include <gpstk/Rinex3NavHeader.hpp>
#include <gpstk/Rinex3NavStream.hpp>
namespace gnsstk = gpstk;
#else
#include <gnsstk/GPSWeekSecond.hpp>
#include <gnsstk/Rinex3NavData.hpp>
#include <gnsstk/Rinex3NavHeader.hpp>
#include <gnsstk/Rinex3NavStream.hpp>
#endif
#if USE_GLOG_AND_GFLAGS
#include <gflags/gflags.h>
#if GFLAGS_OLD_NAMESPACE
namespace gflags
{
using namespace google;
}
#endif
#else
#include <absl/flags/flag.h>
#include <absl/flags/parse.h>
#include <absl/flags/usage.h>
#include <absl/flags/usage_config.h>
std::string TstVersionString() { return "1.0"; }
#endif
int main(int argc, char** argv)
{
const std::string intro_help(
std::string("\n rinex2assist converts navigation RINEX files into XML files for Assisted GNSS\n") +
"Copyright (C) 2018 (see AUTHORS file for a list of contributors)\n" +
"This program comes with ABSOLUTELY NO WARRANTY;\n" +
"See COPYING file to see a copy of the General Public License.\n \n" +
"Usage: \n" +
" rinex2assist <RINEX Nav file input>");
#if USE_GLOG_AND_GFLAGS
gflags::SetUsageMessage(intro_help);
google::SetVersionString("1.0");
gflags::ParseCommandLineFlags(&argc, &argv, true);
#else
absl::FlagsUsageConfig empty_config;
empty_config.version_string = &TstVersionString;
absl::SetFlagsUsageConfig(empty_config);
absl::SetProgramUsageMessage(intro_help);
absl::ParseCommandLine(argc, argv);
#endif
if ((argc != 2))
{
std::cerr << "Usage:\n";
std::cerr << " " << argv[0]
<< " <RINEX Nav file input>"
<< '\n';
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
std::string xml_filename;
// Uncompress if RINEX file is gzipped
std::string rinex_filename(argv[1]);
std::string input_filename = rinex_filename;
std::size_t found = rinex_filename.find_last_of('.');
if (found != std::string::npos)
{
if (rinex_filename.size() >= found + 3)
{
if ((rinex_filename.substr(found + 1, found + 3) == "gz"))
{
std::ifstream file(rinex_filename, std::ios_base::in | std::ios_base::binary);
if (file.fail())
{
std::cerr << "Could not open file " << rinex_filename << '\n';
return 1;
}
boost::iostreams::filtering_streambuf<boost::iostreams::input> in;
try
{
in.push(boost::iostreams::gzip_decompressor());
}
catch (const boost::exception& e)
{
std::cerr << "Could not decompress file " << rinex_filename << '\n';
return 1;
}
in.push(file);
std::string rinex_filename_unzipped = rinex_filename.substr(0, found);
std::ofstream output_file(rinex_filename_unzipped.c_str(), std::ios_base::out | std::ios_base::binary | std::ios_base::trunc);
if (file.fail())
{
std::cerr << "Could not create file " << rinex_filename_unzipped << '\n';
return 1;
}
boost::iostreams::copy(in, output_file);
input_filename = rinex_filename_unzipped;
}
}
if (rinex_filename.size() >= found + 2)
{
if ((rinex_filename.substr(found + 1, found + 2) == "Z"))
{
std::ifstream file(rinex_filename, std::ios_base::in | std::ios_base::binary);
if (file.fail())
{
std::cerr << "Could not open file" << rinex_filename << '\n';
return 1;
}
file.close();
std::string uncompress_executable(UNCOMPRESS_EXECUTABLE);
if (!uncompress_executable.empty())
{
// option k is not always available, so we save a copy of the original file
std::string argum = std::string("/bin/cp " + rinex_filename + " " + rinex_filename + ".aux");
int s1 = std::system(argum.c_str());
std::string argum2 = std::string(uncompress_executable + " -f " + rinex_filename);
int s2 = std::system(argum2.c_str());
std::string argum3 = std::string("/bin/mv " + rinex_filename + +".aux" + " " + rinex_filename);
int s3 = std::system(argum3.c_str());
input_filename = rinex_filename.substr(0, found);
if ((s1 != 0) or (s2 != 0) or (s3 != 0))
{
std::cerr << "Failure uncompressing file.\n";
return 1;
}
}
else
{
std::cerr << "uncompress program not found.\n";
return 1;
}
}
}
}
std::map<int, Gps_Ephemeris> eph_map;
std::map<int, Galileo_Ephemeris> eph_gal_map;
Gps_Utc_Model gps_utc_model;
Gps_Iono gps_iono;
Galileo_Utc_Model gal_utc_model;
Galileo_Iono gal_iono;
int i = 0;
int j = 0;
try
{
// Read nav file
gnsstk::Rinex3NavStream rnffs(input_filename.c_str()); // Open navigation data file
gnsstk::Rinex3NavData rne;
gnsstk::Rinex3NavHeader hdr;
// Read header
rnffs >> hdr;
// Check that it really is a RINEX navigation file
if (hdr.fileType.substr(0, 1) != "N")
{
std::cerr << "This is not a valid RINEX navigation file, or file not found.\n";
std::cerr << "No XML file will be created.\n";
return 1;
}
// Collect UTC parameters from RINEX header
if (hdr.fileSys == "G: (GPS)" || hdr.fileSys == "MIXED")
{
gps_utc_model.valid = (hdr.valid > 2147483648) ? true : false;
gps_utc_model.A1 = hdr.mapTimeCorr["GPUT"].A0;
gps_utc_model.A0 = hdr.mapTimeCorr["GPUT"].A1;
#if GNSSTK_OLDER_THAN_9
gps_utc_model.tot = hdr.mapTimeCorr["GPUT"].refSOW;
gps_utc_model.WN_T = hdr.mapTimeCorr["GPUT"].refWeek;
#else
if (hdr.mapTimeCorr["GPUT"].refTime != gnsstk::CommonTime::BEGINNING_OF_TIME)
{
gnsstk::GPSWeekSecond gws(hdr.mapTimeCorr["GPUT"].refTime);
gps_utc_model.tot = gws.getSOW();
gps_utc_model.WN_T = gws.getWeek();
}
#endif
gps_utc_model.DeltaT_LS = hdr.leapSeconds;
gps_utc_model.WN_LSF = hdr.leapWeek;
gps_utc_model.DN = hdr.leapDay;
gps_utc_model.DeltaT_LSF = hdr.leapDelta;
// Collect iono parameters from RINEX header
gps_iono.valid = (hdr.mapIonoCorr["GPSA"].param[0] == 0) ? false : true;
gps_iono.alpha0 = hdr.mapIonoCorr["GPSA"].param[0];
gps_iono.alpha1 = hdr.mapIonoCorr["GPSA"].param[1];
gps_iono.alpha2 = hdr.mapIonoCorr["GPSA"].param[2];
gps_iono.alpha3 = hdr.mapIonoCorr["GPSA"].param[3];
gps_iono.beta0 = hdr.mapIonoCorr["GPSB"].param[0];
gps_iono.beta1 = hdr.mapIonoCorr["GPSB"].param[1];
gps_iono.beta2 = hdr.mapIonoCorr["GPSB"].param[2];
gps_iono.beta3 = hdr.mapIonoCorr["GPSB"].param[3];
}
if (hdr.fileSys == "E: (GAL)" || hdr.fileSys == "MIXED")
{
gal_utc_model.A0 = hdr.mapTimeCorr["GAUT"].A0;
gal_utc_model.A1 = hdr.mapTimeCorr["GAUT"].A1;
gal_utc_model.Delta_tLS = hdr.leapSeconds;
#if GNSSTK_OLDER_THAN_9
gal_utc_model.tot = hdr.mapTimeCorr["GAUT"].refSOW;
gal_utc_model.WNot = hdr.mapTimeCorr["GAUT"].refWeek;
#else
if (hdr.mapTimeCorr["GAUT"].refTime != gnsstk::CommonTime::BEGINNING_OF_TIME)
{
gnsstk::GPSWeekSecond gws(hdr.mapTimeCorr["GAUT"].refTime);
gal_utc_model.tot = gws.getSOW();
gal_utc_model.WNot = gws.getWeek();
}
#endif
gal_utc_model.WN_LSF = hdr.leapWeek;
gal_utc_model.DN = hdr.leapDay;
gal_utc_model.Delta_tLSF = hdr.leapDelta;
gal_utc_model.flag_utc_model = (hdr.mapTimeCorr["GAUT"].A0 == 0.0);
gal_iono.ai0 = hdr.mapIonoCorr["GAL"].param[0];
gal_iono.ai1 = hdr.mapIonoCorr["GAL"].param[1];
gal_iono.ai2 = hdr.mapIonoCorr["GAL"].param[2];
gal_iono.Region1_flag = false;
gal_iono.Region2_flag = false;
gal_iono.Region3_flag = false;
gal_iono.Region4_flag = false;
gal_iono.Region5_flag = false;
gal_iono.tow = 0.0;
gal_iono.WN = 0.0;
}
// Read navigation data
while (rnffs >> rne)
{
if (rne.satSys == "G" or rne.satSys.empty())
{
// Fill GPS ephemeris object
Gps_Ephemeris eph;
eph.PRN = rne.PRNID;
eph.tow = rne.xmitTime;
eph.IODE_SF2 = rne.IODE;
eph.IODE_SF3 = rne.IODE;
eph.Crs = rne.Crs;
eph.delta_n = rne.dn;
eph.M_0 = rne.M0;
eph.Cuc = rne.Cuc;
eph.ecc = rne.ecc;
eph.Cus = rne.Cus;
eph.sqrtA = rne.Ahalf;
eph.toe = rne.Toe;
eph.toc = rne.Toc;
eph.Cic = rne.Cic;
eph.OMEGA_0 = rne.OMEGA0;
eph.Cis = rne.Cis;
eph.i_0 = rne.i0;
eph.Crc = rne.Crc;
eph.omega = rne.w;
eph.OMEGAdot = rne.OMEGAdot;
eph.idot = rne.idot;
eph.code_on_L2 = rne.codeflgs; //
eph.WN = rne.weeknum;
eph.L2_P_data_flag = rne.L2Pdata;
eph.SV_accuracy = rne.accuracy;
eph.SV_health = rne.health;
eph.TGD = rne.Tgd;
eph.IODC = rne.IODC;
eph.AODO = 0; //
eph.fit_interval_flag = (rne.fitint > 4) ? true : false;
eph.spare1 = 0.0;
eph.spare2 = 0.0;
eph.af0 = rne.af0;
eph.af1 = rne.af1;
eph.af2 = rne.af2;
eph.integrity_status_flag = false; //
eph.alert_flag = false; //
eph.antispoofing_flag = false; //
eph_map[i] = eph;
i++;
}
if (rne.satSys == "E")
{
// Fill Galileo ephemeris object
Galileo_Ephemeris eph;
eph.PRN = rne.PRNID;
eph.M_0 = rne.M0;
eph.ecc = rne.ecc;
eph.sqrtA = rne.Ahalf;
eph.OMEGA_0 = rne.OMEGA0;
eph.i_0 = rne.i0;
eph.omega = rne.w;
eph.OMEGAdot = rne.OMEGAdot;
eph.delta_n = rne.dn;
eph.idot = rne.idot;
eph.Cuc = rne.Cuc;
eph.Cus = rne.Cus;
eph.Crc = rne.Crc;
eph.Crs = rne.Crs;
eph.Cic = rne.Cic;
eph.Cis = rne.Cis;
eph.toe = rne.Toe;
eph.toc = rne.Toc;
eph.af0 = rne.af0;
eph.af1 = rne.af1;
eph.af2 = rne.af2;
eph.WN = rne.weeknum;
eph_gal_map[j] = eph;
j++;
}
}
}
catch (std::exception& e)
{
std::cerr << "Error reading the RINEX file: " << e.what() << '\n';
std::cerr << "No XML file will be created.\n";
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
if (i == 0 and j == 0)
{
std::cerr << "No navigation data found in the RINEX file. No XML file will be created.\n";
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
// Write XML ephemeris
if (i != 0)
{
std::ofstream ofs;
if (xml_filename.empty())
{
xml_filename = "gps_ephemeris.xml";
}
try
{
ofs.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs);
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map);
}
catch (std::exception& e)
{
std::cerr << "Problem creating the XML file " << xml_filename << ": " << e.what() << '\n';
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
std::cout << "Generated file: " << xml_filename << '\n';
}
if (j != 0)
{
std::ofstream ofs2;
xml_filename = "gal_ephemeris.xml";
try
{
ofs2.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs2);
xml << boost::serialization::make_nvp("GNSS-SDR_gal_ephemeris_map", eph_gal_map);
}
catch (std::exception& e)
{
std::cerr << "Problem creating the XML file " << xml_filename << ": " << e.what() << '\n';
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
std::cout << "Generated file: " << xml_filename << '\n';
}
// Write XML UTC
if (gps_utc_model.valid)
{
std::ofstream ofs3;
xml_filename = "gps_utc_model.xml";
try
{
ofs3.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs3);
xml << boost::serialization::make_nvp("GNSS-SDR_utc_model", gps_utc_model);
}
catch (std::exception& e)
{
std::cerr << "Problem creating the XML file " << xml_filename << ": " << e.what() << '\n';
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
std::cout << "Generated file: " << xml_filename << '\n';
}
// Write XML iono
if (gps_iono.valid)
{
std::ofstream ofs4;
xml_filename = "gps_iono.xml";
try
{
ofs4.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs4);
xml << boost::serialization::make_nvp("GNSS-SDR_iono_model", gps_iono);
}
catch (std::exception& e)
{
std::cerr << "Problem creating the XML file " << xml_filename << ": " << e.what() << '\n';
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
std::cout << "Generated file: " << xml_filename << '\n';
}
if (gal_utc_model.A0 != 0)
{
std::ofstream ofs5;
xml_filename = "gal_utc_model.xml";
try
{
ofs5.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs5);
xml << boost::serialization::make_nvp("GNSS-SDR_gal_utc_model", gal_utc_model);
}
catch (std::exception& e)
{
std::cerr << "Problem creating the XML file " << xml_filename << ": " << e.what() << '\n';
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
std::cout << "Generated file: " << xml_filename << '\n';
}
if (gal_iono.ai0 != 0)
{
std::ofstream ofs7;
xml_filename = "gal_iono.xml";
try
{
ofs7.open(xml_filename.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs7);
xml << boost::serialization::make_nvp("GNSS-SDR_gal_iono_model", gal_iono);
}
catch (std::exception& e)
{
std::cerr << "Problem creating the XML file " << xml_filename << ": " << e.what() << '\n';
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 1;
}
std::cout << "Generated file: " << xml_filename << '\n';
}
#if USE_GLOG_AND_GFLAGS
gflags::ShutDownCommandLineFlags();
#endif
return 0;
}

View File

@@ -0,0 +1,129 @@
#!/bin/sh
# GNSS-SDR shell script that tries to download the latest Galileo Almanac file
# published by the European GNSS Service Centre.
#
# Usage: ./download-galileo-almanac.sh
#
# SPDX-FileCopyrightText: 2022 Carles Fernandez-Prades <cfernandez(at)cttc.es>
# SPDX-License-Identifier: GPL-3.0-or-later
if ! [ -x "$(command -v wget)" ]; then
echo "Please install wget before using this script."
exit 1
fi
help()
{
echo "This script tries to download the most recent Galileo Almanac XML file"
echo "published by the European GNSS Service Centre."
echo "More info at https://www.gsc-europa.eu/gsc-products/almanac"
echo "If today there is no published file, the script will look up to one week ago."
echo ""
echo "Usage:"
echo "./download-galileo-almanac.sh [OPTION]"
echo " Options:"
echo " -h, --help Prints this message"
echo " -r, --rename Gets latest Galileo Almanac XML file and saves it as gal_almanac.xml"
echo " -d, --date [date] Retrieves file for a specific date, with format YYYY-MM-DD"
echo " -rd [date] Retrieves file for a specific date, with format YYYY-MM-DD"
echo " and saves it as gal_almanac.xml"
echo ""
echo " Examples:"
echo " ./download-galileo-almanac.sh # Gets latest Galileo Almanac XML file"
echo " ./download-galileo-almanac.sh -r # Gets latest Galileo Almanac XML file, stores it as gal_almanac.xml"
echo " ./download-galileo-almanac.sh -d 2022-03-15 # Gets Galileo Almanac XML file for that day"
echo " ./download-galileo-almanac.sh -rd 2022-03-15 # Gets Galileo Almanac XML file for that day, stores it as gal_almanac.xml"
}
if [ "$1" = "-h" ] || [ "$1" = "--help" ] ; then
help
exit 0
fi
BASE_URL="https://www.gsc-europa.eu/sites/default/files/sites/all/files/"
YEAR=$(date '+%Y')
SPACING="-"
MONTH=$(date '+%m')
DAY=$(date '+%d')
TERMINATION1="_0.xml"
TERMINATION2=".xml"
COUNTER=1
MAX_COUNTER=7
if [ "$1" = "-d" ] || [ "$1" = "--date" ] ; then
if wget "$BASE_URL$2$TERMINATION2" >/dev/null 2>&1 ; then
echo "Downloaded latest Galileo almanac from $BASE_URL$2$TERMINATION2"
exit 0
else
echo "Couldn't find an XML file for that date."
exit 1
fi
elif [ "$1" = "-rd" ] ; then
if wget -O gal_almanac.xml "$BASE_URL$2$TERMINATION2" >/dev/null 2>&1 ; then
echo "Downloaded latest Galileo almanac from $BASE_URL$2$TERMINATION2"
exit 0
else
echo "Couldn't find an XML file for that date."
rm gal_almanac.xml
exit 1
fi
else
echo "According to system time, today is $(date '+%Y-%m-%d'). Searching for the latest Galileo almanac ..."
fi
if [ "$1" = "-r" ] || [ "$1" = "--rename" ]; then
RENAME="yes"
fi
lowercase()
{
echo "$1" | sed "y/ABCDEFGHIJKLMNOPQRSTUVWXYZ/abcdefghijklmnopqrstuvwxyz/"
}
OS=$(lowercase "$(uname)")
date_before()
{
if [ "$OS" = "darwin" ]; then
YEAR=$(date -v -"$COUNTER"d '+%Y')
MONTH=$(date -v -"$COUNTER"d '+%m')
DAY=$(date -v -"$COUNTER"d '+%d')
else
YEAR=$(date -d "$COUNTER day ago" '+%Y')
MONTH=$(date -d "$COUNTER day ago" '+%m')
DAY=$(date -d "$COUNTER day ago" '+%d')
fi
COUNTER=$((COUNTER+1))
}
download_rename_file()
{
[ "$RENAME" = "yes" ] && set -- -O gal_almanac.xml "$@"
wget "$@"
}
try_download()
{
while [ $COUNTER -le $MAX_COUNTER ]
do
url="$BASE_URL$YEAR$SPACING$MONTH$SPACING$DAY$TERMINATION2"
if download_rename_file "$url" >/dev/null 2>&1 ; then
echo "Downloaded latest Galileo almanac from $url"
exit 0
else
date_before
try_download
fi
done
}
url="$BASE_URL$YEAR$SPACING$MONTH$SPACING$DAY$TERMINATION1"
if download_rename_file "$url" >/dev/null 2>&1 ; then
echo "Downloaded latest Galileo almanac from $url"
else
try_download
echo "Couldn't find a recent Galileo almanac."
exit 1
fi

View File

@@ -0,0 +1,14 @@
#!/bin/sh
# GNSS-SDR shell script that enables the remote GNSS-SDR restart telecommand
# usage: ./gnss-sdr-harness.sh ./gnss-sdr -c config_file.conf
# SPDX-FileCopyrightText: Javier Arribas <javier.arribas(at)cttc.es>
# SPDX-License-Identifier: GPL-3.0-or-later
echo "Executing" "$@"
"$@"
while [ $? -eq 42 ]
do
echo "Restarting GNSS-SDR..."
"$@"
done

View File

@@ -0,0 +1,147 @@
/*!
* \file README.txt
* \brief How to add a block to the Simulink Library repository of Matlab,
* how to use the "gnss_sdr_tcp_connector_parallel_tracking_start.m" script
* and how to replace the tracking block of the library. Parallel Computing
* version.
*
* \author David Pubill, 2012. dpubill(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
IMPORTANT: Please, to use this tracking check the configuration file called
'gnss-sdr_tcp_connector_tracking.conf'. There are two major changes:
1.- Choose the [GPS_L1_CA_TCP_CONNECTOR_Tracking] tracking algorithm.
2.- Choose a tcp port for channel 0 (e.g. Tracking.port_ch0=2070;)
A) HOW TO add a block to the Simulink Library repository of your Matlab installation
---------------------------------------------------------------------------------
(These steps should be followed only the first time)
1.- Copy the content of this folder to a folder accessible from Simulink.
2.- In the Matlab Command Window type:
>> simulink;
to open the Simulink Library Browser.
3.- Right-click on the Simulink/User-Defined Functions of the Simulink
Library menu, and click on "Open User-Defined Functions library"
(Window_1).
4.- Open the library model 'gnss_sdr_tcp_connector_tracking_lib.mdl'
(Window_2)
5.- If this is not the first time there should be an existing 'gnss-sdr'
block in the 'User-Defined Functions' window that should be deleted
before drag and drop the new 'gnss_sdr' block (which includes 3 blocks:
- 'gnss_sdr_tcp_connector_tracking_rx' block
- 'gnss_sdr_tcp_connector_tracking' block
- 'gnss_sdr_tcp_connector_tracking_tx' block)
from Window_2 to Window_1. A new message should appear: "This library
is locked. The action performed requires it to be unlocked". Then,
click on the "Unlock" button (the block will be copied) and close
Window_2.
6.- Right-click on the 'gnss-sdr' block and click on "Link Options -->
Disable link", repeat the action but now clicking on "Link Options -->
Break link". This action disables and breaks the link with the
original library model.
7.- On Window_1 save the "simulink/User-Defined Functions" library.
To do that go to "File > Save". Then, close Window_1.
8.- From "Simulink Library Browser" window, press F5 to refresh and generate
the new Simulink Library repository (it may take a few seconds). This
completes the installation of the custom Simulink block.
B) HOW TO use the "gnss_sdr_tcp_connector_parallel_tracking_start.m" script:
----------------------------------------------------------------
----------------------- ------------------ -----------------------
| | | | | |
| gnss_sdr_tcp_ | | gnss_sdr_tcp_ | | gnss_sdr_tcp_ |
| connector_tracking_ | --> | connector_ | --> | connector_tracking_ |
| rx | | tracking | | tx |
| | | | | |
----------------------- ------------------ -----------------------
The 'gnss_sdr_tcp_connector_parallel_tracking_start.m' is the script that
builds and configures a Simulink model for interacting with the GNSS-SDR
platform through a TCP communication. Some 'User parameters' can be
modified but, by default, these are the values assigned:
%User parameters
host = '84.88.61.86'; %Remote IP address (GNSS-SDR computer IP)
port = 2070; %Remote port (GNSS-SDR computer port for Ch0)
num_vars_rx = 9; %Number of variables expected from GNSS-SDR
num_vars_tx = 4; %Number of variable to be transmitted to GNSS-SDR
timeout = '40'; %Timeout in seconds
'host', 'port' and 'timeout' parameters configure both
'gnss_sdr_tcp_connector_tracking_rx' and 'gnss_sdr_tcp_connector_tracking_tx'
blocks. The 'port' parameter sets the base port number for the first
channel (ch0). Each of the subsequent channels increases their port by one
unit (e.g. ch0_port=2070, ch1_port=2071,...).
Also the name of the tracking block can be modified. It must match with
the Simulink model name:
%Name of the tracking block, it must match the Simulink model name
tracking_block_name = 'gnss_sdr_tcp_connector_tracking';
To configure the MATLAB to work in parallel mode (the 'Parallel Computing'
Toolbox must be installed in the MATLAB) type in the Matlab Command Window
the following:
>> matlabpool(C)
where C is the number of cores of the computer to be used.
Then it should appear a message like this one:
"Destroying 1 pre-existing parallel job(s) created by matlabpool that were
in the finished or failed state.
Starting matlabpool using the 'local' configuration ... connected to 4
labs."
Once the MATLAB is configured to work in parallel mode, type the following
to run the script:
>> gnss_sdr_tcp_connector_parallel_tracking_start(N,C);
where N must match the number of channels configured in the GNSS-SDR
platform and C is the same as before.
Note: to stop working with the parallel mode type in the Command Window
the following:
>> matlabpool close
C) HOW TO replace the tracking block of the library
------------------------------------------------
1.- Open the library model 'gnss_sdr_tcp_connector_tracking_lib.mdl'
2.- Unlock the library. Click on "Edit > Unlock Library".
3.- Open the "gnss-sdr" block and change the "gnss_sdr_tcp_connector_tracking"
block by another one. If the name is different it must be updated in
the "gnss_sdr_tcp_connector_parallel_tracking_start.m" code (see
section B)
4.- Save the new library.
5.- Go to section A and follow the instructions.

View File

@@ -0,0 +1,188 @@
% This MATLAB function builds and configures a Simulink model
% for interacting with the GNSS-SDR platform through a TCP
% communication. Parallel Computing version.
% \author David Pubill, 2012. dpubill(at)cttc.es
%
% ----------------------------------------------------------------------
%
% Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
%
% GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver
%
% This file is part of GNSS-SDR.
%
% SPDX-License-Identifier: GPL-3.0-or-later
%
% ----------------------------------------------------------------------
%/
function gnss_sdr_tcp_connector_parallel_tracking_start(num_channels, num_cores)
%The parallel for (parfor) loop allows to build and run a Simulink
%model in parallel mode, programming different threads
parfor i = 0:num_cores-1;
%Open and close the Simulink Library
simulink('open');
simulink('close');
%User parameters
host = '84.88.61.86'; %Remote IP address (GNSS-SDR computer IP)
port = 2070; %Remote port (GNSS-SDR computer port for Ch0)
num_vars_rx = 9; %Number of variables expected from GNSS-SDR
num_vars_tx = 4; %Number of variable to be transmitted to GNSS-SDR
timeout = '40'; %Timeout [s]
%Name of the tracking block, it must match the Simulink model name
tracking_block_name = 'gnss_sdr_tcp_connector_tracking';
% Layout coordinates for the gnss_sdr_tcp_connector_tracking blocks
X0 = 20;
X1 = 170;
Y0 = 20;
Y1 = 140;
X_offset = 200;
Y_offset = 160;
%Calculate the size of the data received from GNSS-SDR
%(float = 4 bytes each variable)
datasize_RX = num_vars_rx*4;
%Create a Simulink model
model_name = ['gnss_sdr_tcp_connector_parallel_tracking_aux_', num2str(i)];
new_system(model_name);
open_system(model_name);
%Set parameters to avoid warnings in the Command Window
set_param(model_name,...
'InheritedTsInSrcMsg', 'none');
warning('off', 'Simulink:Commands:SetParamLinkChangeWarn');
%Assign values to the variables used by Simulink in the base workspace
assignin('base', 'Ti', 1e-3);
assignin('base', 'f0', 1.57542e9);
assignin('base', 'SFunSlope', 3.5);
assignin('base', 'Tc', 4e-3/4092);
assignin('base', 'T', 1e-3);
assignin('base', 'B_PLL', 50);
assignin('base', 'B_DLL', 2);
%Calculate some variables to control the number of blocks that
%should content each Simulink model in function of the number of
%cores specified
min_num_blocks_per_model = floor(num_channels/num_cores);
id = rem(num_channels,num_cores);
if(i<id)
aux=1;
else
aux=0;
end
%Build the Simulink model for the core 'i'
for m = 0:min_num_blocks_per_model+aux-1
index = m + min_num_blocks_per_model*i + min(id,i);
%Add and prepare an empty block to become the TCP connector block
tcp_connector_block=[model_name, '/gnss_sdr_tcp_connector_tracking_', num2str(index)];
add_block('simulink/Ports & Subsystems/Subsystem', tcp_connector_block);
delete_line(tcp_connector_block, 'In1/1', 'Out1/1')
tcp_connector_tracking_i_In1 = [model_name,'/gnss_sdr_tcp_connector_tracking_',num2str(index),'/In1'];
tcp_connector_tracking_i_Out1 = [model_name,'/gnss_sdr_tcp_connector_tracking_',num2str(index),'/Out1'];
delete_block(tcp_connector_tracking_i_In1);
delete_block(tcp_connector_tracking_i_Out1);
%Add to the TCP connector block the receiver, the tracking and the
%transmitter blocks
tcp_connector_tracking_rx_block = [model_name,'/gnss_sdr_tcp_connector_tracking_',num2str(index),'/gnss_sdr_tcp_connector_tracking_rx'];
tcp_connector_tracking_block = [model_name,'/gnss_sdr_tcp_connector_tracking_',num2str(index),'/', tracking_block_name];
tcp_connector_tracking_tx_block = [model_name,'/gnss_sdr_tcp_connector_tracking_',num2str(index),'/gnss_sdr_tcp_connector_tracking_tx'];
add_block('simulink/User-Defined Functions/gnss_sdr/gnss_sdr_tcp_connector_tracking_rx',tcp_connector_tracking_rx_block);
path_to_tracking_block = ['simulink/User-Defined Functions/gnss_sdr/', tracking_block_name];
add_block(path_to_tracking_block, tcp_connector_tracking_block);
add_block('simulink/User-Defined Functions/gnss_sdr/gnss_sdr_tcp_connector_tracking_tx',tcp_connector_tracking_tx_block);
%Connect the receiver block to the tracking block
for j=1:num_vars_rx;
rx_out_ports =['gnss_sdr_tcp_connector_tracking_rx/', num2str(j)];
tracking_in_ports =[tracking_block_name, '/', num2str(j)];
add_line(tcp_connector_block, rx_out_ports, tracking_in_ports)
end
%Connect the tracking block to the transmitter block
for k=1:num_vars_tx;
tracking_out_ports =[tracking_block_name, '/', num2str(k)];
tx_in_ports =['gnss_sdr_tcp_connector_tracking_tx/',num2str(k)];
add_line(tcp_connector_block, tracking_out_ports, tx_in_ports)
end
%Add, place and connect two scopes in the TCP connector block
name_scope_1 = [tcp_connector_block,'/Scope'];
add_block('simulink/Sinks/Scope', name_scope_1, 'Position', [500 300 550 350]);
set_param(name_scope_1, 'NumInputPorts', '4', 'LimitDataPoints', 'off');
add_line(tcp_connector_block, 'gnss_sdr_tcp_connector_tracking_rx/9', 'Scope/1', 'autorouting','on')
tracking_scope_port2 = [tracking_block_name,'/2'];
add_line(tcp_connector_block, tracking_scope_port2, 'Scope/2', 'autorouting','on')
tracking_scope_port3 = [tracking_block_name,'/3'];
add_line(tcp_connector_block, tracking_scope_port3, 'Scope/3', 'autorouting','on')
tracking_scope_port4 = [tracking_block_name,'/4'];
add_line(tcp_connector_block, tracking_scope_port4, 'Scope/4', 'autorouting','on')
name_scope_2 = [tcp_connector_block,'/EPL'];
add_block('simulink/Sinks/Scope', name_scope_2, 'Position', [500 400 550 450]);
set_param(name_scope_2, 'LimitDataPoints', 'off');
tracking_scope2_port5 = [tracking_block_name,'/5'];
add_line(tcp_connector_block, tracking_scope2_port5, 'EPL/1', 'autorouting','on')
num_port = port+index;
%Set the TCP receiver parameters
tcp_receiver = [model_name,'/gnss_sdr_tcp_connector_tracking_',num2str(index),'/gnss_sdr_tcp_connector_tracking_rx/RX'];
set_param(tcp_receiver, 'Port', num2str(num_port), 'Host', host, 'DataSize', num2str(datasize_RX), 'Timeout', timeout);
%Set the TCP transmitter parameters
tcp_transmitter = [model_name, '/gnss_sdr_tcp_connector_tracking_',num2str(index),'/gnss_sdr_tcp_connector_tracking_tx/TX'];
set_param(tcp_transmitter, 'Port', num2str(num_port), 'Host', host,'Timeout', timeout);
%New layout coordinates for each block
X2 = X0 + floor(m/4)*X_offset;
X3 = X1 + floor(m/4)*X_offset;
Y2 = Y0 + (m-4*floor(m/4))*Y_offset;
Y3 = Y1 + (m-4*floor(m/4))*Y_offset;
%Place the block in the layout
set_param(tcp_connector_block, 'Position', [X2 Y2 X3 Y3]);
end
%Set parameters to configure the model Solver
set_param(model_name,...
'SolverType', 'Fixed-step', 'Solver', 'FixedStepDiscrete',...
'FixedStep', 'auto', 'StopTime', 'inf');
%Save the model with a definitive name
model_name_ready = ['gnss_sdr_tcp_connector_parallel_tracking_ready_', num2str(i)];
save_system(model_name, model_name_ready);
%Pause the thread 'i*5' seconds in function of the number of core.
%This allows the system to establish the TCP connections in the
%correct order
if (aux == 0)
pause(i*5);
end
%Run the Simulink model
set_param(model_name_ready,'simulationcommand','start');
end
end

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,5 @@
/*
* SPDX-FileCopyrightText: David Pubill, 2012. dpubill(at)cttc.es
*
* SPDX-License-Identifier: GPL-3.0-or-later
*/

View File

@@ -0,0 +1,124 @@
/*!
* \file README.txt
* \brief How to add a block to the Simulink Library repository of Matlab,
* how to use the "gnss_sdr_galileo_e1_tcp_connector_tracking_start.m" script and how
* to replace the tracking block of the library.
*
* \author David Pubill, 2012. dpubill(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
IMPORTANT: Please, to use this tracking check the configuration file called
'gnss-sdr_galileo_e1_tcp_connector_tracking.conf'. There are two major changes:
1.- Choose the [Galileo_E1_TCP_CONNECTOR_Tracking] tracking algorithm.
2.- Choose a tcp port for channel 0 (e.g. Tracking.port_ch0=2070;)
A) HOW TO add a block to the Simulink Library repository of your Matlab installation
---------------------------------------------------------------------------------
(These steps should be followed only the first time)
1.- Copy the content of this folder to a folder accessible from Simulink.
2.- In the Matlab Command Window type:
>> simulink;
to open the Simulink Library Browser.
3.- Right-click on the Simulink/User-Defined Functions of the Simulink
Library menu, and click on "Open User-Defined Functions library"
(Window_1)
4.- Open the library model 'gnss_sdr_galileo_e1_tcp_connector_tracking_lib.mdl'
(Window_2)
5.- If this is not the first time there should be an existing 'gnss-sdr'
block in the 'User-Defined Functions' window that should be deleted
before drag and drop the new 'gnss_sdr' block (which includes 3 blocks:
- 'gnss_sdr_galileo_e1_tcp_connector_tracking_rx' block
- 'gnss_sdr_galileo_e1_tcp_connector_tracking' block
- 'gnss_sdr_galileo_e1_tcp_connector_tracking_tx' block)
from Window_2 to Window_1. A new message should appear: "This library
is locked. The action performed requires it to be unlocked". Then,
click on the "Unlock" button (the block will be copied) and close
Window_2.
6.- Right-click on the 'gnss-sdr' block and click on "Link Options -->
Disable link", repeat the action but now clicking on "Link Options -->
Break link". This action disables and breaks the link with the
original library model.
7.- On Window_1 save the "simulink/User-Defined Functions" library.
To do that go to "File > Save". Then, close Window_1.
8.- From "Simulink Library Browser" window, press F5 to refresh and generate
the new Simulink Library repository (it may take a few seconds). This
completes the installation of the custom Simulink block.
B) HOW TO use the "gnss_sdr_galileo_e1_tcp_connector_tracking_start.m" script:
----------------------------------------------------------------
---------------------- ---------------- ----------------------
| | | gnss_sdr_ | | |
| gnss_sdr_galileo_e1_ | | galileo_e1_ | | gnss_sdr_galileo_e1_ |
| tcp_connector_ | --> | tcp_connector_ | --> | tcp_connector_ |
| tracking_rx | | tracking | | tracking_tx |
| | | | | |
---------------------- ---------------- ----------------------
The 'gnss_sdr_galileo_e1_tcp_connector_tracking_start.m' is the script that builds
and configures a simulink model for interacting with the GNSS-SDR platform
through a TCP communication. 'User parameters' can be modified but, by
default, these are the values assigned:
%User parameters
host = '84.88.61.86'; %Remote IP address (GNSS-SDR computer IP)
port = 2070; %Remote port (GNSS-SDR computer port for Ch0)
num_vars_rx = 13; %Number of variables expected from GNSS-SDR
num_vars_tx = 4; %Number of variable to be transmitted to GNSS-SDR
timeout = '10'; %Timeout in seconds
'host', 'port' and 'timeout' parameters configure both
'gnss_sdr_galileo_e1_tcp_connector_tracking_rx' and
'gnss_sdr_galileo_e1_tcp_connector_tracking_tx' blocks. The 'port' parameter
sets the base port number for the first channel (ch0). Each of the subsequent
channels increases their port by one unit (e.g. ch0_port=2070, ch1_port=2071,...).
Also the name of the tracking block can be modified. It must match with
the Simulink model name:
%Name of the tracking block, it must match the Simulink model name
tracking_block_name = 'gnss_sdr_galileo_e1_tcp_connector_tracking';
To run the script just type in the Matlab Command window the following:
>>gnss_sdr_galileo_e1_tcp_connector_tracking_start(N);
where N must match the number of channels configured in the GNSS-SDR
platform.
C) HOW TO replace the tracking block of the library
------------------------------------------------
1.- Open the library model 'gnss_sdr_galileo_e1_tcp_connector_tracking_lib.mdl'
2.- Unlock the library. Click on "Edit > Unlock Library".
3.- Open the "gnss-sdr" block and change the "gnss_sdr_galileo_e1_tcp_connector_tracking"
block by another one. If the name is different it must be updated in
the "gnss_sdr_galileo_e1_tcp_connector_parallel_tracking_start.m" code (see
section B)
4.- Save the new library.
5.- Go to section A and follow the instructions.

View File

@@ -0,0 +1,124 @@
/*!
* \file README.txt
* \brief How to add a block to the Simulink Library repository of Matlab,
* how to use the "gnss_sdr_tcp_connector_tracking_start.m" script and how
* to replace the tracking block of the library.
*
* \author David Pubill, 2012. dpubill(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
IMPORTANT: Please, to use this tracking check the configuration file called
'gnss-sdr_tcp_connector_tracking.conf'. There are two major changes:
1.- Choose the [GPS_L1_CA_TCP_CONNECTOR_Tracking] tracking algorithm.
2.- Choose a tcp port for channel 0 (e.g. Tracking.port_ch0=2060;)
A) HOW TO add a block to the Simulink Library repository of your Matlab installation
---------------------------------------------------------------------------------
(These steps should be followed only the first time)
1.- Copy the content of this folder to a folder accessible from Simulink.
2.- In the Matlab Command Window type:
>> simulink;
to open the Simulink Library Browser.
3.- Right-click on the Simulink/User-Defined Functions of the Simulink
Library menu, and click on "Open User-Defined Functions library"
(Window_1)
4.- Open the library model 'gnss_sdr_tcp_connector_tracking_lib.mdl'
(Window_2)
5.- If this is not the first time there should be an existing 'gnss-sdr'
block in the 'User-Defined Functions' window that should be deleted
before drag and drop the new 'gnss_sdr' block (which includes 3 blocks:
- 'gnss_sdr_tcp_connector_tracking_rx' block
- 'gnss_sdr_tcp_connector_tracking' block
- 'gnss_sdr_tcp_connector_tracking_tx' block)
from Window_2 to Window_1. A new message should appear: "This library
is locked. The action performed requires it to be unlocked". Then,
click on the "Unlock" button (the block will be copied) and close
Window_2.
6.- Right-click on the 'gnss-sdr' block and click on "Link Options -->
Disable link", repeat the action but now clicking on "Link Options -->
Break link". This action disables and breaks the link with the
original library model.
7.- On Window_1 save the "simulink/User-Defined Functions" library.
To do that go to "File > Save". Then, close Window_1.
8.- From "Simulink Library Browser" window, press F5 to refresh and generate
the new Simulink Library repository (it may take a few seconds). This
completes the installation of the custom Simulink block.
B) HOW TO use the "gnss_sdr_tcp_connector_tracking_start.m" script:
----------------------------------------------------------------
----------------------- ------------------ -----------------------
| | | | | |
| gnss_sdr_tcp_ | | gnss_sdr_tcp_ | | gnss_sdr_tcp_ |
| connector_tracking_ | --> | connector_ | --> | connector_tracking_ |
| rx | | tracking | | tx |
| | | | | |
----------------------- ------------------ -----------------------
The 'gnss_sdr_tcp_connector_tracking_start.m' is the script that builds and
configures a simulink model for interacting with the GNSS-SDR platform
through a TCP communication. 'User parameters' can be modified but, by
default, these are the values assigned:
%User parameters
host = '84.88.61.86'; %Remote IP address (GNSS-SDR computer IP)
port = 2070; %Remote port (GNSS-SDR computer port for Ch0)
num_vars_rx = 9; %Number of variables expected from GNSS-SDR
num_vars_tx = 4; %Number of variable to be transmitted to GNSS-SDR
timeout = '40'; %Timeout in seconds
'host', 'port' and 'timeout' parameters configure both
'gnss_sdr_tcp_connector_tracking_rx' and 'gnss_sdr_tcp_connector_tracking_tx'
blocks. The 'port' parameter sets the base port number for the first
channel (ch0). Each of the subsequent channels increases their port by one
unit (e.g. ch0_port=2070, ch1_port=2071,...).
Also the name of the tracking block can be modified. It must match with
the Simulink model name:
%Name of the tracking block, it must match the Simulink model name
tracking_block_name = 'gnss_sdr_tcp_connector_tracking';
To run the script just type in the Matlab Command window the following:
>>gnss_sdr_tcp_connector_tracking_start(N);
where N must match the number of channels configured in the GNSS-SDR
platform.
C) HOW TO replace the tracking block of the library
------------------------------------------------
1.- Open the library model 'gnss_sdr_tcp_connector_tracking_lib.mdl'
2.- Unlock the library. Click on "Edit > Unlock Library".
3.- Open the "gnss-sdr" block and change the "gnss_sdr_tcp_connector_tracking"
block by another one. If the name is different it must be updated in
the "gnss_sdr_tcp_connector_parallel_tracking_start.m" code (see
section B)
4.- Save the new library.
5.- Go to section A and follow the instructions.

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,5 @@
/*
* SPDX-FileCopyrightText: David Pubill, 2012. dpubill(at)cttc.es
*
* SPDX-License-Identifier: GPL-3.0-or-later
*/

View File

@@ -0,0 +1,162 @@
%
% This MATLAB function builds and configures a simulink model
% for interacting with the GNSS-SDR platform through a TCP communication.
% \author David Pubill, 2012. dpubill(at)cttc.es
%
% ----------------------------------------------------------------------
%
% Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
%
% GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver
%
% This file is part of GNSS-SDR.
%
% SPDX-License-Identifier: GPL-3.0-or-later
%
% ----------------------------------------------------------------------
%/
function gnss_sdr_galileo_e1_tcp_connector_tracking_start(num_channels)
%User parameters
host = '84.88.61.86'; %Remote IP address (GNSS-SDR computer IP)
port = 2070; %Remote port (GNSS-SDR computer port for Ch0)
num_vars_rx = 13; %Number of variables expected from GNSS-SDR
num_vars_tx = 4; %Number of variable to be transmitted to GNSS-SDR
timeout = '10'; %Timeout [s]
%name of the tracking block, it must match the name of the Simulink
%model
tracking_block_name = 'gnss_sdr_galileo_e1_tcp_connector_tracking';
% Layout coordinates for the first gnss_sdr_galileo_e1_tcp_connector_tracking
% block and offset definitions
X0 = 20;
X1 = 170;
Y0 = 20;
Y1 = 140;
X_offset = 200;
Y_offset = 160;
%Calculate the size of the data received from GNSS-SDR
%(float = 4 bytes each variable)
datasize_RX = num_vars_rx*4;
%Create a Simulink model
simulink('open');
new_system('gnss_sdr_galileo_e1_tcp_connector_tracking_aux');
open_system('gnss_sdr_galileo_e1_tcp_connector_tracking_aux');
%Set parameters to avoid warnings in the Command Window
set_param('gnss_sdr_galileo_e1_tcp_connector_tracking_aux',...
'InheritedTsInSrcMsg', 'none');
warning('off', 'Simulink:Commands:SetParamLinkChangeWarn');
%Assign values to the variables used by Simulink in the base workspace
%DLL
assignin('base', 'B_DLL', 2);
assignin('base', 'zeta_DLL', 0.7);
assignin('base', 'k_DLL', 1);
assignin('base', 'd_pdi_code', 0.004);
%PLL
assignin('base', 'B_PLL', 30);
assignin('base', 'zeta_PLL', 0.65);
assignin('base', 'k_PLL', 0.25);
assignin('base', 'd_pdi_carr', 0.004);
%Block generation from the Simulink Library
for i = 0:num_channels-1;
%Add and prepare an empty block to become the TCP connector block
tcp_connector_block=['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_', num2str(i)];
add_block('simulink/Ports & Subsystems/Subsystem', tcp_connector_block);
delete_line(tcp_connector_block,'In1/1', 'Out1/1')
tcp_connector_tracking_i_In1 = ['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_',num2str(i),'/In1'];
tcp_connector_tracking_i_Out1 = ['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_',num2str(i),'/Out1'];
delete_block(tcp_connector_tracking_i_In1);
delete_block(tcp_connector_tracking_i_Out1);
%Add to the TCP connector block the receiver, the tracking and the
%transmitter blocks
tcp_connector_tracking_rx_block = ['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_',num2str(i),'/gnss_sdr_galileo_e1_tcp_connector_tracking_rx'];
tcp_connector_tracking_block = ['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_',num2str(i),'/',tracking_block_name];
tcp_connector_tracking_tx_block = ['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_',num2str(i),'/gnss_sdr_galileo_e1_tcp_connector_tracking_tx'];
add_block('simulink/User-Defined Functions/gnss_sdr/gnss_sdr_galileo_e1_tcp_connector_tracking_rx',tcp_connector_tracking_rx_block);
path_to_tracking_block = ['simulink/User-Defined Functions/gnss_sdr/', tracking_block_name];
add_block(path_to_tracking_block, tcp_connector_tracking_block);
add_block('simulink/User-Defined Functions/gnss_sdr/gnss_sdr_galileo_e1_tcp_connector_tracking_tx',tcp_connector_tracking_tx_block);
%Connect the receiver block to the tracking block
for j=1:num_vars_rx;
rx_out_ports =['gnss_sdr_galileo_e1_tcp_connector_tracking_rx/',num2str(j)];
tracking_in_ports =[tracking_block_name,'/',num2str(j)];
add_line(tcp_connector_block, rx_out_ports, tracking_in_ports)
end
%Connect the tracking block to the transmitter block
for k=1:num_vars_tx;
tracking_out_ports =[tracking_block_name,'/',num2str(k)];
tx_in_ports =['gnss_sdr_galileo_e1_tcp_connector_tracking_tx/',num2str(k)];
add_line(tcp_connector_block, tracking_out_ports, tx_in_ports)
end
%Add, place and connect two scopes in the TCP connector block
name_scope_1 = [tcp_connector_block,'/Scope'];
add_block('simulink/Sinks/Scope', name_scope_1, 'Position', [600 425 650 475]);
set_param(name_scope_1, 'NumInputPorts', '5', 'LimitDataPoints', 'off');
add_line(tcp_connector_block, 'gnss_sdr_galileo_e1_tcp_connector_tracking_rx/10', 'Scope/1', 'autorouting','on')
add_line(tcp_connector_block, 'gnss_sdr_galileo_e1_tcp_connector_tracking_rx/11', 'Scope/2', 'autorouting','on')
tracking_scope_port3 = [tracking_block_name,'/2'];
add_line(tcp_connector_block, tracking_scope_port3, 'Scope/3', 'autorouting','on')
tracking_scope_port4 = [tracking_block_name,'/3'];
add_line(tcp_connector_block, tracking_scope_port4, 'Scope/4', 'autorouting','on')
tracking_scope_port5 = [tracking_block_name,'/4'];
add_line(tcp_connector_block, tracking_scope_port5, 'Scope/5', 'autorouting','on')
name_scope_2 = [tcp_connector_block,'/EPL'];
add_block('simulink/Sinks/Scope', name_scope_2, 'Position', [475 500 525 550]);
set_param(name_scope_2, 'LimitDataPoints', 'off');
tracking_scope2_port5 = [tracking_block_name,'/5'];
add_line(tcp_connector_block, tracking_scope2_port5, 'EPL/1', 'autorouting','on')
%Set the TCP receiver parameters
tcp_receiver = ['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_',num2str(i),'/gnss_sdr_galileo_e1_tcp_connector_tracking_rx/RX'];
set_param(tcp_receiver, 'Port', num2str(port+i), 'Host', host, 'DataSize', num2str(datasize_RX), 'Timeout', timeout);
%Set the TCP transmitter parameters
tcp_transmitter = ['gnss_sdr_galileo_e1_tcp_connector_tracking_aux/gnss_sdr_galileo_e1_tcp_connector_tracking_',num2str(i),'/gnss_sdr_galileo_e1_tcp_connector_tracking_tx/TX'];
set_param(tcp_transmitter, 'Port', num2str(port+i), 'Host', host,'Timeout', timeout);
%New layout coordinates for each block
X2 = X0 + floor(i/4)*X_offset;
X3 = X1 + floor(i/4)*X_offset;
Y2 = Y0 + (i-4*floor(i/4))*Y_offset;
Y3 = Y1 + (i-4*floor(i/4))*Y_offset;
%Place the block in the layout
set_param(tcp_connector_block, 'Position', [X2 Y2 X3 Y3]);
end
%Set parameters to configure the model Solver
set_param('gnss_sdr_galileo_e1_tcp_connector_tracking_aux',...
'SolverType', 'Fixed-step', 'Solver', 'FixedStepDiscrete',...
'FixedStep', 'auto', 'StopTime', 'inf');
%Save the model with a definitive name
save_system('gnss_sdr_galileo_e1_tcp_connector_tracking_aux', 'gnss_sdr_galileo_e1_tcp_connector_tracking_ready');
simulink('close');
%Run the Simulink model
set_param('gnss_sdr_galileo_e1_tcp_connector_tracking_ready','simulationcommand','start');
end

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,5 @@
/*
* SPDX-FileCopyrightText: David Pubill, 2012. dpubill(at)cttc.es
*
* SPDX-License-Identifier: GPL-3.0-or-later
*/

Some files were not shown because too many files have changed in this diff Show More