mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-09-16 17:29:48 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Gastd
commit
dc5c6937f3
@ -34,6 +34,9 @@
|
||||
#include <map>
|
||||
#include <boost/date_time/posix_time/posix_time.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/io_signature.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));
|
||||
// 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;
|
||||
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>) )
|
||||
{
|
||||
@ -323,6 +326,76 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump
|
||||
hybrid_pvt_cc::~hybrid_pvt_cc()
|
||||
{
|
||||
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)
|
||||
{
|
||||
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);
|
||||
|
||||
if (pvt_result == true)
|
||||
|
||||
@ -86,6 +86,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
||||
int GPS_week = 0;
|
||||
double utc = 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 TX_time_corrected_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;
|
||||
|
||||
// SV ECEF DEBUG OUTPUT
|
||||
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << gps_ephemeris_iter->second.d_satpos_X
|
||||
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " 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] Z=" << gps_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [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++;
|
||||
// compute the UTC time for this SV (just to print the associated UTC timestamp)
|
||||
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
|
||||
{
|
||||
@ -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
|
||||
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
|
||||
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
|
||||
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_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
|
||||
|
||||
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
|
||||
DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " X=" << gps_cnav_ephemeris_iter->second.d_satpos_X
|
||||
LOG(INFO) << "(new)ECEF GPS L2M satellite SV ID=" << gps_cnav_ephemeris_iter->second.i_satellite_PRN
|
||||
<< " 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] Z=" << gps_cnav_ephemeris_iter->second.d_satpos_Z
|
||||
<< " [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) << "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
|
||||
if( GST != 0.0)
|
||||
{
|
||||
|
||||
@ -49,7 +49,7 @@ message(STATUS "Build type set to ${CMAKE_BUILD_TYPE}.")
|
||||
|
||||
set(VERSION_INFO_MAJOR_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
|
||||
|
||||
|
||||
|
||||
@ -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:
|
||||
|
||||
* C. Ferná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á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.
|
||||
|
||||
|
||||
@ -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']))
|
||||
|
||||
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 name in filenames:
|
||||
@ -111,8 +114,14 @@ class volk_gnsssdr_modtool:
|
||||
infile = os.path.join(root, name);
|
||||
instring = open(infile, 'r').read();
|
||||
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);
|
||||
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);
|
||||
dest = os.path.join(self.my_dict['destination'], 'volk_gnsssdr_' + self.my_dict['name'], os.path.dirname(newrelpath), newname);
|
||||
|
||||
|
||||
@ -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;
|
||||
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));
|
||||
tmp_double = current_gnss_synchro[i].Pseudorange_m;
|
||||
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;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
|
||||
@ -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;
|
||||
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/GPS_TWO_PI;
|
||||
|
||||
@ -264,16 +264,16 @@ int hybrid_observables_cc::general_work (int noutput_items,
|
||||
{
|
||||
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
|
||||
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));
|
||||
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));
|
||||
tmp_double = current_gnss_synchro[i].Pseudorange_m;
|
||||
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;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
|
||||
}
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
|
||||
@ -31,6 +31,7 @@ include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/core/receiver
|
||||
${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/libswiftcnav
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
|
||||
@ -29,6 +29,7 @@ include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/core/system_parameters
|
||||
${CMAKE_SOURCE_DIR}/src/core/receiver
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs/libswiftcnav
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
@ -39,4 +40,4 @@ file(GLOB TELEMETRY_DECODER_GR_BLOCKS_HEADERS "*.h")
|
||||
list(SORT 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})
|
||||
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})
|
||||
@ -29,6 +29,7 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include <bitset>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <gnuradio/io_signature.h>
|
||||
@ -39,14 +40,6 @@
|
||||
|
||||
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_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(
|
||||
Gnss_Satellite satellite,
|
||||
bool dump) :
|
||||
gr::block("gps_l2c_telemetry_decoder_cc",
|
||||
Gnss_Satellite satellite, 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)))
|
||||
{
|
||||
@ -69,18 +60,19 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc(
|
||||
// initialize internal vars
|
||||
d_dump = dump;
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
LOG(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
|
||||
DLOG(INFO) << "GPS L2C M TELEMETRY PROCESSING: satellite " << d_satellite;
|
||||
d_decimation_output_factor = 0;
|
||||
//set_output_multiple (1);
|
||||
d_average_count = 0;
|
||||
d_flag_invert_buffer_symbols = false;
|
||||
d_flag_invert_input_symbols = false;
|
||||
d_channel = 0;
|
||||
d_flag_valid_word = false;
|
||||
d_TOW_at_current_symbol = 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)
|
||||
{
|
||||
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
|
||||
|
||||
bool flag_new_cnav_frame = false;
|
||||
int last_frame_preamble_start = 0;
|
||||
// copy correlation samples into samples vector
|
||||
d_sample_buf.push_back(in[0].Prompt_I);
|
||||
cnav_msg_t msg;
|
||||
u32 delay = 0;
|
||||
|
||||
//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
|
||||
|
||||
// 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
|
||||
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
|
||||
current_synchro_data = in[0];
|
||||
|
||||
if (d_flag_valid_word == true)
|
||||
{
|
||||
double Prn_timestamp_at_preamble_ms = 0;
|
||||
//2. Add the telemetry decoder information
|
||||
if (flag_new_cnav_frame == true)
|
||||
{
|
||||
//update TOW at the preamble instant
|
||||
Prn_timestamp_at_preamble_ms = (in[0].Tracking_timestamp_secs * 1000.0) - (d_block_size - last_frame_preamble_start) * GPS_L2_M_PERIOD;
|
||||
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;
|
||||
//2. Add the telemetry decoder information
|
||||
//check if new CNAV frame is available
|
||||
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
|
||||
for (u32 i = 0; i < GPS_L2_CNAV_DATA_PAGE_BITS ; i++) {
|
||||
raw_bits[GPS_L2_CNAV_DATA_PAGE_BITS-1-i]=((msg.raw_msg[i/8] >> (7 - i%8)) & 1u);
|
||||
}
|
||||
|
||||
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
|
||||
{
|
||||
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++;
|
||||
if (d_average_count == d_decimation_output_factor)
|
||||
{
|
||||
d_average_count = 0;
|
||||
//3. Make the output (copy the object contents to the GNURadio reserved memory)
|
||||
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;
|
||||
}
|
||||
{
|
||||
d_average_count = 0;
|
||||
//3. Make the output (copy the object contents to the GNURadio reserved memory)
|
||||
out[0] = current_synchro_data;
|
||||
return 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -288,223 +230,25 @@ void gps_l2c_telemetry_decoder_cc::set_channel(int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
LOG(INFO) << "GPS L2C CNAV channel set to " << channel;
|
||||
}
|
||||
|
||||
|
||||
// ### 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)
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump == true)
|
||||
{
|
||||
symbols_vd2.push_back(*symbol_it);
|
||||
}
|
||||
// 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)
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
bool preamble_detected = true;
|
||||
bool inv_preamble_detected = true;
|
||||
// compare the buffer bits with the preamble bits
|
||||
for (std::vector<int>::iterator preample_bit_it = preample_it->begin(); preample_bit_it < preample_it->end(); ++preample_bit_it)
|
||||
{
|
||||
preamble_detected = *preample_bit_it == d_buffer[preample_bit_it - preample_it->begin()] ? preamble_detected : false ;
|
||||
inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it - preample_it->begin()] ? inv_preamble_detected : false ;
|
||||
}
|
||||
if (preamble_detected || inv_preamble_detected)
|
||||
{
|
||||
// copy candidate
|
||||
std::vector<int> candidate;
|
||||
std::copy(d_buffer.begin(), d_buffer.begin() + cnav_msg_length, std::back_inserter(candidate));
|
||||
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;
|
||||
try
|
||||
{
|
||||
d_dump_filename = "telemetry_L2CM_";
|
||||
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
||||
d_dump_filename.append(".dat");
|
||||
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
|
||||
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
|
||||
<< " Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
@ -37,14 +37,19 @@
|
||||
#include <string>
|
||||
#include <utility> // for pair
|
||||
#include <vector>
|
||||
#include <boost/crc.hpp>
|
||||
#include <gnuradio/block.h>
|
||||
#include "gnss_satellite.h"
|
||||
#include "viterbi_decoder.h"
|
||||
#include "gps_cnav_navigation_message.h"
|
||||
#include "gps_cnav_ephemeris.h"
|
||||
#include "gps_cnav_iono.h"
|
||||
#include "concurrent_queue.h"
|
||||
|
||||
extern "C" {
|
||||
#include "cnav_msg.h"
|
||||
#include "edc.h"
|
||||
#include "bits.h"
|
||||
}
|
||||
|
||||
#include "GPS_L2C.h"
|
||||
|
||||
class gps_l2c_telemetry_decoder_cc;
|
||||
@ -72,19 +77,13 @@ public:
|
||||
int general_work (int noutput_items, gr_vector_int &ninput_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:
|
||||
friend gps_l2c_telemetry_decoder_cc_sptr
|
||||
gps_l2c_make_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;
|
||||
Gnss_Satellite d_satellite;
|
||||
@ -93,59 +92,17 @@ private:
|
||||
std::string d_dump_filename;
|
||||
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_Preamble;
|
||||
bool d_flag_valid_word;
|
||||
|
||||
bool d_flag_invert_input_symbols;
|
||||
bool d_flag_invert_buffer_symbols;
|
||||
int d_decimation_output_factor;
|
||||
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;
|
||||
};
|
||||
|
||||
@ -16,9 +16,11 @@
|
||||
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
add_subdirectory(libswiftcnav)
|
||||
|
||||
set(TELEMETRY_DECODER_LIB_SOURCES
|
||||
gps_l1_ca_subframe_fsm.cc
|
||||
viterbi_decoder.cc
|
||||
viterbi_decoder.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
@ -33,7 +35,8 @@ include_directories(
|
||||
)
|
||||
|
||||
file(GLOB TELEMETRY_DECODER_LIB_HEADERS "*.h")
|
||||
|
||||
list(SORT 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})
|
||||
target_link_libraries(telemetry_decoder_lib gnss_system_parameters)
|
||||
target_link_libraries(telemetry_decoder_lib gnss_system_parameters)
|
||||
|
||||
@ -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
301
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.c
Normal 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
50
src/algorithms/telemetry_decoder/libs/libswiftcnav/bits.h
Normal 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
491
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c
Normal 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
121
src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h
Normal 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
146
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.c
Normal 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
41
src/algorithms/telemetry_decoder/libs/libswiftcnav/edc.h
Normal 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
73
src/algorithms/telemetry_decoder/libs/libswiftcnav/fec.h
Normal 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
|
||||
@ -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
252
src/algorithms/telemetry_decoder/libs/libswiftcnav/viterbi27.c
Normal 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);
|
||||
}
|
||||
@ -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());
|
||||
// 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);
|
||||
// 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_Doppler_hz = d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
|
||||
|
||||
@ -65,6 +65,7 @@ include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/adapters
|
||||
${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/libswiftcnav
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/observables/adapters
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/observables/gnuradio_blocks
|
||||
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters
|
||||
@ -141,6 +142,8 @@ target_link_libraries(gnss_rx ${Boost_LIBRARIES}
|
||||
tracking_lib
|
||||
tracking_adapters
|
||||
channel_adapters
|
||||
telemetry_decoder_libswiftcnav
|
||||
telemetry_decoder_lib
|
||||
telemetry_decoder_adapters
|
||||
obs_adapters
|
||||
pvt_adapters
|
||||
|
||||
@ -95,12 +95,14 @@ const int32_t GPS_L2C_M_INIT_REG[115] =
|
||||
// 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_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_SYMBOLS_PER_BIT = 2;
|
||||
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_BITS_EXTENDED_BYTES = 304; //!< GPS L2 CNAV page length, including preamble and CRC [bits]
|
||||
|
||||
// common to all messages
|
||||
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 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 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 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 double CNAV_E_ECCENTRICITY_LSB = TWO_N34;
|
||||
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_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 double CNAV_TOE2_LSB = 300.0;
|
||||
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 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 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 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 double CNAV_CIS_LSB = TWO_N30;
|
||||
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}});
|
||||
|
||||
|
||||
// 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...)
|
||||
|
||||
|
||||
|
||||
@ -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_N59 = (1.73472347597681e-018); //!< 2^-59
|
||||
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
|
||||
|
||||
@ -119,7 +119,7 @@ public:
|
||||
void serialize(Archive& archive, const unsigned int version)
|
||||
{
|
||||
using boost::serialization::make_nvp;
|
||||
|
||||
if(version){};
|
||||
archive & make_nvp("i_satellite_PRN",i_satellite_PRN);
|
||||
archive & make_nvp("M0_1", M0_1);
|
||||
archive & make_nvp("e_1", e_1);
|
||||
|
||||
@ -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 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_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);
|
||||
|
||||
// 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;
|
||||
|
||||
}
|
||||
|
||||
@ -153,7 +153,8 @@ public:
|
||||
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("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_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
|
||||
* 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
|
||||
|
||||
@ -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)
|
||||
{
|
||||
@ -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 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));
|
||||
|
||||
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)
|
||||
{
|
||||
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_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_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_OMEGA = static_cast<double>(read_navigation_signed(data_bits, CNAV_OMEGA));
|
||||
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;
|
||||
b_flag_iono_valid = true;
|
||||
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:
|
||||
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 (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
|
||||
// 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()
|
||||
{
|
||||
return utc_model_record;
|
||||
|
||||
@ -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);
|
||||
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);
|
||||
void print_gps_word_bytes(unsigned int GPS_word);
|
||||
|
||||
Gps_CNAV_Ephemeris ephemeris_record;
|
||||
Gps_CNAV_Iono iono_record;
|
||||
@ -68,7 +67,8 @@ public:
|
||||
double d_TOW;
|
||||
bool b_flag_ephemeris_1;
|
||||
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
|
||||
|
||||
@ -89,13 +89,13 @@ public:
|
||||
// public functions
|
||||
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
|
||||
*/
|
||||
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();
|
||||
/*!
|
||||
@ -109,7 +109,12 @@ public:
|
||||
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();
|
||||
|
||||
|
||||
@ -29,11 +29,12 @@
|
||||
*/
|
||||
|
||||
#include "gps_cnav_utc_model.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model()
|
||||
{
|
||||
valid = false;
|
||||
d_A2 = 0;
|
||||
d_A1 = 0;
|
||||
d_A0 = 0;
|
||||
d_t_OT = 0;
|
||||
@ -43,3 +44,75 @@ Gps_CNAV_Utc_Model::Gps_CNAV_Utc_Model()
|
||||
i_DN = 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;
|
||||
}
|
||||
|
||||
@ -34,7 +34,7 @@
|
||||
|
||||
#include "boost/assign.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
|
||||
@ -46,6 +46,7 @@ class Gps_CNAV_Utc_Model
|
||||
public:
|
||||
bool valid;
|
||||
// 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_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]
|
||||
@ -60,6 +61,12 @@ public:
|
||||
*/
|
||||
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>
|
||||
/*
|
||||
* \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.
|
||||
|
||||
@ -29,6 +29,7 @@
|
||||
*/
|
||||
|
||||
#include "gps_utc_model.h"
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
|
||||
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 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;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user