mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 12:40:35 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Gastd 2017-03-29 12:17:21 -03:00
commit dc5c6937f3
35 changed files with 2053 additions and 508 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc
View File

@ -34,6 +34,9 @@
#include <map> #include <map>
#include <boost/date_time/posix_time/posix_time.hpp> #include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/math/common_factor_rt.hpp> #include <boost/math/common_factor_rt.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/serialization/map.hpp>
#include <gnuradio/gr_complex.h> #include <gnuradio/gr_complex.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <glog/logging.h> #include <glog/logging.h>
@ -158,7 +161,7 @@ void hybrid_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg)
gps_cnav_ephemeris = boost::any_cast<std::shared_ptr<Gps_CNAV_Ephemeris>>(pmt::any_ref(msg)); gps_cnav_ephemeris = boost::any_cast<std::shared_ptr<Gps_CNAV_Ephemeris>>(pmt::any_ref(msg));
// update/insert new ephemeris record to the global ephemeris map // update/insert new ephemeris record to the global ephemeris map
d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris;
DLOG(INFO) << "New GPS CNAV ephemeris record has arrived "; LOG(INFO) << "New GPS CNAV ephemeris record has arrived ";
} }
else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>) ) else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr<Gps_CNAV_Iono>) )
{ {
@ -323,6 +326,76 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump
hybrid_pvt_cc::~hybrid_pvt_cc() hybrid_pvt_cc::~hybrid_pvt_cc()
{ {
msgctl(sysv_msqid, IPC_RMID, NULL); msgctl(sysv_msqid, IPC_RMID, NULL);
//save GPS L2CM ephemeris to XML file
std::string file_name="eph_GPS_L2CM.xml";
if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0)
{
try
{
std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs);
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_cnav_ephemeris_map);
ofs.close();
LOG(INFO) << "Saved GPS L2CM Ephemeris map data";
}
catch (std::exception& e)
{
LOG(WARNING) << e.what();
}
}
else
{
LOG(WARNING) << "Failed to save GPS L2CM Ephemeris, map is empty";
}
//save GPS L1 CA ephemeris to XML file
file_name="eph_GPS_L1CA.xml";
if (d_ls_pvt->gps_ephemeris_map.size() > 0)
{
try
{
std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs);
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_ephemeris_map);
ofs.close();
LOG(INFO) << "Saved GPS L1 CA Ephemeris map data";
}
catch (std::exception& e)
{
LOG(WARNING) << e.what();
}
}
else
{
LOG(WARNING) << "Failed to save GPS L1 CA Ephemeris, map is empty";
}
//save Galileo E1 ephemeris to XML file
file_name="eph_Galileo_E1.xml";
if (d_ls_pvt->galileo_ephemeris_map.size() > 0)
{
try
{
std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out);
boost::archive::xml_oarchive xml(ofs);
xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->galileo_ephemeris_map);
ofs.close();
LOG(INFO) << "Saved Galileo E1 Ephemeris map data";
}
catch (std::exception& e)
{
LOG(WARNING) << e.what();
}
}
else
{
LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty";
}
} }
@ -451,6 +524,7 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v
if ((d_sample_counter % d_output_rate_ms) == 0) if ((d_sample_counter % d_output_rate_ms) == 0)
{ {
bool pvt_result; bool pvt_result;
//std::cout<<"obs map size at pvt="<<gnss_observables_map.size()<<std::endl;
pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging);
if (pvt_result == true) if (pvt_result == true)

19
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -86,6 +86,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
int GPS_week = 0; int GPS_week = 0;
double utc = 0.0; double utc = 0.0;
double GST = 0.0; double GST = 0.0;
double secondsperweek = 604800.0;
//double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s) //double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s)
double TX_time_corrected_s = 0.0; double TX_time_corrected_s = 0.0;
double SV_clock_bias_s = 0.0; double SV_clock_bias_s = 0.0;
@ -196,8 +198,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
// SV ECEF DEBUG OUTPUT // SV ECEF DEBUG OUTPUT
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " X=" << gps_ephemeris_iter->second.d_satpos_X << " TX Time corrected="<<TX_time_corrected_s << " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]"; << " [m] PR_obs=" << obs(valid_obs) << " [m]";
@ -205,7 +207,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
valid_obs++; valid_obs++;
// compute the UTC time for this SV (just to print the associated UTC timestamp) // compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week; GPS_week = gps_ephemeris_iter->second.i_GPS_week;
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
@ -233,7 +234,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 3- compute the current ECEF position for this SV using corrected TX time // 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); //std::cout<<"TX time["<<gps_cnav_ephemeris_iter->second.i_satellite_PRN<<"]="<<TX_time_corrected_s<<std::endl;
double dtr = gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
@ -243,15 +245,17 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 4- fill the observations vector with the corrected observables // 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1); obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s; obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr*GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz; d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week; GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
GPS_week=GPS_week%1024; //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV)
// SV ECEF DEBUG OUTPUT // SV ECEF DEBUG OUTPUT
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN LOG(INFO) << "(new)ECEF GPS L2M satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
<< " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X << " TX Time corrected="<<TX_time_corrected_s
<< " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y << " [m] Y=" << gps_cnav_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z << " [m] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]"; << " [m] PR_obs=" << obs(valid_obs) << " [m]";
@ -305,7 +309,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]"; DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
double secondsperweek = 604800.0;
// Compute GST and Gregorian time // Compute GST and Gregorian time
if( GST != 0.0) if( GST != 0.0)
{ {

2
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/CMakeLists.txt
View File

@ -49,7 +49,7 @@ message(STATUS "Build type set to ${CMAKE_BUILD_TYPE}.")
set(VERSION_INFO_MAJOR_VERSION 0) set(VERSION_INFO_MAJOR_VERSION 0)
set(VERSION_INFO_MINOR_VERSION 0) set(VERSION_INFO_MINOR_VERSION 0)
set(VERSION_INFO_MAINT_VERSION 9) set(VERSION_INFO_MAINT_VERSION 9.git)
include(VolkVersion) #setup version info include(VolkVersion) #setup version info

2
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/README.md
View File

@ -57,7 +57,7 @@ This figure shows the role of some VOLK_GNSSSDR kernels in the context of a GNSS
If you use VOLK_GNSSSDR in your research and/or software, please cite the following paper: If you use VOLK_GNSSSDR in your research and/or software, please cite the following paper:
* C. Fern&aacute;ndez-Prades, J. Arribas, P. Closas, [*Accelerating GNSS Software Receivers*](https://zenodo.org/record/266493#.WJR8j7bhB89), in Proc. of the ION GNSS+ 2016 Conference, pp. 44-61, Portland, Oregon, Sept. 12-16, 2016. * C. Fern&aacute;ndez-Prades, J. Arribas, P. Closas, [*Accelerating GNSS Software Receivers*](https://zenodo.org/record/266493), in Proc. of the ION GNSS+ 2016 Conference, pp. 44-61, Portland, Oregon, Sept. 12-16, 2016.
Citations are useful for the continued development and maintenance of the library. Citations are useful for the continued development and maintenance of the library.

9
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/python/volk_gnsssdr_modtool/volk_gnsssdr_modtool_generate.py
View File

@ -102,6 +102,9 @@ class volk_gnsssdr_modtool:
os.makedirs(os.path.join(self.my_dict['destination'], 'volk_gnsssdr_' + self.my_dict['name'], 'kernels/volk_gnsssdr_' + self.my_dict['name'])) os.makedirs(os.path.join(self.my_dict['destination'], 'volk_gnsssdr_' + self.my_dict['name'], 'kernels/volk_gnsssdr_' + self.my_dict['name']))
current_kernel_names = self.get_current_kernels(); current_kernel_names = self.get_current_kernels();
need_ifdef_updates = ["constant.h", "volk_complex.h", "volk_malloc.h", "volk_prefs.h",
"volk_common.h", "volk_cpu.tmpl.h", "volk_config_fixed.tmpl.h",
"volk_typedefs.h", "volk.tmpl.h"]
for root, dirnames, filenames in os.walk(self.my_dict['base']): for root, dirnames, filenames in os.walk(self.my_dict['base']):
for name in filenames: for name in filenames:
@ -111,8 +114,14 @@ class volk_gnsssdr_modtool:
infile = os.path.join(root, name); infile = os.path.join(root, name);
instring = open(infile, 'r').read(); instring = open(infile, 'r').read();
outstring = re.sub(self.volk_gnsssdr, 'volk_gnsssdr_' + self.my_dict['name'], instring); outstring = re.sub(self.volk_gnsssdr, 'volk_gnsssdr_' + self.my_dict['name'], instring);
if name in need_ifdef_updates:
outstring = re.sub(self.volk_included, 'INCLUDED_VOLK_' + self.my_dict['name'].upper(), outstring)
newname = re.sub(self.volk_gnsssdr, 'volk_gnsssdr_' + self.my_dict['name'], name); newname = re.sub(self.volk_gnsssdr, 'volk_gnsssdr_' + self.my_dict['name'], name);
relpath = os.path.relpath(infile, self.my_dict['base']); relpath = os.path.relpath(infile, self.my_dict['base']);
if name == 'VolkConfig.cmake.in':
outstring = re.sub("VOLK", 'VOLK_' + self.my_dict['name'].upper(), outstring)
newname = "Volk%sConfig.cmake.in" % self.my_dict['name']
newrelpath = re.sub(self.volk_gnsssdr, 'volk_gnsssdr_' + self.my_dict['name'], relpath); newrelpath = re.sub(self.volk_gnsssdr, 'volk_gnsssdr_' + self.my_dict['name'], relpath);
dest = os.path.join(self.my_dict['destination'], 'volk_gnsssdr_' + self.my_dict['name'], os.path.dirname(newrelpath), newname); dest = os.path.join(self.my_dict['destination'], 'volk_gnsssdr_' + self.my_dict['name'], os.path.dirname(newrelpath), newname);

6
src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
View File

@ -252,12 +252,12 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
{ {
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; 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/GALILEO_TWO_PI;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m; tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN; tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
} }

1
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@ -249,7 +249,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{ {
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
//tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI; tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI;

8
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -264,16 +264,16 @@ int hybrid_observables_cc::general_work (int noutput_items,
{ {
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].d_TOW_hybrid_at_current_symbol; //tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Prn_timestamp_ms; tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m; tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN; tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
} }
} }
catch (const std::ifstream::failure& e) catch (const std::ifstream::failure& e)

1
src/algorithms/telemetry_decoder/adapters/CMakeLists.txt
View File

@ -31,6 +31,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/core/receiver
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/gnuradio_blocks
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs/libswiftcnav
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS}

3
src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt
View File

@ -29,6 +29,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/core/system_parameters ${CMAKE_SOURCE_DIR}/src/core/system_parameters
${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/core/receiver
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs/libswiftcnav
${GLOG_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
@ -39,4 +40,4 @@ file(GLOB TELEMETRY_DECODER_GR_BLOCKS_HEADERS "*.h")
list(SORT TELEMETRY_DECODER_GR_BLOCKS_HEADERS) list(SORT TELEMETRY_DECODER_GR_BLOCKS_HEADERS)
add_library(telemetry_decoder_gr_blocks ${TELEMETRY_DECODER_GR_BLOCKS_SOURCES} ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS}) add_library(telemetry_decoder_gr_blocks ${TELEMETRY_DECODER_GR_BLOCKS_SOURCES} ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS})
source_group(Headers FILES ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS}) source_group(Headers FILES ${TELEMETRY_DECODER_GR_BLOCKS_HEADERS})
target_link_libraries(telemetry_decoder_gr_blocks telemetry_decoder_lib gnss_system_parameters ${GNURADIO_RUNTIME_LIBRARIES}) target_link_libraries(telemetry_decoder_gr_blocks telemetry_decoder_libswiftcnav telemetry_decoder_lib gnss_system_parameters ${GNURADIO_RUNTIME_LIBRARIES})

506
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc
View File

@ -29,6 +29,7 @@
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include <bitset>
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
@ -39,14 +40,6 @@
using google::LogMessage; using google::LogMessage;
// logging levels
#define EVENT 2 // logs important events which don't occur every block
#define FLOW 3 // logs the function calls of block processing functions
#define SAMP_SYNC 4 // about 1 log entry per sample -> high output
#define LMORE 5 //
gps_l2c_telemetry_decoder_cc_sptr gps_l2c_telemetry_decoder_cc_sptr
gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump) gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump)
{ {
@ -56,9 +49,7 @@ gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump)
gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc( gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc(
Gnss_Satellite satellite, Gnss_Satellite satellite, bool dump) : gr::block("gps_l2c_telemetry_decoder_cc",
bool dump) :
gr::block("gps_l2c_telemetry_decoder_cc",
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{ {
@ -69,18 +60,19 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc(
// initialize internal vars // initialize internal vars
d_dump = dump; d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "GPS L2C M TELEMETRY PROCESSING: satellite " << d_satellite; DLOG(INFO) << "GPS L2C M TELEMETRY PROCESSING: satellite " << d_satellite;
d_block_size = GPS_L2_SAMPLES_PER_SYMBOL * GPS_L2_SYMBOLS_PER_BIT * GPS_L2_CNAV_DATA_PAGE_BITS * 2; // two CNAV frames
d_decimation_output_factor = 0; d_decimation_output_factor = 0;
//set_output_multiple (1); //set_output_multiple (1);
d_average_count = 0; d_average_count = 0;
d_flag_invert_buffer_symbols = false;
d_flag_invert_input_symbols = false;
d_channel = 0; d_channel = 0;
d_flag_valid_word = false; d_flag_valid_word = false;
d_TOW_at_current_symbol = 0; d_TOW_at_current_symbol = 0;
d_TOW_at_Preamble = 0; d_TOW_at_Preamble = 0;
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble d_state=0; //initial state
d_crc_error_count=0;
//initialize the CNAV frame decoder (libswiftcnav)
cnav_msg_decoder_init(&d_cnav_decoder);
} }
@ -91,18 +83,6 @@ gps_l2c_telemetry_decoder_cc::~gps_l2c_telemetry_decoder_cc()
} }
void gps_l2c_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
if (noutput_items != 0)
{
unsigned ninputs = ninput_items_required.size ();
for (unsigned i = 0; i < ninputs; i++)
ninput_items_required[i] = noutput_items;
}
//LOG(INFO) << "forecast(): " << "noutput_items=" << noutput_items << "\tninput_items_required ninput_items_required.size()=" << ninput_items_required.size();
}
void gps_l2c_telemetry_decoder_cc::set_decimation(int decimation) void gps_l2c_telemetry_decoder_cc::set_decimation(int decimation)
{ {
d_decimation_output_factor = decimation; d_decimation_output_factor = decimation;
@ -117,161 +97,123 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__(
Gnss_Synchro *out = (Gnss_Synchro *) output_items[0]; // output Gnss_Synchro *out = (Gnss_Synchro *) output_items[0]; // output
bool flag_new_cnav_frame = false; bool flag_new_cnav_frame = false;
int last_frame_preamble_start = 0; cnav_msg_t msg;
// copy correlation samples into samples vector u32 delay = 0;
d_sample_buf.push_back(in[0].Prompt_I);
//add the symbol to the decoder
u8 symbol_clip=(u8)(in[0].Prompt_I>0) * 255;
flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay);
consume_each(1); //one by one consume_each(1); //one by one
// decode only if enough samples in buffer
if(d_sample_buf.size() >= d_block_size)
{
if (in[0].Flag_valid_symbol_output == false) // check if the tracking is locked
{
LOG(INFO) << "Discarting channel " << d_channel << " tracking not ready!" << std::endl;
d_flag_valid_word = false;
}
else
{
d_flag_invert_buffer_symbols = d_flag_invert_input_symbols;
while (true)
{
if (d_flag_invert_buffer_symbols == true)
{
for (std::vector<double>::iterator symbol_it = d_sample_buf.begin(); symbol_it != d_sample_buf.end(); symbol_it++)
{
*symbol_it = -(*symbol_it);
}
//LOG(INFO)<<"Inverting buffer symbols";
}
// align symbols in pairs
// and obtain the bits by decoding the symbols (viterbi decoder)
// they can be already aligned or shifted by one position
std::vector<int> bits;
d_symbol_aligner_and_decoder.get_bits(d_sample_buf, bits);
//std::stringstream ss;
//for (std::vector<int>::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it)
// {
// ss << *bit_it;
// }
//LOG(INFO) << "get_bits=" << ss.str() << std::endl;
// search for preambles
// and extract the corresponding message candidates
std::vector<msg_candiate_int_t> msg_candidates;
d_frame_detector.get_frame_candidates(bits, msg_candidates);
// verify checksum
// and return the valid messages
std::vector<msg_candiate_int_t> valid_msgs;
d_crc_verifier.get_valid_frames(msg_candidates, valid_msgs);
if (valid_msgs.size() == 0)
{
if (d_flag_invert_buffer_symbols == d_flag_invert_input_symbols)
{
d_flag_invert_buffer_symbols = not d_flag_invert_buffer_symbols;
}
else
{//already tested the symbol inversion but CRC still fail
LOG(INFO) << "Discarting this buffer, no CNAV frames detected CH " << this->d_channel;
break;
}
}
else
{ //at least one frame has good CRC, keep the invert sign for the next frames
d_flag_invert_input_symbols = d_flag_invert_buffer_symbols;
std::vector<int> tmp_msg;
std::string msg;
//todo: now the symbol buffer size is two CNAV frames because the preamble is not detected.
// Use the first valid frame to realign the bufer symbols with the preamble start and not miss a frame
LOG(INFO) << valid_msgs.size() << " GOOD L2C CNAV FRAME DETECTED! CH " <<this->d_channel;
for (unsigned int i = 0;i < valid_msgs.size(); i++)
{
tmp_msg = valid_msgs.at(i).second;
d_CNAV_Message.decode_page(tmp_msg);
std::cout << "Valid CNAV frame with relative preamble start at " << valid_msgs.at(i).first << std::endl;
flag_new_cnav_frame = true;
d_flag_valid_word = true;
last_frame_preamble_start = valid_msgs.at(i).first;
// 4. Push the new navigation data to the queues
if (d_CNAV_Message.have_new_ephemeris() == true)
{
// get ephemeris object for this SV
std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj= std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris());
std::cout << "New GPS CNAV Ephemeris received for SV " << tmp_obj->i_satellite_PRN << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_CNAV_Message.have_new_iono() == true)
{
std::shared_ptr<Gps_CNAV_Iono> tmp_obj= std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono());
std::cout << "New GPS CNAV IONO model received for SV " << d_satellite.get_PRN() << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
}
break;
}
}
}
// clear all processed samples in the input buffer
d_sample_buf.clear();
}
// UPDATE GNSS SYNCHRO DATA // UPDATE GNSS SYNCHRO DATA
Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block
//1. Copy the current tracking output //1. Copy the current tracking output
current_synchro_data = in[0]; current_synchro_data = in[0];
if (d_flag_valid_word == true) //2. Add the telemetry decoder information
{ //check if new CNAV frame is available
double Prn_timestamp_at_preamble_ms = 0; if (flag_new_cnav_frame == true)
//2. Add the telemetry decoder information {
if (flag_new_cnav_frame == true) std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> raw_bits;
{ //Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder
//update TOW at the preamble instant for (u32 i = 0; i < GPS_L2_CNAV_DATA_PAGE_BITS ; i++) {
Prn_timestamp_at_preamble_ms = (in[0].Tracking_timestamp_secs * 1000.0) - (d_block_size - last_frame_preamble_start) * GPS_L2_M_PERIOD; raw_bits[GPS_L2_CNAV_DATA_PAGE_BITS-1-i]=((msg.raw_msg[i/8] >> (7 - i%8)) & 1u);
d_TOW_at_Preamble = d_CNAV_Message.d_TOW - GPS_L2_CNAV_DATA_PAGE_DURATION_S;
d_TOW_at_current_symbol = d_TOW_at_Preamble + (d_block_size - last_frame_preamble_start) * GPS_L2_M_PERIOD;
current_synchro_data.d_TOW = d_TOW_at_Preamble;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol;
current_synchro_data.Flag_preamble = false;
current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0;
current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;
}
else
{
d_TOW_at_current_symbol = d_TOW_at_Preamble + (d_block_size - last_frame_preamble_start) * GPS_L2_M_PERIOD;
current_synchro_data.d_TOW = d_TOW_at_Preamble;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol;
current_synchro_data.Flag_preamble = false;
current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0;
current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;
}
current_synchro_data.Flag_valid_word = true;
} }
d_CNAV_Message.decode_page(raw_bits);
//Push the new navigation data to the queues
if (d_CNAV_Message.have_new_ephemeris() == true)
{
// get ephemeris object for this SV
std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj= std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris());
std::cout << "New GPS CNAV Ephemeris received for SV " << tmp_obj->i_satellite_PRN << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_CNAV_Message.have_new_iono() == true)
{
std::shared_ptr<Gps_CNAV_Iono> tmp_obj= std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono());
std::cout << "New GPS CNAV IONO model received for SV " << d_satellite.get_PRN() << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_CNAV_Message.have_new_utc_model() == true)
{
std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj= std::make_shared<Gps_CNAV_Utc_Model>(d_CNAV_Message.get_utc_model());
std::cout << "New GPS CNAV UTC model received for SV " << d_satellite.get_PRN() << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
//update TOW at the preamble instant
double Prn_timestamp_at_preamble_ms = (in[0].Tracking_timestamp_secs * 1000.0);
d_TOW_at_Preamble=(int)msg.tow;
current_synchro_data.d_TOW = d_TOW_at_Preamble;
std::cout<<"["<<(int)msg.prn<<"] deco delay: "<<delay<<"[symbols]"<<std::endl;
d_TOW_at_current_symbol=(double)msg.tow * 6.0 + (double)delay * GPS_L2_M_PERIOD + GPS_L2_M_PERIOD;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol;
current_synchro_data.Flag_preamble = false;
current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0;
current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;
d_flag_valid_word=true;
}
else else
{
d_TOW_at_current_symbol +=GPS_L2_M_PERIOD;
current_synchro_data.d_TOW = d_TOW_at_Preamble;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
current_synchro_data.d_TOW_hybrid_at_current_symbol = d_TOW_at_current_symbol;
current_synchro_data.Flag_preamble = false;
current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0;
if (current_synchro_data.Flag_valid_symbol_output==false)
{ {
current_synchro_data.Flag_valid_word = false; d_flag_valid_word=false;
} }
}
current_synchro_data.Flag_valid_word=d_flag_valid_word;
// if (flag_PLL_180_deg_phase_locked == true)
// {
// //correct the accumulated phase for the Costas loop phase shift, if required
// current_synchro_data.Carrier_phase_rads += GPS_PI;
// }
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
tmp_double = d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_synchro_data.Prn_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = d_TOW_at_Preamble;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (const std::ifstream::failure & e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
}
d_average_count++; d_average_count++;
if (d_average_count == d_decimation_output_factor) if (d_average_count == d_decimation_output_factor)
{ {
d_average_count = 0; d_average_count = 0;
//3. Make the output (copy the object contents to the GNURadio reserved memory) //3. Make the output (copy the object contents to the GNURadio reserved memory)
out[0] = current_synchro_data; out[0] = current_synchro_data;
//std::cout<<"GPS L2 TLM output on CH="<<this->d_channel << " SAMPLE STAMP="<<d_sample_counter/d_decimation_output_factor<<std::endl; return 1;
return 1; }
}
else else
{ {
return 0; return 0;
} }
} }
@ -288,223 +230,25 @@ void gps_l2c_telemetry_decoder_cc::set_channel(int channel)
{ {
d_channel = channel; d_channel = channel;
LOG(INFO) << "GPS L2C CNAV channel set to " << channel; LOG(INFO) << "GPS L2C CNAV channel set to " << channel;
} // ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
// ### helper class for symbol alignment and viterbi decoding ###
gps_l2c_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_decoder()
{
// convolutional code properties
d_KK = 7;
int nn = 2;
int g_encoder[nn];
g_encoder[0] = 121; //171o
g_encoder[1] = 91; //133o
d_vd1 = new Viterbi_Decoder(g_encoder, d_KK, nn);
d_vd2 = new Viterbi_Decoder(g_encoder, d_KK, nn);
d_past_symbol = 0;
}
gps_l2c_telemetry_decoder_cc::symbol_aligner_and_decoder::~symbol_aligner_and_decoder()
{
delete d_vd1;
delete d_vd2;
}
void gps_l2c_telemetry_decoder_cc::symbol_aligner_and_decoder::reset()
{
d_past_symbol = 0;
d_vd1->reset();
d_vd2->reset();
}
bool gps_l2c_telemetry_decoder_cc::symbol_aligner_and_decoder::get_bits(const std::vector<double> & symbols, std::vector<int> & bits)
{
const int traceback_depth = 5 * d_KK;
int nbits_requested = symbols.size() / GPS_L2_SYMBOLS_PER_BIT;
int nbits_decoded;
// fill two vectors with the two possible symbol alignments
std::vector<double> symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector
std::vector<double> symbols_vd2; // shifted symbol vector -> add past sample in front of input vector
symbols_vd2.push_back(d_past_symbol);
for (std::vector<double>::const_iterator symbol_it = symbols.begin(); symbol_it != symbols.end() - 1; ++symbol_it)
{ {
symbols_vd2.push_back(*symbol_it); if (d_dump_file.is_open() == false)
}
// arrays for decoded bits
int * bits_vd1 = new int[nbits_requested];
int * bits_vd2 = new int[nbits_requested];
// decode
float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1, nbits_requested, nbits_decoded);
float metric_vd2 = d_vd2->decode_continuous(symbols_vd2.data(), traceback_depth, bits_vd2, nbits_requested, nbits_decoded);
//LOG(INFO)<<"metric_vd1="<<metric_vd1<<" metric_vd2="<<metric_vd2;
// choose the bits with the better metric
for (int i = 0; i < nbits_decoded; i++)
{
if (metric_vd1 > metric_vd2)
{// symbols aligned
bits.push_back(bits_vd1[i]);
}
else
{// symbols shifted
bits.push_back(bits_vd2[i]);
}
}
d_past_symbol = symbols.back();
delete[] bits_vd1;
delete[] bits_vd2;
return metric_vd1 > metric_vd2;
}
// ### helper class for detecting the preamble and collect the corresponding message candidates ###
void gps_l2c_telemetry_decoder_cc::frame_detector::reset()
{
d_buffer.clear();
}
void gps_l2c_telemetry_decoder_cc::frame_detector::get_frame_candidates(const std::vector<int> & bits, std::vector<std::pair<int,std::vector<int>>> & msg_candidates)
{
//std::stringstream ss;
unsigned int cnav_msg_length = 300;
std::vector<std::vector<int>> preambles = {{1, 0, 0, 0, 1, 0, 1 ,1}};
//LOG(INFO) << "get_frame_candidates(): " << "d_buffer.size()=" << d_buffer.size() << "\tbits.size()=" << bits.size();
//ss << "copy bits ";
int count = 0;
// copy new bits into the working buffer
for (std::vector<int>::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it)
{
d_buffer.push_back(*bit_it);
//ss << *bit_it;
count++;
}
//LOG(INFO) << ss.str() << " into working buffer (" << count << " bits)";
int relative_preamble_start = 0;
while(d_buffer.size() >= cnav_msg_length)
{
// compare with all preambles
for (std::vector<std::vector<int>>::iterator preample_it = preambles.begin(); preample_it < preambles.end(); ++preample_it)
{ {
bool preamble_detected = true; try
bool inv_preamble_detected = true; {
// compare the buffer bits with the preamble bits d_dump_filename = "telemetry_L2CM_";
for (std::vector<int>::iterator preample_bit_it = preample_it->begin(); preample_bit_it < preample_it->end(); ++preample_bit_it) d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
{ d_dump_filename.append(".dat");
preamble_detected = *preample_bit_it == d_buffer[preample_bit_it - preample_it->begin()] ? preamble_detected : false ; d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it - preample_it->begin()] ? inv_preamble_detected : false ; d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
} LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel
if (preamble_detected || inv_preamble_detected) << " Log file: " << d_dump_filename.c_str();
{ }
// copy candidate catch (const std::ifstream::failure &e)
std::vector<int> candidate; {
std::copy(d_buffer.begin(), d_buffer.begin() + cnav_msg_length, std::back_inserter(candidate)); LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
if(inv_preamble_detected) }
{
// invert bits
for (std::vector<int>::iterator candidate_bit_it = candidate.begin(); candidate_bit_it != candidate.end(); candidate_bit_it++)
*candidate_bit_it = *candidate_bit_it == 0 ? 1 : 0;
}
msg_candidates.push_back(std::pair<int,std::vector<int>>(relative_preamble_start, candidate));
//ss.str("");
//ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected?" inverted":" normal") << " detected! candidate=";
//for (std::vector<int>::iterator bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it)
// ss << *bit_it;
//LOG(INFO) << ss.str();
}
}
relative_preamble_start++;
// remove bit in front
d_buffer.pop_front();
}
}
// ### helper class for checking the CRC of the message candidates ###
void gps_l2c_telemetry_decoder_cc::crc_verifier::reset()
{
}
void gps_l2c_telemetry_decoder_cc::crc_verifier::get_valid_frames(const std::vector<msg_candiate_int_t> & msg_candidates, std::vector<msg_candiate_int_t> & valid_msgs)
{
std::vector <unsigned char> tmp_msg;
LOG(INFO) << "get_valid_frames(): " << "msg_candidates.size()=" << msg_candidates.size();
// for each candidate
for (std::vector<msg_candiate_int_t>::const_iterator candidate_it = msg_candidates.begin(); candidate_it < msg_candidates.end(); ++candidate_it)
{
// convert to bytes
std::vector<unsigned char> candidate_bytes;
zerropad_back_and_convert_to_bytes(candidate_it->second, candidate_bytes);
// verify CRC
d_checksum_agent.reset(0);
d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size());
unsigned int crc = d_checksum_agent.checksum();
//LOG(INFO) << "candidate " << ": final crc remainder= " << std::hex << crc
// << std::setfill(' ') << std::resetiosflags(std::ios::hex);
// the final remainder must be zero for a valid message, because the CRC is done over the received CRC value
if (crc == 0)
{
valid_msgs.push_back(msg_candiate_int_t(candidate_it->first, candidate_it->second));
std::cout << "Valid CNAV message found!"<<std::endl;
} }
} }
} }
void gps_l2c_telemetry_decoder_cc::crc_verifier::zerropad_back_and_convert_to_bytes(const std::vector<int> & msg_candidate, std::vector<unsigned char> & bytes)
{
//std::stringstream ss;
const size_t bits_per_byte = 8;
unsigned char byte = 0;
//LOG(INFO) << "zerropad_back_and_convert_to_bytes():" << byte;
for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
{
int idx_bit = candidate_bit_it - msg_candidate.begin();
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
// ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1)
{
bytes.push_back(byte);
//LOG(INFO) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
byte = 0;
}
}
bytes.push_back(byte); // implies: insert 6 zeros at the end to fit the 250bits into a multiple of bytes
//LOG(INFO) << " -> byte=" << std::setw(2)
// << std::setfill('0') << std::hex << (unsigned int)byte
// << std::setfill(' ') << std::resetiosflags(std::ios::hex);
}
void gps_l2c_telemetry_decoder_cc::crc_verifier::zerropad_front_and_convert_to_bytes(const std::vector<int> & msg_candidate, std::vector<unsigned char> & bytes)
{
//std::stringstream ss;
const size_t bits_per_byte = 8;
unsigned char byte = 0;
int idx_bit = 6; // insert 6 zeros at the front to fit the 250bits into a multiple of bytes
//LOG(INFO) << "zerropad_front_and_convert_to_bytes():" << byte;
for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
{
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
// ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1)
{
bytes.push_back(byte);
//LOG(INFO) << ss.str() << " -> byte=" << std::setw(2)
// << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
byte = 0;
}
idx_bit++;
}
//LOG(INFO) << " -> byte=" << std::setw(2)
// << std::setfill('0') << std::hex << (unsigned int)byte
// << std::setfill(' ') << std::resetiosflags(std::ios::hex);
}

69
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h
View File

@ -37,14 +37,19 @@
#include <string> #include <string>
#include <utility> // for pair #include <utility> // for pair
#include <vector> #include <vector>
#include <boost/crc.hpp>
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include "gnss_satellite.h" #include "gnss_satellite.h"
#include "viterbi_decoder.h"
#include "gps_cnav_navigation_message.h" #include "gps_cnav_navigation_message.h"
#include "gps_cnav_ephemeris.h" #include "gps_cnav_ephemeris.h"
#include "gps_cnav_iono.h" #include "gps_cnav_iono.h"
#include "concurrent_queue.h" #include "concurrent_queue.h"
extern "C" {
#include "cnav_msg.h"
#include "edc.h"
#include "bits.h"
}
#include "GPS_L2C.h" #include "GPS_L2C.h"
class gps_l2c_telemetry_decoder_cc; class gps_l2c_telemetry_decoder_cc;
@ -72,19 +77,13 @@ public:
int general_work (int noutput_items, gr_vector_int &ninput_items, int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
/*!
* \brief Function which tells the scheduler how many input items
* are required to produce noutput_items output items.
*/
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private: private:
friend gps_l2c_telemetry_decoder_cc_sptr friend gps_l2c_telemetry_decoder_cc_sptr
gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump);
gps_l2c_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); gps_l2c_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump);
void viterbi_decoder(double *page_part_symbols, int *page_part_bits);
void align_samples();
bool d_dump; bool d_dump;
Gnss_Satellite d_satellite; Gnss_Satellite d_satellite;
@ -93,59 +92,17 @@ private:
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;
cnav_msg_decoder_t d_cnav_decoder;
int d_state;
int d_crc_error_count;
double d_TOW_at_current_symbol; double d_TOW_at_current_symbol;
double d_TOW_at_Preamble; double d_TOW_at_Preamble;
bool d_flag_valid_word; bool d_flag_valid_word;
bool d_flag_invert_input_symbols;
bool d_flag_invert_buffer_symbols;
int d_decimation_output_factor; int d_decimation_output_factor;
int d_average_count; int d_average_count;
size_t d_block_size; //!< number of samples which are processed during one invocation of the algorithms
std::vector<double> d_sample_buf; //!< input buffer holding the samples to be processed in one block
typedef std::pair<int,std::vector<int>> msg_candiate_int_t;
typedef std::pair<int,std::vector<unsigned char>> msg_candiate_char_t;
// helper class for symbol alignment and Viterbi decoding
class symbol_aligner_and_decoder
{
public:
symbol_aligner_and_decoder();
~symbol_aligner_and_decoder();
void reset();
bool get_bits(const std::vector<double> & symbols, std::vector<int> & bits);
private:
int d_KK;
Viterbi_Decoder * d_vd1;
Viterbi_Decoder * d_vd2;
double d_past_symbol;
} d_symbol_aligner_and_decoder;
// helper class for detecting the preamble and collect the corresponding message candidates
class frame_detector
{
public:
void reset();
void get_frame_candidates(const std::vector<int> & bits, std::vector<std::pair<int, std::vector<int>>> & msg_candidates);
private:
std::deque<int> d_buffer;
} d_frame_detector;
// helper class for checking the CRC of the message candidates
class crc_verifier
{
public:
void reset();
void get_valid_frames(const std::vector<msg_candiate_int_t> & msg_candidates, std::vector<msg_candiate_int_t> & valid_msgs);
private:
typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type;
crc_24_q_type d_checksum_agent;
void zerropad_front_and_convert_to_bytes(const std::vector<int> & msg_candidate, std::vector<unsigned char> & bytes);
void zerropad_back_and_convert_to_bytes(const std::vector<int> & msg_candidate, std::vector<unsigned char> & bytes);
} d_crc_verifier;
Gps_CNAV_Navigation_Message d_CNAV_Message; Gps_CNAV_Navigation_Message d_CNAV_Message;
}; };

7
src/algorithms/telemetry_decoder/libs/CMakeLists.txt
View File

@ -16,9 +16,11 @@
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>. # along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
# #
add_subdirectory(libswiftcnav)
set(TELEMETRY_DECODER_LIB_SOURCES set(TELEMETRY_DECODER_LIB_SOURCES
gps_l1_ca_subframe_fsm.cc gps_l1_ca_subframe_fsm.cc
viterbi_decoder.cc viterbi_decoder.cc
) )
include_directories( include_directories(
@ -33,7 +35,8 @@ include_directories(
) )
file(GLOB TELEMETRY_DECODER_LIB_HEADERS "*.h") file(GLOB TELEMETRY_DECODER_LIB_HEADERS "*.h")
list(SORT TELEMETRY_DECODER_LIB_HEADERS) list(SORT TELEMETRY_DECODER_LIB_HEADERS)
add_library(telemetry_decoder_lib ${TELEMETRY_DECODER_LIB_SOURCES} ${TELEMETRY_DECODER_LIB_HEADERS}) add_library(telemetry_decoder_lib ${TELEMETRY_DECODER_LIB_SOURCES} ${TELEMETRY_DECODER_LIB_HEADERS})
source_group(Headers FILES ${TELEMETRY_DECODER_LIB_HEADERS}) source_group(Headers FILES ${TELEMETRY_DECODER_LIB_HEADERS})
target_link_libraries(telemetry_decoder_lib gnss_system_parameters) target_link_libraries(telemetry_decoder_lib gnss_system_parameters)

35
src/algorithms/telemetry_decoder/libs/libswiftcnav/CMakeLists.txt Normal file
View File

@ -0,0 +1,35 @@
# Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors)
#
# This file is part of GNSS-SDR.
#
# GNSS-SDR is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# GNSS-SDR is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
set(TELEMETRY_DECODER_LIBSWIFTCNAV_SOURCES
cnav_msg.c
bits.c
edc.c
viterbi27.c
)
include_directories(
$(CMAKE_CURRENT_SOURCE_DIR)
)
file(GLOB TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS "*.h")
list(SORT TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS)
add_library(telemetry_decoder_libswiftcnav STATIC ${TELEMETRY_DECODER_LIBSWIFTCNAV_SOURCES} ${TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS})
source_group(Headers FILES ${TELEMETRY_DECODER_LIBSWIFTCNAV_HEADERS})
set_target_properties(telemetry_decoder_libswiftcnav PROPERTIES LINKER_LANGUAGE C)

301
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c Normal file
View File

@ -0,0 +1,301 @@
/*!
* \file bits.c
* \author Fergus Noble <fergus@swift-nav.com>
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2013, 2016 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "bits.h"
#include <limits.h>
#include <string.h>
static const u8 bitn[16] = {0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4};
/** \defgroup bits Bit Utils
* Bit field packing, unpacking and utility functions.
* \{ */
/** Computes the parity of a 32-bit word.
*
* References:
* -# https://graphics.stanford.edu/~seander/bithacks.html#ParityParallel
*
* \param x Word for which to compute parity
* \return 1 if there are an odd number of bits set.
* 0 if there are an even number of bits set.
*/
u8 parity(u32 x)
{
x ^= x >> 16;
x ^= x >> 8;
x ^= x >> 4;
x &= 0xF;
return (0x6996 >> x) & 1;
}
/** Get bit field from buffer as an unsigned integer.
* Unpacks `len` bits at bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \return Bit field as an unsigned value.
*/
u32 getbitu(const u8 *buff, u32 pos, u8 len)
{
u32 bits = 0;
u32 i=0;
for (i = pos; i < pos + len; i++)
{
bits = (bits << 1) +
((buff[i/8] >> (7 - i%8)) & 1u);
}
return bits;
}
/** Get bit field from buffer as a signed integer.
* Unpacks `len` bits at bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* This function sign extends the `len` bit field to a signed 32 bit integer.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \return Bit field as a signed value.
*/
s32 getbits(const u8 *buff, u32 pos, u8 len)
{
s32 bits = (s32)getbitu(buff, pos, len);
/* Sign extend, taken from:
* http://graphics.stanford.edu/~seander/bithacks.html#VariableSignExtend
*/
s32 m = 1u << (len - 1);
return (bits ^ m) - m;
}
/** Set bit field in buffer from an unsigned integer.
* Packs `len` bits into bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \param data Unsigned integer to be packed into bit field.
*/
void setbitu(u8 *buff, u32 pos, u32 len, u32 data)
{
u32 mask = 1u << (len - 1);
if (len <= 0 || 32 < len)
return;
u32 i = 0;
for (i = pos; i < pos + len; i++, mask >>= 1)
{
if (data & mask)
buff[i/8] |= 1u << (7 - i % 8);
else
buff[i/8] &= ~(1u << (7 - i % 8));
}
}
/** Set bit field in buffer from a signed integer.
* Packs `len` bits into bit position `pos` from the start of the buffer.
* Maximum bit field length is 32 bits, i.e. `len <= 32`.
*
* \param buff
* \param pos Position in buffer of start of bit field in bits.
* \param len Length of bit field in bits.
* \param data Signed integer to be packed into bit field.
*/
void setbits(u8 *buff, u32 pos, u32 len, s32 data)
{
setbitu(buff, pos, len, (u32)data);
}
/**
* Shift MSB bit buffer contents to the left.
* The method performs in-place shift operation.
*
* \param[in,out] buf Pointer to buffer head.
* \param[in] size Number of bytes in the buffer.
* \param[in] shift Number of bits for left shift operation.
*
* \return None
*/
void bitshl(void *buf, u32 size, u32 shift)
{
if (shift > size * CHAR_BIT)
{
/* Quick check: if the shift is larger, than the buffer, zero the data */
memset(buf, 0, size);
return;
}
unsigned char *dst = buf; /* Destination byte. */
const unsigned char *src = dst + shift / CHAR_BIT; /* First source byte, possibly incomplete. */
u32 copy_bits = size * CHAR_BIT - shift; /* Number of bits to move */
u32 byte_shift = copy_bits % CHAR_BIT; /* Shift of data */
u32 full_bytes = copy_bits / CHAR_BIT; /* Number of bytes to move */
if (0 == byte_shift)
{
/* When moving data in character boundaries, use built-in functions: move
* data, and then zero the tail. */
memmove(dst, src, full_bytes);
memset(dst + full_bytes, 0, size - full_bytes);
}
else
{
/* Create an accumulator: it will hold a value of two consecutive bytes */
u32 acc = *src++;
u32 i = 0;
for (i = 0; i < full_bytes; ++i)
{
acc = (acc << CHAR_BIT) | *src++;
*dst++ = acc >> byte_shift;
}
*dst++ = acc << CHAR_BIT >> byte_shift;
if (full_bytes + 1 < size)
{
memset(dst, 0, size - full_bytes - 1);
}
}
}
/**
* Performs block bit copy operation.
*
* The function copies given number of bits from the source to destination.
*
* \param[in,out] dst Pointer to destination buffer.
* \param[in] dst_index Destination bit index.
* \param[in] src Source buffer.
* \param[in] src_index Source bit index.
* \param[in] count Number of bits to copy.
*
* \return None
*
* \todo This function can be optimized for copying aligned data and using
* proper native type like long.
*/
void bitcopy(void *dst, u32 dst_index, const void *src, u32 src_index,
u32 count)
{
u32 limit1 = count / 32;
u32 limit2 = count % 32;
u32 idx = 0;
for (idx = 0; idx < limit1; ++idx)
{
u32 tmp = getbitu(src, src_index, 32);
setbitu(dst, dst_index, 32, tmp);
src_index += 32;
dst_index += 32;
}
if (0 != limit2)
{
u32 tmp = getbitu(src, src_index, limit2);
setbitu(dst, dst_index, limit2, tmp);
}
}
/**
* Count number of bits set to certain value in 64 bits word
*
* \param[in] v input 64 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u64(u64 v, u8 bv)
{
u8 r = 0;
int i = 0;
for (i = 0; i < 16; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 64 - r;
}
/**
* Count number of bits set to certain value in 32 bits word
*
* \param[in] v input 32 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u32(u32 v, u8 bv)
{
u8 r = 0;
int i = 0;
for (i = 0; i < 8; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 32 - r;
}
/**
* Count number of bits set to certain value in 16 bits word
*
* \param[in] v input 16 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u16(u16 v, u8 bv)
{
u8 r = 0;
int i = 0;
for (i= 0; i < 4; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 16 - r;
}
/**
* Count number of bits set to certain value in 8 bits word
*
* \param[in] v input 8 bits word to count bits in
* \param[in] bv 1 or 0 - which value to count
*
* \return Number of bits set to one or zero.
*/
u8 count_bits_u8(u8 v, u8 bv)
{
u8 r = 0;
int i = 0;
for (i = 0; i < 2; i++)
r += bitn[(v >> (i*4)) & 0xf];
return bv == 1 ? r : 8 - r;
}
/** \} */

50
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h Normal file
View File

@ -0,0 +1,50 @@
/*!
* \file bits.h
* \author Fergus Noble <fergus@swift-nav.com>
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2013, 2016 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef LIBSWIFTNAV_BITS_H
#define LIBSWIFTNAV_BITS_H
#include "swift_common.h"
u8 parity(u32 x);
u32 getbitu(const u8 *buff, u32 pos, u8 len);
s32 getbits(const u8 *buff, u32 pos, u8 len);
void setbitu(u8 *buff, u32 pos, u32 len, u32 data);
void setbits(u8 *buff, u32 pos, u32 len, s32 data);
void bitcopy(void *dst, u32 dst_index,
const void *src, u32 src_index, u32 count);
void bitshl(void *buf, u32 size, u32 shift);
u8 count_bits_u64(u64 v, u8 bv);
u8 count_bits_u32(u32 v, u8 bv);
u8 count_bits_u16(u16 v, u8 bv);
u8 count_bits_u8(u8 v, u8 bv);
#endif /* LIBSWIFTNAV_BITS_H */

491
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c Normal file
View File

@ -0,0 +1,491 @@
/*!
* \file cnav_msg.c
* \author Valeri Atamaniouk <valeri@swift-nav.com>
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2016 Swift Navigation Inc.
* Contact: Valeri Atamaniouk <valeri@swift-nav.com>
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "edc.h"
#include "bits.h"
#include "cnav_msg.h"
#include <limits.h>
#include <string.h>
/** \defgroup GPS_L2 GPS L2
* GPS L2 operations
* \{ */
/** \defgroup gps_cnav_decoder Decoder
* GPS L2C CNAV message decoding.
* \{ */
/*
* Block Viterbi decoding parameters.
*/
/** Viterbi decoder reversed polynomial A */
#define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/
/** Viterbi decoder reversed polynomial B */
#define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */
/*
* GPS L2C message constants.
*/
/** GPS L2C preamble */
#define GPS_CNAV_PREAMBLE1 (0b10001011u)
/** Inverted GPS L2C preamble */
#define GPS_CNAV_PREAMBLE2 (0b01110100u)
/** GPS L2C preamble length in bits */
#define GPS_CNAV_PREAMBLE_LENGTH (8)
/** GPS L2C CNAV message length in bits */
#define GPS_CNAV_MSG_LENGTH (300)
/** GPS LC2 CNAV CRC length in bits */
#define GPS_CNAV_MSG_CRC_LENGTH (24)
/** GPS L2C CNAV message payload length in bits */
#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH-GPS_CNAV_MSG_CRC_LENGTH)
/** GPS L2C CNAV message lock detector threshold */
#define GPS_CNAV_LOCK_MAX_CRC_FAILS (10)
/**
* Computes CRC-24Q from a CNAV message buffer.
* CRC-24Q is computed for 274 bits. For a purpose of 8-bit alignment, the
* message is assumed to be prepended with four zero bits.
*
* \param[in] part Decoder component with payload
*
* \return CRC-24Q value.
*
* \private
*/
static u32 _cnav_compute_crc(cnav_v27_part_t *part)
{
u32 crc = crc24q_bits(0, part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
part->invert);
return crc;
}
/**
* Extracts CRC-24Q from a CNAV message buffer.
* CRC-24Q value is the last 24 bits from 300 bits message buffer.
*
* \param[in] part Decoded component with payload.
*
* \return CRC24-Q value.
*
* \private
*/
static u32 _cnav_extract_crc(const cnav_v27_part_t *part)
{
u32 crc = getbitu(part->decoded, GPS_CNAV_MSG_DATA_LENGTH,
GPS_CNAV_MSG_CRC_LENGTH);
if (part->invert)
{
crc ^= 0xFFFFFF;
}
return crc;
}
/**
* Helper to rescan for preamble in the received buffer.
* Occasionally there could be a false lock on message contents if it the
* preamble sequence is encountered in the message body. For this case, the
* function performs for a scan for a preamble with a different offset:
* - When found, the preamble octet is moved into the head of the buffer.
* - When not found, only 7 bits are left in the buffer.
*
* \param[in,out] part Decoded component.
*
* \return None
*
* \private
*/
static void _cnav_rescan_preamble(cnav_v27_part_t *part)
{
part->preamble_seen = false;
if (part->n_decoded > GPS_CNAV_PREAMBLE_LENGTH + 1)
{
size_t i = 0;
size_t j = 0;
for (i = 1, j = part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH; i < j; ++i)
{
u32 c = getbitu(part->decoded, i, GPS_CNAV_PREAMBLE_LENGTH);
if (GPS_CNAV_PREAMBLE1 == c || GPS_CNAV_PREAMBLE2 == c)
{
part->preamble_seen = true;
part->invert = (GPS_CNAV_PREAMBLE2 == c);
/* We shift the accumulated bits to the beginning of the buffer */
bitshl(part->decoded, sizeof(part->decoded), i);
part->n_decoded -= i;
break;
}
}
}
if (!part->preamble_seen && part->n_decoded >= GPS_CNAV_PREAMBLE_LENGTH)
{
bitshl(part->decoded, sizeof(part->decoded),
part->n_decoded - GPS_CNAV_PREAMBLE_LENGTH + 1);
part->n_decoded = GPS_CNAV_PREAMBLE_LENGTH - 1;
}
}
/**
* Feed a symbol into Viterbi decoder instance.
*
* The method uses block Viterbi decoder. It first accumulates initial number of
* symbols, and after that runs decoding every time the buffer is full. Only
* some of the decoded symbols are used.
*
* \param[in,out] part Decoder object
* \param[in] s Symbol (0x00 - Hard 0, 0xFF - Hard 1)
*
* \return None
*
* \private
*/
static void _cnav_add_symbol(cnav_v27_part_t *part, u8 ch)
{
part->symbols[part->n_symbols++] = ch;
if (part->init)
{
/* Initial step - load more symbols without decoding. */
if (part->n_symbols < (GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2)
{
return;
}
part->init = false;
}
else if (part->n_symbols < GPS_L2C_V27_DECODE_BITS * 2)
{
/* Wait until decoding block is accumulated */
return;
}
/* Feed accumulated symbols into the buffer, reset the number of accumulated
* symbols. */
v27_update(&part->dec, part->symbols, part->n_symbols / 2);
part->n_symbols = 0;
/* Decode N+M bits, where:
* - N - Number of bits to put into decoded buffer
* - M - Number of bits in the tail to ignore.
*/
unsigned char tmp_bits[ (GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS +
CHAR_BIT - 1) / CHAR_BIT];
v27_chainback_likely(&part->dec, tmp_bits,
GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
/* Read decoded bits and add them to the decoded buffer */
bitcopy(part->decoded, part->n_decoded, tmp_bits, 0, GPS_L2C_V27_DECODE_BITS);
part->n_decoded += GPS_L2C_V27_DECODE_BITS;
/* Depending on the decoder state, one of the following actions are
* possible:
* - If no message lock
* - If no preamble seen - look for preamble
* - If preamble seen - collect 300 bits
* - If 300 bits are collected - verify CRC
* - If CRC is OK - message lock is acquired
* - If CRC fails - rescan for preamble
* - If found - continue collecting 300 bits
* - If not found - continue preamble wait
* - If message lock
* - If 300 bits collected, compute CRC
* - If CRC is OK, message can be decoded
* - If CRC is not OK, discard data
*/
bool retry = true;
while (retry)
{
retry = false;
if (!part->preamble_seen)
{
/* Rescan for preamble if possible. The first bit is ignored. */
_cnav_rescan_preamble(part);
}
if (part->preamble_seen && GPS_CNAV_MSG_LENGTH <= part->n_decoded)
{
/* We have collected 300 bits starting from message preamble. Now try
* to compute CRC-24Q */
u32 crc = _cnav_compute_crc(part);
u32 crc2 = _cnav_extract_crc(part);
if (part->message_lock)
{
/* We have message lock */
part->crc_ok = (crc == crc2);
if (part->crc_ok)
{
/* Reset message lock counter */
part->n_crc_fail = 0;
}
else
{
/* Increment message lock counter */
part->n_crc_fail++;
if (part->n_crc_fail > GPS_CNAV_LOCK_MAX_CRC_FAILS)
{
/* CRC has failed too many times - drop the lock. */
part->n_crc_fail = 0;
part->message_lock = false;
part->preamble_seen = false;
/* Try to find a new preamble, reuse data from buffer. */
retry = true;
}
}
}
else if (crc == crc2)
{
/* CRC match - message can be decoded */
part->message_lock = true;
part->crc_ok = true;
part->n_crc_fail = 0;
}
else
{
/* There is no message lock and the CRC check fails. Assume there is
* false positive lock - rescan for preamble. */
part->crc_ok = false;
part->preamble_seen = false;
/* CRC mismatch - try to re-scan for preamble */
retry = true;
}
}
else
{
/* No preamble or preamble and less than 300 bits decoded */
}
}
}
/**
* Invert message bits in the buffer.
*
* The method inverts bits of the decoded data.
*
* \param[in,out] part Decoder component with a message buffer.
*
* \return None
*/
static void _cnav_msg_invert(cnav_v27_part_t *part)
{
size_t i = 0;
for (i = 0; i < sizeof(part->decoded); i++)
{
part->decoded[i] ^= 0xFFu;
}
}
/**
* Performs CNAV message decoding.
*
* This function decoded CNAV message, if the following conditions are met:
* - Buffer contains 300 bits.
* - First 8 bits are matching direct or inverse preamble.
* - Message data CRC matches one in the buffer.
*
* In case the message starts with inverted preamble, the data is inverted
* before parsing.
*
* \param[in,out] part Decoder component.
* \param[out] msg Container for a decoded message.
* \param[out] delay Delay of the message in symbols.
*
* \retval true The message has been decoded, and \a msg container is populated.
* \retval false Not enough data or CRC is not correct.
*
* \private
*/
static bool _cnav_msg_decode(cnav_v27_part_t *part, cnav_msg_t *msg, u32 *delay)
{
bool res = false;
if (GPS_CNAV_MSG_LENGTH <= part->n_decoded)
{
if (part->crc_ok)
{
/* CRC is OK */
if (part->invert)
{
_cnav_msg_invert(part);
}
msg->prn = getbitu(part->decoded, 8, 6);
msg->msg_id = getbitu(part->decoded, 14, 6);
msg->tow = getbitu(part->decoded, 20, 17);
msg->alert = getbitu(part->decoded, 37, 1) ? true : false;
/* copy RAW message for GNSS-SDR */
memcpy(msg->raw_msg,part->decoded,GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS);
*delay = (part->n_decoded - GPS_CNAV_MSG_LENGTH + GPS_L2C_V27_DELAY_BITS) * 2 + part->n_symbols;
if (part->invert)
{
_cnav_msg_invert(part);
}
res = true;
}
else
{
/* CRC mismatch - no decoding */
}
bitshl(part->decoded, sizeof(part->decoded), GPS_CNAV_MSG_LENGTH);
part->n_decoded -= GPS_CNAV_MSG_LENGTH;
}
return res;
}
/**
* Initialize CNAV decoder.
*
* CNAV decoder contains two Viterbi decoders that are used to estimate bit and
* message boundary.
*
* \param[out] dec Decoder structure.
*
* \return None
*/
void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
{
memset(dec, 0, sizeof(*dec));
v27_init(&dec->part1.dec,
dec->part1.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(),
0);
v27_init(&dec->part2.dec,
dec->part2.decisions,
GPS_L2_V27_HISTORY_LENGTH_BITS,
cnav_msg_decoder_get_poly(),
0);
dec->part1.init = true;
dec->part2.init = true;
_cnav_add_symbol(&dec->part2, 0x80);
}
/**
* Adds a received symbol to decoder.
*
* The method feeds the symbol into the decoder. In case there is a sufficient
* information to produce a message, the message is decoded and symbol delay is
* reported.
* The time of the last input symbol can be computed from the message ToW and
* delay by the formulae:
* \code
* symbolTime_ms = msg->tow * 6000 + *pdelay * 20
* \endcode
*
* \param[in,out] dec Decoder object.
* \param[in] symbol Symbol value probability, where 0x00 - 100% of 0,
* 0xFF - 100% of 1.
* \param[out] msg Buffer for decoded message. The message is available
* only when message lock is acquired and CRC is correct.
* \param[out] pdelay Delay of message generation in symbols.
*
* \retval true The message has been decoded. ToW parameter is available.
* \retval false More data is required.
*/
bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
u8 symbol,
cnav_msg_t *msg,
u32 *pdelay)
{
_cnav_add_symbol(&dec->part1, symbol);
_cnav_add_symbol(&dec->part2, symbol);
if (dec->part1.message_lock)
{
/* Flush data in decoder. */
dec->part2.n_decoded = 0;
dec->part2.n_symbols = 0;
return _cnav_msg_decode(&dec->part1, msg, pdelay);
}
if (dec->part2.message_lock)
{
/* Flush data in decoder. */
dec->part1.n_decoded = 0;
dec->part1.n_symbols = 0;
return _cnav_msg_decode(&dec->part2, msg, pdelay);
}
return false;
}
/**
* Provides a singleton polynomial object.
*
* The method constructs and returns polynomial object for CNAV message
* decoding. The same polynomial can be used also for other message handling.
*
* The object is initialized on the first call. The method is thread-safe.
*
* @return Pointer to polynomial object for CNAV message decoding.
*/
const v27_poly_t *cnav_msg_decoder_get_poly(void)
{
static v27_poly_t instance;
static bool initialized = false;
if (!initialized)
{
/* Coefficients for polynomial object */
const signed char coeffs[2] = { GPS_L2C_V27_POLY_A, GPS_L2C_V27_POLY_B };
/* Racing condition handling: the data can be potential initialized more
* than once in case multiple threads request concurrent access. However,
* nature of the v27_poly_init() function and data alignment ensure that
* the data returned from the earlier finished call is consistent and can
* be used even when re-initialization is happening.
*
* Other possible approaches are:
* - Replace late initialization with an explicit call.
* - Use POSIX synchronization objects like pthread_once_t.
*/
v27_poly_init(&instance, coeffs);
initialized = true;
}
return &instance;
}
/** \} */
/** \} */

121
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h Normal file
View File

@ -0,0 +1,121 @@
/*!
* \file cnav_msg.h
* \author Valeri Atamaniouk <valeri@swift-nav.com>
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2016 Swift Navigation Inc.
* Contact: Valeri Atamaniouk <valeri@swift-nav.com>
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef LIBSWIFTNAV_CNAV_MSG_H
#define LIBSWIFTNAV_CNAV_MSG_H
#include "fec.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <limits.h>
#include "swift_common.h"
/** \addtogroup GPS_L2
* \{ */
/** \addtogroup gps_cnav_decoder
* \{ */
/** Size of the Viterbi decoder history. */
#define GPS_L2_V27_HISTORY_LENGTH_BITS 64
/** Bits to accumulate before decoding starts. */
#define GPS_L2C_V27_INIT_BITS (32)
/** Bits to decode at a time. */
#define GPS_L2C_V27_DECODE_BITS (32)
/** Bits in decoder tail. We ignore them. */
#define GPS_L2C_V27_DELAY_BITS (32)
/**
* GPS CNAV message container.
*
* @sa cnav_msg_decoder_add_symbol
*/
typedef struct
{
u8 prn; /**< SV PRN. 0..31 */
u8 msg_id; /**< Message id. 0..31 */
u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */
bool alert; /**< CNAV message alert flag */
u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */
} cnav_msg_t;
/**
* GPS CNAV decoder component.
* This component controls symbol decoding string.
*
* @sa cnav_msg_decoder_t
*/
typedef struct {
v27_t dec; /**< Viterbi block decoder object */
v27_decision_t decisions[GPS_L2_V27_HISTORY_LENGTH_BITS];
/**< Decision graph */
unsigned char symbols[(GPS_L2C_V27_INIT_BITS + GPS_L2C_V27_DECODE_BITS) * 2];
/**< Symbol buffer */
size_t n_symbols; /**< Count of symbols in the symbol buffer */
unsigned char decoded[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS];
/**< Decode buffer */
size_t n_decoded; /**< Number of bits in the decode buffer */
bool preamble_seen; /**< When true, the decode buffer is aligned on
* preamble. */
bool invert; /**< When true, indicates the bits are inverted */
bool message_lock; /**< When true, indicates the message boundary
* is found. */
bool crc_ok; /**< Flag that the last message had good CRC */
size_t n_crc_fail; /**< Counter for CRC failures */
bool init; /**< Initial state flag. When true, initial bits
* do not produce output. */
} cnav_v27_part_t;
/**
* GPS CNAV message lock and decoder object.
*
* Decoder uses two Viterbi decoder objects to ensure the lock is acquired when
* the input symbol phase is not known.
*/
typedef struct
{
cnav_v27_part_t part1; /**< Decoder for odd symbol pairs */
cnav_v27_part_t part2; /**< Decoder for even symbol pairs */
} cnav_msg_decoder_t;
const v27_poly_t *cnav_msg_decoder_get_poly(void);
void cnav_msg_decoder_init(cnav_msg_decoder_t *dec);
bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
unsigned char symbol,
cnav_msg_t *msg,
u32 *delay);
/** \} */
/** \} */
#endif /* LIBSWIFTNAV_CNAV_MSG_H */

146
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c Normal file
View File

@ -0,0 +1,146 @@
/*!
* \file edc.c
* \author Fergus Noble <fergus@swift-nav.com>
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "edc.h"
/** \defgroup edc Error Detection and Correction
* Error detection and correction functions.
* \{ */
/** \defgroup crc CRC
* Cyclic redundancy checks.
* \{ */
static const u32 crc24qtab[256] = {
0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17,
0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E,
0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E,
0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7,
0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE,
0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7,
0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077,
0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E,
0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5,
0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC,
0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C,
0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375,
0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C,
0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15,
0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5,
0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C,
0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3,
0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A,
0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A,
0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703,
0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A,
0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863,
0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3,
0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA,
0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61,
0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58,
0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8,
0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1,
0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88,
0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1,
0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401,
0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538
};
/** Calculate Qualcomm 24-bit Cyclical Redundancy Check (CRC-24Q).
*
* The CRC polynomial used is:
* \f[
* x^{24} + x^{23} + x^{18} + x^{17} + x^{14} + x^{11} + x^{10} +
* x^7 + x^6 + x^5 + x^4 + x^3 + x+1
* \f]
* Mask 0x1864CFB, not reversed, not XOR'd
*
* \param buf Array of data to calculate CRC for
* \param len Length of data array
* \param crc Initial CRC value
*
* \return CRC-24Q value
*/
u32 crc24q(const u8 *buf, u32 len, u32 crc)
{
u32 i = 0;
for (i = 0; i < len; i++)
crc = ((crc << 8) & 0xFFFFFF) ^ crc24qtab[((crc >> 16) ^ buf[i]) & 0xff];
return crc;
}
/**
* Computes CRC-24Q for left-aligned bit message.
* This function is used for left-aligned bit messages, for example SBAS and
* GPS CNAV.
* GPS message is 300 bits total, but 276 bits without CRC. It takes 34.5
* 8-bit bytes, and when computing CRC the message has to be padded with zero
* bits.
*
* \param[in] crc Initial CRC value
* \param[in] buf Pointer to MSB-aligned data.
* \param[in] n_bits Number of bits in the data buffer.
* \param[in] invert Flag to compute inverted CRC.
*
* \return CRC-24Q value
*/
u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert)
{
u16 acc = 0;
u8 b = 0;
u32 shift = 8 - n_bits % 8;
u32 i = 0;
for (i = 0; i < n_bits / 8; ++i)
{
acc = (acc << 8) | *buf++;
if (invert)
{
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
}
acc = (acc << 8) | *buf;
if (invert)
{
acc ^= 0xFFu;
}
b = (acc >> shift) & 0xFFu;
crc = ((crc << 8) & 0xFFFFFFu) ^ crc24qtab[((crc >> 16) ^ b) & 0xFFu];
return crc;
}
/** \} */
/** \} */

41
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h Normal file
View File

@ -0,0 +1,41 @@
/*!
* \file edc.h
* \author Fergus Noble <fergus@swift-nav.com>
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef LIBSWIFTNAV_EDC_H
#define LIBSWIFTNAV_EDC_H
#include "swift_common.h"
u32 crc24q(const u8 *buf, u32 len, u32 crc);
u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert);
#endif /* LIBSWIFTNAV_EDC_H */

73
src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h Normal file
View File

@ -0,0 +1,73 @@
/*!
* \file fec.h
* \author Phil Karn, KA9Q
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2004, Phil Karn, KA9Q
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef LIBSWIFTNAV_FEC_H_
#define LIBSWIFTNAV_FEC_H_
/* r=1/2 k=7 convolutional encoder polynomials
* The NASA-DSN convention is to use V27POLYA inverted, then V27POLYB
* The CCSDS/NASA-GSFC convention is to use V27POLYB, then V27POLYA inverted
*/
#define V27POLYA 0x4f
#define V27POLYB 0x6d
typedef struct {
unsigned char c0[32];
unsigned char c1[32];
} v27_poly_t;
typedef struct {
unsigned int w[2];
} v27_decision_t;
/* State info for instance of r=1/2 k=7 Viterbi decoder
*/
typedef struct {
unsigned int metrics1[64]; /* Path metric buffer 1 */
unsigned int metrics2[64]; /* Path metric buffer 2 */
/* Pointers to path metrics, swapped on every bit */
unsigned int *old_metrics, *new_metrics;
const v27_poly_t *poly; /* Polynomial to use */
v27_decision_t *decisions; /* Beginning of decisions for block */
unsigned int decisions_index; /* Index of current decision */
unsigned int decisions_count; /* Number of decisions in history */
} v27_t;
void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2]);
void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count,
const v27_poly_t *poly, unsigned char initial_state);
void v27_update(v27_t *v, const unsigned char *syms, int nbits);
void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits,
unsigned char final_state);
void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits);
#endif

85
src/algorithms/telemetry_decoder/libs/libswiftcnav/swift_common.h Normal file
View File

@ -0,0 +1,85 @@
/*!
* \file swift_common.h
* \author Henry Hallam <henry@swift-nav.com>
* Fergus Noble <fergus@swift-nav.com>
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2012 Swift Navigation Inc.
* Contact: Henry Hallam <henry@swift-nav.com>
* Fergus Noble <fergus@swift-nav.com>
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef LIBSWIFTNAV_COMMON_H
#define LIBSWIFTNAV_COMMON_H
/** \defgroup common Common definitions
* Common definitions used throughout the library.
* \{ */
#define ABS(x) ((x) < 0 ? -(x) : (x))
#define MIN(x, y) (((x) < (y)) ? (x) : (y))
#define MAX(x, y) (((x) > (y)) ? (x) : (y))
#define CLAMP_DIFF(a,b) (MAX((a),(b)) - (b))
#include <stdint.h>
#include <stdbool.h>
#include <inttypes.h>
#ifndef COMMON_INT_TYPES
#define COMMON_INT_TYPES
/** \defgroup common_inttypes Integer types
* Specified-width integer type definitions for shorter and nicer code.
*
* These should be used in preference to unspecified width types such as
* `int` which can lead to portability issues between different platforms.
* \{ */
/** Signed 8-bit integer. */
typedef int8_t s8;
/** Signed 16-bit integer. */
typedef int16_t s16;
/** Signed 32-bit integer. */
typedef int32_t s32;
/** Signed 64-bit integer. */
typedef int64_t s64;
/** Unsigned 8-bit integer. */
typedef uint8_t u8;
/** Unsigned 16-bit integer. */
typedef uint16_t u16;
/** Unsigned 32-bit integer. */
typedef uint32_t u32;
/** Unsigned 64-bit integer. */
typedef uint64_t u64;
#endif
/** \} */
/** \} */
#endif /* LIBSWIFTNAV_COMMON_H */

252
src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c Normal file
View File

@ -0,0 +1,252 @@
/*!
* \file viterbi27.c
* \author Phil Karn, KA9Q
* \brief K=7 r=1/2 Viterbi decoder in portable C
*
* -------------------------------------------------------------------------
* This file was originally borrowed from libswiftnav
* <https://github.com/swift-nav/libswiftnav>,
* a portable C library implementing GNSS related functions and algorithms,
* and then modified by J. Arribas and C. Fernandez
*
* Copyright (C) 2004, Phil Karn, KA9Q
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* This file is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as
* published by the Free Software Foundation, version 3.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdlib.h>
#include "fec.h"
static inline int parity(int x)
{
x ^= x >> 16;
x ^= x >> 8;
x ^= x >> 4;
x &= 0xf;
return (0x6996 >> x) & 1;
}
/** Initialize a v27_poly_t struct for use with a v27_t decoder.
*
* \param poly Structure to initialize.
* \param polynomial Byte array representing the desired polynomials.
*/
void v27_poly_init(v27_poly_t *poly, const signed char polynomial[2])
{
int state;
for(state = 0; state < 32; state++)
{
poly->c0[state] = (polynomial[0] < 0) ^ parity((2*state) & abs(polynomial[0])) ? 255 : 0;
poly->c1[state] = (polynomial[1] < 0) ^ parity((2*state) & abs(polynomial[1])) ? 255 : 0;
}
}
/** Initialize a v27_t struct for Viterbi decoding.
*
* \param v Structure to initialize
* \param decisions Array of v27_decision_t structs, capacity = decisions_count.
* Must remain valid as long as v is used.
* \param decisions_count Size of decisions array. Equal to the number of bit
* decisions kept in history.
* \param poly Struct describing the polynomials to use. Must remain valid as
* long as v is used. May be shared between multiple decoders.
* \param initial_state Initial state of the decoder shift register. Usually zero.
*/
void v27_init(v27_t *v, v27_decision_t *decisions, unsigned int decisions_count,
const v27_poly_t *poly, unsigned char initial_state)
{
int i;
v->old_metrics = v->metrics1;
v->new_metrics = v->metrics2;
v->poly = poly;
v->decisions = decisions;
v->decisions_index = 0;
v->decisions_count = decisions_count;
for(i = 0; i < 64; i++)
v->old_metrics[i] = 63;
v->old_metrics[initial_state & 63] = 0; /* Bias known start state */
}
/* C-language butterfly */
#define BFLY(i) {\
unsigned int metric,m0,m1,decision;\
metric = (v->poly->c0[i] ^ sym0) + (v->poly->c1[i] ^ sym1);\
m0 = v->old_metrics[i] + metric;\
m1 = v->old_metrics[i+32] + (510 - metric);\
decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i)&31);\
m0 -= (metric+metric-510);\
m1 += (metric+metric-510);\
decision = (signed int)(m0-m1) > 0;\
v->new_metrics[2*i+1] = decision ? m1 : m0;\
d->w[i/16] |= decision << ((2*i+1)&31);\
}
/** Update a v27_t decoder with a block of symbols.
*
* \param v Structure to update.
* \param syms Array of symbols to use. Must contain two symbols per bit.
* 0xff = strong 1, 0x00 = strong 0.
* \param nbits Number of bits corresponding to the provided symbols.
*/
void v27_update(v27_t *v, const unsigned char *syms, int nbits)
{
unsigned char sym0, sym1;
unsigned int *tmp;
int normalize = 0;
while(nbits--)
{
v27_decision_t *d = &v->decisions[v->decisions_index];
d->w[0] = d->w[1] = 0;
sym0 = *syms++;
sym1 = *syms++;
BFLY(0);
BFLY(1);
BFLY(2);
BFLY(3);
BFLY(4);
BFLY(5);
BFLY(6);
BFLY(7);
BFLY(8);
BFLY(9);
BFLY(10);
BFLY(11);
BFLY(12);
BFLY(13);
BFLY(14);
BFLY(15);
BFLY(16);
BFLY(17);
BFLY(18);
BFLY(19);
BFLY(20);
BFLY(21);
BFLY(22);
BFLY(23);
BFLY(24);
BFLY(25);
BFLY(26);
BFLY(27);
BFLY(28);
BFLY(29);
BFLY(30);
BFLY(31);
/* Normalize metrics if they are nearing overflow */
if(v->new_metrics[0] > (1 << 30))
{
int i;
unsigned int minmetric = 1 << 31;
for(i = 0; i < 64; i++)
{
if(v->new_metrics[i] < minmetric)
minmetric = v->new_metrics[i];
}
for(i = 0; i < 64; i++)
v->new_metrics[i] -= minmetric;
normalize += minmetric;
}
/* Advance decision index */
if(++v->decisions_index >= v->decisions_count)
v->decisions_index = 0;
/* Swap pointers to old and new metrics */
tmp = v->old_metrics;
v->old_metrics = v->new_metrics;
v->new_metrics = tmp;
}
}
/** Retrieve the most likely output bit sequence with known final state from
* a v27_t decoder.
*
* \param v Structure to use.
* \param data Array used to store output bits, capacity = nbits.
* \param nbits Number of bits to retrieve.
* \param final_state Known final state of the decoder shift register.
*/
void v27_chainback_fixed(v27_t *v, unsigned char *data, unsigned int nbits,
unsigned char final_state)
{
int k;
unsigned int decisions_index = v->decisions_index;
final_state %= 64;
final_state <<= 2;
while(nbits-- != 0)
{
/* Decrement decision index */
decisions_index = (decisions_index == 0) ?
v->decisions_count-1 : decisions_index-1;
v27_decision_t *d = &v->decisions[decisions_index];
k = (d->w[(final_state >> 2) / 32] >> ((final_state >> 2) % 32)) & 1;
/* The store into data[] only needs to be done every 8 bits.
* But this avoids a conditional branch, and the writes will
* combine in the cache anyway
*/
data[nbits >> 3] = final_state = (final_state >> 1) | (k << 7);
}
}
/** Retrieve the most likely output bit sequence with unknown final state from
* a v27_t decoder.
*
* \param v Structure to use.
* \param data Array used to store output bits, capacity = nbits.
* \param nbits Number of bits to retrieve.
*/
void v27_chainback_likely(v27_t *v, unsigned char *data, unsigned int nbits)
{
/* Determine state with minimum metric */
int i;
unsigned int best_metric = 0xffffffff;
unsigned char best_state = 0;
for(i = 0; i < 64; i++)
{
if(v->new_metrics[i] < best_metric)
{
best_metric = v->new_metrics[i];
best_state = i;
}
}
v27_chainback_fixed(v, data, nbits, best_state);
}

2
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
View File

@ -438,8 +438,6 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items __attribu
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag()); current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in); current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;

3
src/core/receiver/CMakeLists.txt
View File

@ -65,6 +65,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/adapters
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/gnuradio_blocks
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs/libswiftcnav
${CMAKE_SOURCE_DIR}/src/algorithms/observables/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/observables/adapters
${CMAKE_SOURCE_DIR}/src/algorithms/observables/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/observables/gnuradio_blocks
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters
@ -141,6 +142,8 @@ target_link_libraries(gnss_rx ${Boost_LIBRARIES}
tracking_lib tracking_lib
tracking_adapters tracking_adapters
channel_adapters channel_adapters
telemetry_decoder_libswiftcnav
telemetry_decoder_lib
telemetry_decoder_adapters telemetry_decoder_adapters
obs_adapters obs_adapters
pvt_adapters pvt_adapters

42
src/core/system_parameters/GPS_L2C.h
View File

@ -95,12 +95,14 @@ const int32_t GPS_L2C_M_INIT_REG[115] =
// NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix III) // NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix III)
#define GPS_CNAV_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} #define GPS_CNAV_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1}
#define GPS_CNAV_PREAMBLE_STR "10001011"
#define GPS_CNAV_INV_PREAMBLE_STR "01110100"
const int GPS_L2_CNAV_DATA_PAGE_BITS = 300; //!< GPS L2 CNAV page length, including preamble and CRC [bits] const int GPS_L2_CNAV_DATA_PAGE_BITS = 300; //!< GPS L2 CNAV page length, including preamble and CRC [bits]
const int GPS_L2_SYMBOLS_PER_BIT = 2; const int GPS_L2_SYMBOLS_PER_BIT = 2;
const int GPS_L2_SAMPLES_PER_SYMBOL = 1; const int GPS_L2_SAMPLES_PER_SYMBOL = 1;
const int GPS_L2_CNAV_DATA_PAGE_SYMBOLS = 600;
const int GPS_L2_CNAV_DATA_PAGE_DURATION_S = 12; const int GPS_L2_CNAV_DATA_PAGE_DURATION_S = 12;
const int GPS_L2_CNAV_DATA_PAGE_BITS_EXTENDED_BYTES = 304; //!< GPS L2 CNAV page length, including preamble and CRC [bits]
// common to all messages // common to all messages
const std::vector<std::pair<int,int> > CNAV_PRN( { {9,6} } ); const std::vector<std::pair<int,int> > CNAV_PRN( { {9,6} } );
@ -127,15 +129,15 @@ const std::vector<std::pair<int,int> > CNAV_A_DOT({{108,25}});
const double CNAV_A_DOT_LSB = TWO_N21; const double CNAV_A_DOT_LSB = TWO_N21;
const std::vector<std::pair<int,int> > CNAV_DELTA_N0({{133,17}}); const std::vector<std::pair<int,int> > CNAV_DELTA_N0({{133,17}});
const double CNAV_DELTA_N0_LSB = TWO_N44; const double CNAV_DELTA_N0_LSB = TWO_N44*GPS_L2_PI; //semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_DELTA_N0_DOT({{150,23}}); const std::vector<std::pair<int,int> > CNAV_DELTA_N0_DOT({{150,23}});
const double CNAV_DELTA_N0_DOT_LSB = TWO_N57; const double CNAV_DELTA_N0_DOT_LSB = TWO_N57*GPS_L2_PI;//semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_M0({{173,33}}); const std::vector<std::pair<int,int> > CNAV_M0({{173,33}});
const double CNAV_M0_LSB = TWO_N32; const double CNAV_M0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_E_ECCENTRICITY({{206,33}}); const std::vector<std::pair<int,int> > CNAV_E_ECCENTRICITY({{206,33}});
const double CNAV_E_ECCENTRICITY_LSB = TWO_N34; const double CNAV_E_ECCENTRICITY_LSB = TWO_N34;
const std::vector<std::pair<int,int> > CNAV_OMEGA({{239,33}}); const std::vector<std::pair<int,int> > CNAV_OMEGA({{239,33}});
const double CNAV_OMEGA_LSB = TWO_N32; const double CNAV_OMEGA_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_INTEGRITY_FLAG({{272,1}}); const std::vector<std::pair<int,int> > CNAV_INTEGRITY_FLAG({{272,1}});
const std::vector<std::pair<int,int> > CNAV_L2_PHASING_FLAG({{273,1}}); const std::vector<std::pair<int,int> > CNAV_L2_PHASING_FLAG({{273,1}});
@ -144,13 +146,13 @@ const std::vector<std::pair<int,int> > CNAV_L2_PHASING_FLAG({{273,1}});
const std::vector<std::pair<int,int> > CNAV_TOE2({{39,11}}); const std::vector<std::pair<int,int> > CNAV_TOE2({{39,11}});
const double CNAV_TOE2_LSB = 300.0; const double CNAV_TOE2_LSB = 300.0;
const std::vector<std::pair<int,int> > CNAV_OMEGA0({{50,33}}); const std::vector<std::pair<int,int> > CNAV_OMEGA0({{50,33}});
const double CNAV_OMEGA0_LSB = TWO_N32; const double CNAV_OMEGA0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_I0({{83,33}}); const std::vector<std::pair<int,int> > CNAV_I0({{83,33}});
const double CNAV_I0_LSB = TWO_N32; const double CNAV_I0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_DELTA_OMEGA_DOT({{116,17}}); //Relative to REF = -2.6 x 10-9 semi-circles/second. const std::vector<std::pair<int,int> > CNAV_DELTA_OMEGA_DOT({{116,17}}); //Relative to REF = -2.6 x 10-9 semi-circles/second.
const double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44; const double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44*GPS_L2_PI;//semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_I0_DOT({{133,15}}); const std::vector<std::pair<int,int> > CNAV_I0_DOT({{133,15}});
const double CNAV_I0_DOT_LSB = TWO_N44; const double CNAV_I0_DOT_LSB = TWO_N44*GPS_L2_PI;//semi-circles to radians
const std::vector<std::pair<int,int> > CNAV_CIS({{148,16}}); const std::vector<std::pair<int,int> > CNAV_CIS({{148,16}});
const double CNAV_CIS_LSB = TWO_N30; const double CNAV_CIS_LSB = TWO_N30;
const std::vector<std::pair<int,int> > CNAV_CIC({{164,16}}); const std::vector<std::pair<int,int> > CNAV_CIC({{164,16}});
@ -211,6 +213,28 @@ const double CNAV_BETA3_LSB = TWO_P16;
const std::vector<std::pair<int,int> > CNAV_WNOP({{257,8}}); const std::vector<std::pair<int,int> > CNAV_WNOP({{257,8}});
// MESSAGE TYPE 33 (CLOCK and UTC)
const std::vector<std::pair<int,int> > CNAV_A0({{128,16}});
const double CNAV_A0_LSB = TWO_N35;
const std::vector<std::pair<int,int> > CNAV_A1({{144,13}});
const double CNAV_A1_LSB = TWO_N51;
const std::vector<std::pair<int,int> > CNAV_A2({{157,7}});
const double CNAV_A2_LSB = TWO_N68;
const std::vector<std::pair<int,int> > CNAV_DELTA_TLS({{164,8}});
const double CNAV_DELTA_TLS_LSB = 1;
const std::vector<std::pair<int,int> > CNAV_TOT({{172,16}});
const double CNAV_TOT_LSB = TWO_P4;
const std::vector<std::pair<int,int> > CNAV_WN_OT({{188,13}});
const double CNAV_WN_OT_LSB = 1;
const std::vector<std::pair<int,int> > CNAV_WN_LSF({{201,13}});
const double CNAV_WN_LSF_LSB = 1;
const std::vector<std::pair<int,int> > CNAV_DN({{214,4}});
const double CNAV_DN_LSB = 1;
const std::vector<std::pair<int,int> > CNAV_DELTA_TLSF({{218,8}});
const double CNAV_DELTA_TLSF_LSB = 1;
// TODO: Add more frames (Almanac, etc...) // TODO: Add more frames (Almanac, etc...)

1
src/core/system_parameters/MATH_CONSTANTS.h
View File

@ -86,6 +86,7 @@ const double TWO_N55 = (2.775557561562891e-017); //!< 2^-55
const double TWO_N57 = (6.938893903907228e-18); //!< 2^-57 const double TWO_N57 = (6.938893903907228e-18); //!< 2^-57
const double TWO_N59 = (1.73472347597681e-018); //!< 2^-59 const double TWO_N59 = (1.73472347597681e-018); //!< 2^-59
const double TWO_N60 = (8.673617379884036e-19); //!< 2^-60 const double TWO_N60 = (8.673617379884036e-19); //!< 2^-60
const double TWO_N68 = (3.388131789017201e-21); //!< 2^-68
const double PI_TWO_N19 = (5.992112452678286e-006); //!< Pi*2^-19 const double PI_TWO_N19 = (5.992112452678286e-006); //!< Pi*2^-19

2
src/core/system_parameters/galileo_ephemeris.h
View File

@ -119,7 +119,7 @@ public:
void serialize(Archive& archive, const unsigned int version) void serialize(Archive& archive, const unsigned int version)
{ {
using boost::serialization::make_nvp; using boost::serialization::make_nvp;
if(version){};
archive & make_nvp("i_satellite_PRN",i_satellite_PRN); archive & make_nvp("i_satellite_PRN",i_satellite_PRN);
archive & make_nvp("M0_1", M0_1); archive & make_nvp("M0_1", M0_1);
archive & make_nvp("e_1", e_1); archive & make_nvp("e_1", e_1);

13
src/core/system_parameters/gps_cnav_ephemeris.cc
View File

@ -177,7 +177,7 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime)
} }
void Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
{ {
double tk; double tk;
double a; double a;
@ -271,4 +271,15 @@ void Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i); d_satvel_Z = d_satpos_Y * sin(i);
// Time from ephemeris reference clock
tk = check_t(transmitTime - d_Toc);
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */
dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_L2_C_m_s * GPS_L2_C_m_s);
return dtr_s;
} }

5
src/core/system_parameters/gps_cnav_ephemeris.h
View File

@ -153,7 +153,8 @@ public:
archive & make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] archive & make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s]
archive & make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] archive & make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week]
archive & make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] archive & make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
archive & make_nvp("d_DELTA_A", d_DELTA_A);
archive & make_nvp("d_A_DOT", d_A_DOT);
archive & make_nvp("d_A_f0", d_A_f0); //!< Coefficient 0 of code phase offset model [s] archive & make_nvp("d_A_f0", d_A_f0); //!< Coefficient 0 of code phase offset model [s]
archive & make_nvp("d_A_f1", d_A_f1); //!< Coefficient 1 of code phase offset model [s/s] archive & make_nvp("d_A_f1", d_A_f1); //!< Coefficient 1 of code phase offset model [s/s]
@ -168,7 +169,7 @@ public:
* \brief Compute the ECEF SV coordinates and ECEF velocity * \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200E) * Implementation of Table 20-IV (IS-GPS-200E)
*/ */
void satellitePosition(double transmitTime); double satellitePosition(double transmitTime);
/*! /*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction

84
src/core/system_parameters/gps_cnav_navigation_message.cc
View File

@ -71,13 +71,6 @@ Gps_CNAV_Navigation_Message::Gps_CNAV_Navigation_Message()
} }
void Gps_CNAV_Navigation_Message::print_gps_word_bytes(unsigned int GPS_word)
{
std::cout << " Word =";
std::cout << std::bitset<32>(GPS_word);
std::cout << std::endl;
}
bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter) bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter)
{ {
@ -174,24 +167,8 @@ signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset<
} }
void Gps_CNAV_Navigation_Message::decode_page(std::vector<int> data) void Gps_CNAV_Navigation_Message::decode_page(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> data_bits)
{ {
std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> data_bits;
try
{
for(int i = 0; i < GPS_L2_CNAV_DATA_PAGE_BITS; i++)
{
data_bits[i] = static_cast<uint8_t>(data[GPS_L2_CNAV_DATA_PAGE_BITS - i - 1]);
}
}
catch(std::exception &e)
{
std::cout << "Exception converting to bitset " << e.what() << std::endl;
return;
}
int PRN; int PRN;
int page_type; int page_type;
@ -210,7 +187,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::vector<int> data)
page_type = static_cast<int>(read_navigation_unsigned(data_bits, CNAV_MSG_TYPE)); page_type = static_cast<int>(read_navigation_unsigned(data_bits, CNAV_MSG_TYPE));
std::cout << "PRN=" << PRN << " TOW=" << d_TOW << " alert_flag=" << alert_flag << " page_type= " << page_type << std::endl; //std::cout << "PRN=" << PRN << " TOW=" << d_TOW << " alert_flag=" << alert_flag << " page_type= " << page_type << std::endl;
switch(page_type) switch(page_type)
{ {
case 10: // Ephemeris 1/2 case 10: // Ephemeris 1/2
@ -231,7 +208,7 @@ void Gps_CNAV_Navigation_Message::decode_page(std::vector<int> data)
ephemeris_record.d_DELTA_DOT_N = ephemeris_record.d_DELTA_DOT_N * CNAV_DELTA_N0_DOT_LSB; ephemeris_record.d_DELTA_DOT_N = ephemeris_record.d_DELTA_DOT_N * CNAV_DELTA_N0_DOT_LSB;
ephemeris_record.d_M_0 = static_cast<double>(read_navigation_signed(data_bits, CNAV_M0)); ephemeris_record.d_M_0 = static_cast<double>(read_navigation_signed(data_bits, CNAV_M0));
ephemeris_record.d_M_0 = ephemeris_record.d_M_0 * CNAV_M0_LSB; ephemeris_record.d_M_0 = ephemeris_record.d_M_0 * CNAV_M0_LSB;
ephemeris_record.d_e_eccentricity = static_cast<double>(read_navigation_signed(data_bits, CNAV_E_ECCENTRICITY)); ephemeris_record.d_e_eccentricity = static_cast<double>(read_navigation_unsigned(data_bits, CNAV_E_ECCENTRICITY));
ephemeris_record.d_e_eccentricity = ephemeris_record.d_e_eccentricity * CNAV_E_ECCENTRICITY_LSB; ephemeris_record.d_e_eccentricity = ephemeris_record.d_e_eccentricity * CNAV_E_ECCENTRICITY_LSB;
ephemeris_record.d_OMEGA = static_cast<double>(read_navigation_signed(data_bits, CNAV_OMEGA)); ephemeris_record.d_OMEGA = static_cast<double>(read_navigation_signed(data_bits, CNAV_OMEGA));
ephemeris_record.d_OMEGA = ephemeris_record.d_OMEGA * CNAV_OMEGA_LSB; ephemeris_record.d_OMEGA = ephemeris_record.d_OMEGA * CNAV_OMEGA_LSB;
@ -309,6 +286,46 @@ void Gps_CNAV_Navigation_Message::decode_page(std::vector<int> data)
iono_record.d_beta3 = iono_record.d_beta3 * CNAV_BETA3_LSB; iono_record.d_beta3 = iono_record.d_beta3 * CNAV_BETA3_LSB;
b_flag_iono_valid = true; b_flag_iono_valid = true;
break; break;
case 33: // (CLOCK & UTC)
ephemeris_record.d_Top = static_cast<double>(read_navigation_unsigned(data_bits, CNAV_TOP1));
ephemeris_record.d_Top = ephemeris_record.d_Top * CNAV_TOP1_LSB;
ephemeris_record.d_Toc = static_cast<double>(read_navigation_unsigned(data_bits, CNAV_TOC));
ephemeris_record.d_Toc = ephemeris_record.d_Toc * CNAV_TOC_LSB;
ephemeris_record.d_A_f0 = static_cast<double>(read_navigation_signed(data_bits, CNAV_AF0));
ephemeris_record.d_A_f0 = ephemeris_record.d_A_f0 * CNAV_AF0_LSB;
ephemeris_record.d_A_f1 = static_cast<double>(read_navigation_signed(data_bits, CNAV_AF1));
ephemeris_record.d_A_f1 = ephemeris_record.d_A_f1 * CNAV_AF1_LSB;
ephemeris_record.d_A_f2 = static_cast<double>(read_navigation_signed(data_bits, CNAV_AF2));
ephemeris_record.d_A_f2 = ephemeris_record.d_A_f2 * CNAV_AF2_LSB;
utc_model_record.d_A0 = static_cast<double>(read_navigation_signed(data_bits, CNAV_A0));
utc_model_record.d_A0 = utc_model_record.d_A0 * CNAV_A0_LSB;
utc_model_record.d_A1 = static_cast<double>(read_navigation_signed(data_bits, CNAV_A1));
utc_model_record.d_A1 = utc_model_record.d_A1 * CNAV_A1_LSB;
utc_model_record.d_A2 = static_cast<double>(read_navigation_signed(data_bits, CNAV_A2));
utc_model_record.d_A2 = utc_model_record.d_A2 * CNAV_A2_LSB;
utc_model_record.d_DeltaT_LS = static_cast<double>(read_navigation_signed(data_bits, CNAV_DELTA_TLS));
utc_model_record.d_DeltaT_LS = utc_model_record.d_DeltaT_LS * CNAV_DELTA_TLS_LSB;
utc_model_record.d_t_OT = static_cast<double>(read_navigation_signed(data_bits, CNAV_TOT));
utc_model_record.d_t_OT = utc_model_record.d_t_OT * CNAV_TOT_LSB;
utc_model_record.i_WN_T = static_cast<double>(read_navigation_signed(data_bits, CNAV_WN_OT));
utc_model_record.i_WN_T = utc_model_record.i_WN_T * CNAV_WN_OT_LSB;
utc_model_record.i_WN_LSF = static_cast<double>(read_navigation_signed(data_bits, CNAV_WN_LSF));
utc_model_record.i_WN_LSF = utc_model_record.i_WN_LSF * CNAV_WN_LSF_LSB;
utc_model_record.i_DN = static_cast<double>(read_navigation_signed(data_bits, CNAV_DN));
utc_model_record.i_DN = utc_model_record.i_DN * CNAV_DN_LSB;
utc_model_record.d_DeltaT_LSF = static_cast<double>(read_navigation_signed(data_bits, CNAV_DELTA_TLSF));
utc_model_record.d_DeltaT_LSF = utc_model_record.d_DeltaT_LSF * CNAV_DELTA_TLSF_LSB;
b_flag_utc_valid = true;
break;
default: default:
break; break;
} }
@ -319,7 +336,7 @@ bool Gps_CNAV_Navigation_Message::have_new_ephemeris() //Check if we have a new
{ {
if (b_flag_ephemeris_1 == true and b_flag_ephemeris_2 == true) if (b_flag_ephemeris_1 == true and b_flag_ephemeris_2 == true)
{ {
if (ephemeris_record.d_Toe1 == ephemeris_record.d_Toe2) if (ephemeris_record.d_Toe1 == ephemeris_record.d_Toe2)// and ephemeris_record.d_Toe1==ephemeris_record.d_Toc)
{ {
//if all ephemeris pages have the same TOE, then they belong to the same block //if all ephemeris pages have the same TOE, then they belong to the same block
// std::cout << "Ephemeris (1, 2) have been received and belong to the same batch" << std::endl; // std::cout << "Ephemeris (1, 2) have been received and belong to the same batch" << std::endl;
@ -365,6 +382,19 @@ Gps_CNAV_Iono Gps_CNAV_Navigation_Message::get_iono()
} }
bool Gps_CNAV_Navigation_Message::have_new_utc_model() //Check if we have a new iono data stored in the galileo navigation class
{
if (b_flag_utc_valid == true)
{
b_flag_utc_valid = false;// clear the flag
return true;
}
else
{
return false;
}
}
Gps_CNAV_Utc_Model Gps_CNAV_Navigation_Message::get_utc_model() Gps_CNAV_Utc_Model Gps_CNAV_Navigation_Message::get_utc_model()
{ {
return utc_model_record; return utc_model_record;

15
src/core/system_parameters/gps_cnav_navigation_message.h
View File

@ -58,7 +58,6 @@ private:
unsigned long int read_navigation_unsigned(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter); unsigned long int read_navigation_unsigned(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter);
signed long int read_navigation_signed(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter); signed long int read_navigation_signed(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter);
bool read_navigation_bool(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter); bool read_navigation_bool(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int,int>> parameter);
void print_gps_word_bytes(unsigned int GPS_word);
Gps_CNAV_Ephemeris ephemeris_record; Gps_CNAV_Ephemeris ephemeris_record;
Gps_CNAV_Iono iono_record; Gps_CNAV_Iono iono_record;
@ -68,7 +67,8 @@ public:
double d_TOW; double d_TOW;
bool b_flag_ephemeris_1; bool b_flag_ephemeris_1;
bool b_flag_ephemeris_2; bool b_flag_ephemeris_2;
bool b_flag_iono_valid; //!< If set, it indicates that the ionospheric parameters are filled (page 18 has arrived and decoded) bool b_flag_iono_valid; //!< If set, it indicates that the ionospheric parameters are filled and are not yet readed by the get_iono
bool b_flag_utc_valid; //!< If set, it indicates that the utc parameters are filled and are not yet readed by the get_utc_model
std::map<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus std::map<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus
@ -89,13 +89,13 @@ public:
// public functions // public functions
void reset(); void reset();
void decode_page(std::vector<int> data); void decode_page(std::bitset<GPS_L2_CNAV_DATA_PAGE_BITS> data_bits);
/*! /*!
* \brief Obtain a GPS SV Ephemeris class filled with current SV data * \brief Obtain a GPS SV Ephemeris class filled with current SV data
*/ */
Gps_CNAV_Ephemeris get_ephemeris(); Gps_CNAV_Ephemeris get_ephemeris();
/*! /*!
* \brief Check if we have a new iono record stored in the galileo navigation class * \brief Check if we have a new iono record stored in the GPS ephemeris class
*/ */
bool have_new_iono(); bool have_new_iono();
/*! /*!
@ -109,7 +109,12 @@ public:
Gps_CNAV_Utc_Model get_utc_model(); Gps_CNAV_Utc_Model get_utc_model();
/*! /*!
* \brief Check if we have a new ephemeris stored in the galileo navigation class * \briefCheck if we have a new GPS UTC model record stored in the GPS ephemeris class
*/
bool have_new_utc_model();
/*!
* \brief Check if we have a new ephemeris stored in the GPS ephemeris class
*/ */
bool have_new_ephemeris(); bool have_new_ephemeris();

75
src/core/system_parameters/gps_cnav_utc_model.cc
View File

@ -29,11 +29,12 @@
*/ */
#include "gps_cnav_utc_model.h" #include "gps_cnav_utc_model.h"
#include <cmath>
Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model() Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model()
{ {
valid = false; valid = false;
d_A2 = 0;
d_A1 = 0; d_A1 = 0;
d_A0 = 0; d_A0 = 0;
d_t_OT = 0; d_t_OT = 0;
@ -43,3 +44,75 @@ Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model()
i_DN = 0; i_DN = 0;
d_DeltaT_LSF = 0; d_DeltaT_LSF = 0;
} }
double Gps_CNAV_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
{
double t_utc;
double t_utc_daytime;
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<double>(i_GPS_week - i_WN_T));
//std::cout<<"d_DeltaT_LS="<<d_DeltaT_LS
// <<"d_A0="<<d_A0
// <<"d_A1="<<d_A1
// <<"d_t_OT="<<d_t_OT
// <<"i_WN_T="<<i_WN_T<<std::endl;
// Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week;
if (weeksToLeapSecondEvent >= 0) // is not in the past
{
//Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
int secondOfLeapSecondEvent = i_DN * 24 * 60 * 60;
if (weeksToLeapSecondEvent > 0)
{
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
else //we are in the same week than the leap second event
{
if (std::abs(gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{
/* 20.3.3.5.2.4a
* Whenever the effectivity time indicated by the WN_LSF and the DN values
* is not in the past (relative to the user's present time), and the user's
* present time does not fall in the time span which starts at six hours prior
* to the effectivity time and ends at six hours after the effectivity time,
* the UTC/GPS-time relationship is given by
*/
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
else
{
/* 20.3.3.5.2.4b
* Whenever the user's current time falls within the time span of six hours
* prior to the effectivity time to six hours after the effectivity time,
* proper accommodation of the leap second event with a possible week number
* transition is provided by the following expression for UTC:
*/
int W = fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400) + 43200;
t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS);
//implement something to handle a leap second event!
}
if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<double>(i_GPS_week - i_WN_T));
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
}
}
else // the effectivity time is in the past
{
/* 20.3.3.5.2.4c
* Whenever the effectivity time of the leap second event, as indicated by the
* WNLSF and DN values, is in the "past" (relative to the user's current time),
* and the user<EFBFBD>s current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/
/* FOR CNAV: Replace the 20.3.3.5.2.4c with 30.3.3.6.2 UTC and GPS Time as follows */
double tmp_d= (gpstime_corrected - d_t_OT + 604800 * static_cast<double>(i_GPS_week - i_WN_T));
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * tmp_d + d_A2*tmp_d*tmp_d;
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
}
double secondsOfWeekBeforeToday = 86400 * floor(gpstime_corrected / 86400);
t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc;
}

9
src/core/system_parameters/gps_cnav_utc_model.h
View File

@ -34,7 +34,7 @@
#include "boost/assign.hpp" #include "boost/assign.hpp"
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <iostream>
/*! /*!
* \brief This class is a storage for the GPS UTC MODEL data as described in in IS-GPS-200H * \brief This class is a storage for the GPS UTC MODEL data as described in in IS-GPS-200H
@ -46,6 +46,7 @@ class Gps_CNAV_Utc_Model
public: public:
bool valid; bool valid;
// UTC parameters // UTC parameters
double d_A2; //!< 2nd order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s/s]
double d_A1; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s/s] double d_A1; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s/s]
double d_A0; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s] double d_A0; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200H) [s]
double d_t_OT; //!< Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200H) [s] double d_t_OT; //!< Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200H) [s]
@ -60,6 +61,12 @@ public:
*/ */
Gps_CNAV_Utc_Model(); Gps_CNAV_Utc_Model();
/*!
* \brief Computes the Coordinated Universal Time (UTC) and
* returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4 + 30.3.3.6.2)
*/
double utc_time(double gpstime_corrected, int i_GPS_week);
template<class Archive> template<class Archive>
/* /*
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file. * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.

6
src/core/system_parameters/gps_utc_model.cc
View File

@ -29,6 +29,7 @@
*/ */
#include "gps_utc_model.h" #include "gps_utc_model.h"
#include <iostream>
#include <cmath> #include <cmath>
Gps_Utc_Model::Gps_Utc_Model() Gps_Utc_Model::Gps_Utc_Model()
@ -50,6 +51,11 @@ double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
double t_utc_daytime; double t_utc_daytime;
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<double>(i_GPS_week - i_WN_T)); double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<double>(i_GPS_week - i_WN_T));
//std::cout<<"d_DeltaT_LS="<<d_DeltaT_LS
// <<"d_A0="<<d_A0
// <<"d_A1="<<d_A1
// <<"d_t_OT="<<d_t_OT
// <<"i_WN_T="<<i_WN_T<<std::endl;
// Determine if the effectivity time of the leap second event is in the past // Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week;