[hamradio-commits] [gnss-sdr] 70/126: Merging branch 'next' of git://github.com/Arribas/gnss-sdr
Carles Fernandez
carles_fernandez-guest at moszumanska.debian.org
Sat Dec 26 18:38:03 UTC 2015
This is an automated email from the git hooks/post-receive script.
carles_fernandez-guest pushed a commit to branch next
in repository gnss-sdr.
commit 2a6381c6fa4f835833cb026b51266999bca0f26c
Merge: 513e92d c8f7e08
Author: Carles Fernandez <carles.fernandez at gmail.com>
Date: Fri Nov 27 13:32:21 2015 +0100
Merging branch 'next' of git://github.com/Arribas/gnss-sdr
README.md | 24 +
conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf | 4 +-
...plex_gpu.conf => gnss-sdr_Hybrid_byte_sim.conf} | 197 +++++---
.../observables/gnuradio_blocks/CMakeLists.txt | 5 +-
.../gnuradio_blocks/gps_l1_ca_observables_cc.cc | 114 ++++-
.../gnuradio_blocks/gps_l1_ca_observables_cc.h | 9 +
.../gps_l1_ca_telemetry_decoder_cc.cc | 20 +-
.../gps_l1_ca_telemetry_decoder_cc.h | 7 +
src/algorithms/tracking/adapters/CMakeLists.txt | 1 +
.../gps_l1_ca_dll_pll_artemisa_tracking.cc | 159 +++++++
.../adapters/gps_l1_ca_dll_pll_artemisa_tracking.h | 114 +++++
.../galileo_e1_dll_pll_veml_tracking_cc.cc | 58 ++-
.../galileo_e1_dll_pll_veml_tracking_cc.h | 18 +-
.../galileo_e1_tcp_connector_tracking_cc.cc | 2 +-
.../galileo_e5a_dll_pll_tracking_cc.cc | 121 ++---
.../galileo_e5a_dll_pll_tracking_cc.h | 34 +-
.../galileo_volk_e1_dll_pll_veml_tracking_cc.cc | 61 ++-
.../galileo_volk_e1_dll_pll_veml_tracking_cc.h | 18 +-
.../gps_l1_ca_dll_fll_pll_tracking_cc.cc | 5 +-
...c => gps_l1_ca_dll_pll_artemisa_tracking_cc.cc} | 355 +++++++-------
....h => gps_l1_ca_dll_pll_artemisa_tracking_cc.h} | 78 ++-
.../gps_l1_ca_dll_pll_optim_tracking_cc.cc | 63 +--
.../gps_l1_ca_dll_pll_optim_tracking_cc.h | 20 +-
.../gps_l1_ca_dll_pll_tracking_cc.cc | 114 ++---
.../gps_l1_ca_dll_pll_tracking_cc.h | 20 +-
.../gps_l1_ca_dll_pll_tracking_gpu_cc.cc | 66 +--
.../gps_l1_ca_dll_pll_tracking_gpu_cc.h | 4 -
.../gps_l2_m_dll_pll_tracking_cc.cc | 70 +--
.../gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.h | 20 +-
src/algorithms/tracking/libs/CMakeLists.txt | 1 +
.../tracking/libs/cpu_multicorrelator.cc | 163 +++++++
src/algorithms/tracking/libs/cpu_multicorrelator.h | 98 ++++
.../tracking/libs/cuda_multicorrelator.cu | 523 +++++++--------------
.../tracking/libs/cuda_multicorrelator.h | 32 +-
.../tracking/libs/tracking_discriminators.cc | 18 +-
.../tracking/libs/tracking_discriminators.h | 10 +-
src/core/system_parameters/GPS_L1_CA.h | 4 +
src/core/system_parameters/MATH_CONSTANTS.h | 2 +-
src/core/system_parameters/gnss_synchro.h | 5 +
src/core/system_parameters/rtcm.cc | 373 ++++++++++++++-
src/core/system_parameters/rtcm.h | 61 ++-
41 files changed, 1987 insertions(+), 1084 deletions(-)
diff --cc README.md
index 1dc8c86,ff91d7b..00afe01
--- a/README.md
+++ b/README.md
@@@ -717,50 -674,6 +717,74 @@@ SignalSource.dump1=fals
~~~~~~
+***Example: OsmoSDR-compatible Signal Source***
+
+[OsmoSDR](http://sdr.osmocom.org/trac) is a small form-factor, inexpensive software defined radio project. It provides a driver for several front-ends, such as [RTL-based dongles](http://sdr.osmocom.org/trac/wiki/rtl-sdr), HackRF, bladeRF, etc. Note that not all the OsmoSDR-compatible devices can work as radio frequency front-ends for proper GNSS signal reception, please check the specifications. For suitable RF front-ends, you can use:
+
+~~~~~~
+;######### SIGNAL_SOURCE CONFIG ############
+SignalSource.implementation=Osmosdr_Signal_Source
+SignalSource.item_type=gr_complex
+SignalSource.sampling_frequency=2000000
+SignalSource.freq=1575420000
+SignalSource.rf_gain=40
+SignalSource.if_gain=30
+SignalSource.enable_throttle_control=false
+SignalSource.osmosdr_args=rtl_tcp,offset_tune=1
+~~~~~~
+
+In case of using a Zarlink's RTL2832 based DVB-T receiver, you can even use the ```rtl_tcp``` I/Q server in order to use the USB dongle remotely. In a terminal, type:
+
+~~~~~~
+$ rtl_tcp -a 127.0.0.1 -p 1234 -f 1575420000 -g 0 -s 2000000
+~~~~~~
+
+and use the following configuration:
+
+~~~~~~
+;######### SIGNAL_SOURCE CONFIG ############
+SignalSource.implementation=RtlTcp_Signal_Source
+SignalSource.item_type=gr_complex
+SignalSource.sampling_frequency=1200000
+SignalSource.freq=1575420000
+SignalSource.gain=40
+SignalSource.rf_gain=40
+SignalSource.if_gain=30
+SignalSource.AGC_enabled=false
+SignalSource.samples=0
+SignalSource.enable_throttle_control=false
+SignalSource.address=127.0.0.1
+SignalSource.port=1234
+SignalSource.swap_iq=false
+SignalSource.repeat=false
+SignalSource.dump=false
+SignalSource.dump_filename=../data/signal_source.dat
+~~~~~~
+
++Example for a dual-frequency receiver:
++
++~~~~~~
++;######### SIGNAL_SOURCE CONFIG ############
++SignalSource.implementation=UHD_Signal_Source
++SignalSource.device_address=192.168.40.2 ; Put your USRP IP address here
++SignalSource.item_type=gr_complex
++SignalSource.RF_channels=2
++SignalSource.sampling_frequency=4000000
++SignalSource.subdevice=A:0 B:0
++
++;######### RF Channels specific settings ######
++SignalSource.freq0=1575420000
++SignalSource.gain0=50
++SignalSource.samples0=0
++SignalSource.dump0=false
++
++SignalSource.freq1=1227600000
++SignalSource.gain1=50
++SignalSource.samples1=0
++SignalSource.dump1=false
++~~~~~~
++
++
Other examples are available at [gnss-sdr/conf/](./conf/).
### Signal Conditioner
diff --cc src/algorithms/observables/gnuradio_blocks/CMakeLists.txt
index 27c266f,27c266f..6f4b25f
--- a/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt
+++ b/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt
@@@ -31,6 -31,6 +31,7 @@@ include_directories
${CMAKE_SOURCE_DIR}/src/algorithms/libs
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs
${GNURADIO_RUNTIME_INCLUDE_DIRS}
++ ${ARMADILLO_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
)
@@@ -38,5 -38,5 +39,5 @@@
file(GLOB OBS_GR_BLOCKS_HEADERS "*.h")
add_library(obs_gr_blocks ${OBS_GR_BLOCKS_SOURCES} ${OBS_GR_BLOCKS_HEADERS})
source_group(Headers FILES ${OBS_GR_BLOCKS_HEADERS})
--add_dependencies(obs_gr_blocks glog-${glog_RELEASE})
--target_link_libraries(obs_gr_blocks ${GNURADIO_RUNTIME_LIBRARIES})
++add_dependencies(obs_gr_blocks glog-${glog_RELEASE} armadillo-${armadillo_RELEASE})
++target_link_libraries(obs_gr_blocks ${GNURADIO_RUNTIME_LIBRARIES} ${ARMADILLO_LIBRARIES})
diff --cc src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
index 8df2646,6c52dcf..4e86773
--- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
+++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
@@@ -35,6 -35,6 +35,7 @@@
#include <map>
#include <sstream>
#include <vector>
++#include <armadillo>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include "control_message_factory.h"
@@@ -63,6 -63,13 +64,13 @@@ gps_l1_ca_observables_cc::gps_l1_ca_obs
d_dump_filename = dump_filename;
d_flag_averaging = flag_averaging;
- for (int i=0;i<d_nchannels;i++)
- {
- d_acc_carrier_phase_queue_rads.push_back(std::deque<double>(d_nchannels));
- d_carrier_doppler_queue_hz.push_back(std::deque<double>(d_nchannels));
- d_symbol_TOW_queue_s.push_back(std::deque<double>(d_nchannels));
- }
++ for (int i = 0; i < d_nchannels; i++)
++ {
++ d_acc_carrier_phase_queue_rads.push_back(std::deque<double>(d_nchannels));
++ d_carrier_doppler_queue_hz.push_back(std::deque<double>(d_nchannels));
++ d_symbol_TOW_queue_s.push_back(std::deque<double>(d_nchannels));
++ }
+
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
@@@ -128,6 -135,33 +136,35 @@@ int gps_l1_ca_observables_cc::general_w
{
//record the word structure in a map for pseudorange computation
current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
+
+ //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE #######
+ d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz);
+ d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads);
+ // save TOW history
+ d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol);
+
- if (d_carrier_doppler_queue_hz[i].size()>GPS_L1_CA_HISTORY_DEEP)
- {
- d_carrier_doppler_queue_hz[i].pop_front();
- }
- if (d_acc_carrier_phase_queue_rads[i].size()>GPS_L1_CA_HISTORY_DEEP)
- {
- d_acc_carrier_phase_queue_rads[i].pop_front();
- }
- if (d_symbol_TOW_queue_s[i].size()>GPS_L1_CA_HISTORY_DEEP)
- {
- d_symbol_TOW_queue_s[i].pop_front();
- }
- }else{
- // Clear the observables history for this channel
- if (d_symbol_TOW_queue_s[i].size()>0)
- {
- d_symbol_TOW_queue_s[i].clear();
- d_carrier_doppler_queue_hz[i].clear();
- d_acc_carrier_phase_queue_rads[i].clear();
- }
++ if (d_carrier_doppler_queue_hz[i].size() > GPS_L1_CA_HISTORY_DEEP)
++ {
++ d_carrier_doppler_queue_hz[i].pop_front();
++ }
++ if (d_acc_carrier_phase_queue_rads[i].size() > GPS_L1_CA_HISTORY_DEEP)
++ {
++ d_acc_carrier_phase_queue_rads[i].pop_front();
++ }
++ if (d_symbol_TOW_queue_s[i].size() > GPS_L1_CA_HISTORY_DEEP)
++ {
++ d_symbol_TOW_queue_s[i].pop_front();
++ }
++ }
++ else
++ {
++ // Clear the observables history for this channel
++ if (d_symbol_TOW_queue_s[i].size() > 0)
++ {
++ d_symbol_TOW_queue_s[i].clear();
++ d_carrier_doppler_queue_hz[i].clear();
++ d_acc_carrier_phase_queue_rads[i].clear();
++ }
}
}
@@@ -150,19 -184,63 +187,63 @@@
double traveltime_ms;
double pseudorange_m;
double delta_rx_time_ms;
+ arma::vec symbol_TOW_vec_s;
+ arma::vec dopper_vec_hz;
+ arma::vec dopper_vec_interp_hz;
+ arma::vec acc_phase_vec_rads;
+ arma::vec acc_phase_vec_interp_rads;
+ arma::vec desired_symbol_TOW(1);
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
-- {
-- // compute the required symbol history shift in order to match the reference symbol
-- delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms;
-- //compute the pseudorange
-- traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms;
-- pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
-- // update the pseudorange object
-- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].debug_var1=delta_rx_time_ms;
-- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GPS_STARTOFFSET_ms/1000.0;
- }
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GPS_STARTOFFSET_ms/1000.0;
-
- if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size()>=GPS_L1_CA_HISTORY_DEEP)
+ {
- // compute interpolated observation values for Doppler and Accumulate carrier phase
- symbol_TOW_vec_s=arma::vec(std::vector<double>(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end()));
- acc_phase_vec_rads=arma::vec(std::vector<double>(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end()));
- dopper_vec_hz=arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
-
- //std::cout<<"symbol_TOW_vec_s[0]="<<symbol_TOW_vec_s[0]<<std::endl;
- //std::cout<<"symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]="<<symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]<<std::endl;
- //std::cout<<"acc_phase_vec_rads="<<acc_phase_vec_rads<<std::endl;
- //std::cout<<"dopper_vec_hz="<<dopper_vec_hz<<std::endl;
-
- desired_symbol_TOW[0]=symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]+delta_rx_time_ms/1000.0;
- //std::cout<<"desired_symbol_TOW="<<desired_symbol_TOW[0]<<std::endl;
-
-// arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz);
-// arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads);
-
- // Curve fitting to cuadratic function
- arma::mat A=arma::ones<arma::mat> (GPS_L1_CA_HISTORY_DEEP,2);
- A.col(1)=symbol_TOW_vec_s;
- //A.col(2)=symbol_TOW_vec_s % symbol_TOW_vec_s;
- arma::mat coef_acc_phase(1,3);
- coef_acc_phase=arma::pinv(A.t()*A)*A.t()*acc_phase_vec_rads;
- arma::mat coef_doppler(1,3);
- coef_doppler=arma::pinv(A.t()*A)*A.t()*dopper_vec_hz;
- arma::vec acc_phase_lin;
- arma::vec carrier_doppler_lin;
- acc_phase_lin=coef_acc_phase[0]+coef_acc_phase[1]*desired_symbol_TOW[0];//+coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
- carrier_doppler_lin=coef_doppler[0]+coef_doppler[1]*desired_symbol_TOW[0];//+coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
- //std::cout<<"acc_phase_vec_interp_rads="<<acc_phase_vec_interp_rads[0]<<std::endl;
- //std::cout<<"dopper_vec_interp_hz="<<dopper_vec_interp_hz[0]<<std::endl;
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads =acc_phase_lin[0];
- current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz =carrier_doppler_lin[0];
- }
++ // compute the required symbol history shift in order to match the reference symbol
++ delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms;
++ //compute the pseudorange
++ traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms;
++ pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
++ // update the pseudorange object
++ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second;
++ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].debug_var1 = delta_rx_time_ms;
++ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
++ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
++ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000.0)/1000.0 + GPS_STARTOFFSET_ms/1000.0;
++
++ if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= GPS_L1_CA_HISTORY_DEEP)
++ {
++ // compute interpolated observation values for Doppler and Accumulate carrier phase
++ symbol_TOW_vec_s = arma::vec(std::vector<double>(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end()));
++ acc_phase_vec_rads = arma::vec(std::vector<double>(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end()));
++ dopper_vec_hz = arma::vec(std::vector<double>(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end()));
++
++ //std::cout<<"symbol_TOW_vec_s[0]="<<symbol_TOW_vec_s[0]<<std::endl;
++ //std::cout<<"symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]="<<symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1]<<std::endl;
++ //std::cout<<"acc_phase_vec_rads="<<acc_phase_vec_rads<<std::endl;
++ //std::cout<<"dopper_vec_hz="<<dopper_vec_hz<<std::endl;
++
++ desired_symbol_TOW[0] = symbol_TOW_vec_s[GPS_L1_CA_HISTORY_DEEP-1] + delta_rx_time_ms / 1000.0;
++ //std::cout<<"desired_symbol_TOW="<<desired_symbol_TOW[0]<<std::endl;
++
++ // arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz);
++ // arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads);
++
++ // Curve fitting to cuadratic function
++ arma::mat A = arma::ones<arma::mat>(GPS_L1_CA_HISTORY_DEEP,2);
++ A.col(1) = symbol_TOW_vec_s;
++ //A.col(2)=symbol_TOW_vec_s % symbol_TOW_vec_s;
++ arma::mat coef_acc_phase(1,3);
++ coef_acc_phase = arma::pinv(A.t() * A) * A.t() * acc_phase_vec_rads;
++ arma::mat coef_doppler(1,3);
++ coef_doppler = arma::pinv(A.t() * A) * A.t() * dopper_vec_hz;
++ arma::vec acc_phase_lin;
++ arma::vec carrier_doppler_lin;
++ acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0];//+coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
++ carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0];//+coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0];
++ //std::cout<<"acc_phase_vec_interp_rads="<<acc_phase_vec_interp_rads[0]<<std::endl;
++ //std::cout<<"dopper_vec_interp_hz="<<dopper_vec_interp_hz[0]<<std::endl;
++ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0];
++ current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0];
++ }
+
- }
++ }
}
if(d_dump == true)
@@@ -179,7 -260,9 +263,9 @@@
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);
+ //tmp_double = (double)(current_gnss_synchro[i].Flag_valid_pseudorange==true);
+ //tmp_double = current_gnss_synchro[i].debug_var1;
- tmp_double= current_gnss_synchro[i].debug_var2;
++ tmp_double = current_gnss_synchro[i].debug_var2;
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));
diff --cc src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
index 7f7dacb,56cfd6d..63bcba1
--- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
+++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
@@@ -37,8 -39,10 +39,9 @@@
#include <utility>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
+ #include <boost/shared_ptr.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
-#include <armadillo>
#include "concurrent_queue.h"
#include "gps_navigation_message.h"
#include "rinex_printer.h"
diff --cc src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
index 260f6de,604379d..0df1c0a
--- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
+++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
@@@ -135,6 -135,7 +135,7 @@@ gps_l1_ca_telemetry_decoder_cc::gps_l1_
d_decimation_output_factor = 1;
d_channel = 0;
Prn_timestamp_at_preamble_ms = 0.0;
- flag_PLL_180_deg_phase_locked=false;
++ flag_PLL_180_deg_phase_locked = false;
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
}
@@@ -224,7 -225,14 +225,16 @@@ int gps_l1_ca_telemetry_decoder_cc::gen
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
- LOG(INFO) <<" Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
- if (corr_value<0)
++ if (corr_value < 0)
+ {
- flag_PLL_180_deg_phase_locked=true; //PLL is locked to opposite phase!
- std::cout<<"PLL in opposite phase for Sat "<<this->d_satellite.get_PRN()<<std::endl;
- }else{
- flag_PLL_180_deg_phase_locked=false;
++ flag_PLL_180_deg_phase_locked = true; //PLL is locked to opposite phase!
++ LOG(INFO) << " PLL in opposite phase for Sat "<< this->d_satellite.get_PRN();
+ }
- LOG(INFO) <<" Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
++ else
++ {
++ flag_PLL_180_deg_phase_locked = false;
++ }
++ LOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
}
}
}
@@@ -334,6 -341,11 +343,11 @@@
current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;
- if (flag_PLL_180_deg_phase_locked==true)
++ 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;
++ current_synchro_data.Carrier_phase_rads += GPS_PI;
+ }
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
diff --cc src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
index 0ea9258,4aa877d..934e00a
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
@@@ -310,10 -310,14 +310,11 @@@ galileo_e1_dll_pll_veml_tracking_cc::~g
int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
- float carr_error_hz;
- float carr_error_filt_hz;
- float code_error_chips;
- float code_error_filt_chips;
- double carr_error_hz;
- carr_error_hz=0.0;
- double carr_error_filt_hz;
- carr_error_filt_hz=0.0;
- double code_error_chips;
- code_error_chips=0.0;
- double code_error_filt_chips;
- code_error_filt_chips=0.0;
++ double carr_error_hz = 0.0;
++ double carr_error_filt_hz = 0.0;
++ double code_error_chips = 0.0;
++ double code_error_filt_chips = 0.0;
++
if (d_enable_tracking == true)
{
@@@ -547,19 -551,28 +548,28 @@@
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
- tmp_float=d_acc_carrier_phase_rad;
++ tmp_float = d_acc_carrier_phase_rad;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
- tmp_float=d_carrier_doppler_hz;
++ tmp_float = d_carrier_doppler_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=d_code_freq_chips;
++ tmp_float = d_code_freq_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
- tmp_float=carr_error_hz;
++ tmp_float = carr_error_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=carr_error_filt_hz;
++ tmp_float = carr_error_filt_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
- tmp_float=code_error_chips;
++ tmp_float = code_error_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=code_error_filt_chips;
++ tmp_float = code_error_filt_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
- tmp_float=d_CN0_SNV_dB_Hz;
++ tmp_float = d_CN0_SNV_dB_Hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=d_carrier_lock_test;
++ tmp_float = d_carrier_lock_test;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples;
d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
diff --cc src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
index 0a988a1,fccccfb..90d7f1b
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
@@@ -400,10 -400,10 +400,10 @@@ int Galileo_E5a_Dll_Pll_Tracking_cc::ge
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
- float carr_error_hz;
- float carr_error_filt_hz;
- float code_error_chips;
- float code_error_filt_chips;
- double carr_error_hz;
- double carr_error_filt_hz;
- double code_error_chips;
- double code_error_filt_chips;
++ double carr_error_hz;
++ double carr_error_filt_hz;
++ double code_error_chips;
++ double code_error_filt_chips;
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
diff --cc src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc
index d9ae8bd,820b998..c3c6596
--- a/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/galileo_volk_e1_dll_pll_veml_tracking_cc.cc
@@@ -594,19 -594,28 +594,28 @@@ int galileo_volk_e1_dll_pll_veml_tracki
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
- tmp_float=d_acc_carrier_phase_rad;
++ tmp_float = d_acc_carrier_phase_rad;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
- tmp_float=d_carrier_doppler_hz;
++ tmp_float = d_carrier_doppler_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=d_code_freq_chips;
++ tmp_float = d_code_freq_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
- tmp_float=carr_error_hz;
++ tmp_float = carr_error_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=carr_error_filt_hz;
++ tmp_float = carr_error_filt_hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
//DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
- tmp_float=code_error_chips;
++ tmp_float = code_error_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=code_error_filt_chips;
++ tmp_float = code_error_filt_chips;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
- tmp_float=d_CN0_SNV_dB_Hz;
++ tmp_float = d_CN0_SNV_dB_Hz;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
- tmp_float=d_carrier_lock_test;
++ tmp_float = d_carrier_lock_test;
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples;
d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
diff --cc src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
index 86886dc,019723f..bd5dcce
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
@@@ -563,23 -563,32 +563,32 @@@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc
//tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// accumulated carrier phase
- d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
- tmp_float=d_acc_carrier_phase_rad;
++ tmp_float = d_acc_carrier_phase_rad;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
// carrier and code frequency
- d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
- d_dump_file.write((char*)&d_code_freq_chips, sizeof(float));
- tmp_float=d_carrier_doppler_hz;
++ tmp_float = d_carrier_doppler_hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
- tmp_float=d_code_freq_chips;
++ tmp_float = d_code_freq_chips;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
//PLL commands
- d_dump_file.write((char*)&carr_error_hz, sizeof(float));
- d_dump_file.write((char*)&carr_error_filt_hz, sizeof(float));
- tmp_float=carr_error_hz;
++ tmp_float = carr_error_hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
- tmp_float=carr_error_filt_hz;
++ tmp_float = carr_error_filt_hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
//DLL commands
- d_dump_file.write((char*)&code_error_chips, sizeof(float));
- d_dump_file.write((char*)&code_error_filt_chips, sizeof(float));
- tmp_float=code_error_chips;
++ tmp_float = code_error_chips;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
- tmp_float=code_error_filt_chips;
++ tmp_float = code_error_filt_chips;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
// CN0 and carrier lock test
- d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
- d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
- tmp_float=d_CN0_SNV_dB_Hz;
++ tmp_float = d_CN0_SNV_dB_Hz;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
- tmp_float=d_carrier_lock_test;
++ tmp_float = d_carrier_lock_test;
+ d_dump_file.write((char*)&tmp_float, sizeof(float));
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples;
diff --cc src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
index fea305f,3927e03..592d351
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
@@@ -115,41 -116,32 +116,29 @@@ Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::Gps_
//--- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
- // Initialization of local code replica
- // Get space for a vector with the C/A code replica sampled 1x/chip
- //d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
- d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_get_alignment()));
-
- multicorrelator_gpu = new cuda_multicorrelator();
- int N_CORRELATORS = 3;
- //local code resampler on CPU (old)
- //multicorrelator_gpu->init_cuda(0, NULL, 2 * d_vector_length , 2 * d_vector_length , N_CORRELATORS);
-
- //local code resampler on GPU (new)
- multicorrelator_gpu->init_cuda_integrated_resampler(0, NULL, 2 * d_vector_length , GPS_L1_CA_CODE_LENGTH_CHIPS , N_CORRELATORS);
-
- // Get space for the resampled early / prompt / late local replicas
- cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped );
-
+ // Set GPU flags
+ cudaSetDeviceFlags(cudaDeviceMapHost);
//allocate host memory
//pinned memory mode - use special function to get OS-pinned memory
- cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped );
-
- //old local codes vector
- // (cudaHostAlloc((void**)&d_local_codes_gpu, (V_LEN * sizeof(gr_complex))*N_CORRELATORS, cudaHostAllocWriteCombined ));
-
- //new integrated shifts
- // (cudaHostAlloc((void**)&d_local_codes_gpu, (2 * d_vector_length * sizeof(gr_complex)), cudaHostAllocWriteCombined ));
-
- // correlator outputs (scalar)
- cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocWriteCombined );
- int N_CORRELATORS=3;
++ int N_CORRELATORS = 3;
+ // Get space for a vector with the C/A code replica sampled 1x/chip
+ cudaHostAlloc((void**)&d_ca_code, (GPS_L1_CA_CODE_LENGTH_CHIPS* sizeof(gr_complex)), cudaHostAllocMapped || cudaHostAllocWriteCombined);
+ // Get space for the resampled early / prompt / late local replicas
+ cudaHostAlloc((void**)&d_local_code_shift_chips, N_CORRELATORS * sizeof(float), cudaHostAllocMapped || cudaHostAllocWriteCombined);
- cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped || cudaHostAllocWriteCombined);
++ cudaHostAlloc((void**)&in_gpu, 2 * d_vector_length * sizeof(gr_complex), cudaHostAllocMapped || cudaHostAllocWriteCombined);
+ // correlator outputs (scalar)
+ cudaHostAlloc((void**)&d_corr_outs_gpu ,sizeof(gr_complex)*N_CORRELATORS, cudaHostAllocMapped || cudaHostAllocWriteCombined );
- //map to EPL pointers
+ //map to EPL pointers
d_Early = &d_corr_outs_gpu[0];
- d_Prompt = &d_corr_outs_gpu[1];
+ d_Prompt = &d_corr_outs_gpu[1];
d_Late = &d_corr_outs_gpu[2];
//--- Perform initializations ------------------------------
+ multicorrelator_gpu = new cuda_multicorrelator();
+ //local code resampler on GPU
- multicorrelator_gpu->init_cuda_integrated_resampler(2 * d_vector_length,GPS_L1_CA_CODE_LENGTH_CHIPS,3);
- multicorrelator_gpu->set_input_output_vectors(
- d_corr_outs_gpu,
- in_gpu
- );
++ multicorrelator_gpu->init_cuda_integrated_resampler(2 * d_vector_length, GPS_L1_CA_CODE_LENGTH_CHIPS, 3);
++ multicorrelator_gpu->set_input_output_vectors(d_corr_outs_gpu, in_gpu);
// define initial code frequency basis of NCO
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
// define residual code phase (in chips)
@@@ -249,7 -242,12 +238,7 @@@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc:
d_local_code_shift_chips[1]=0.0;
d_local_code_shift_chips[2]=d_early_late_spc_chips;
- multicorrelator_gpu->set_local_code_and_taps(GPS_L1_CA_CODE_LENGTH_CHIPS,d_ca_code, d_local_code_shift_chips,3);
- multicorrelator_gpu->set_local_code_and_taps(
- GPS_L1_CA_CODE_LENGTH_CHIPS,
- d_ca_code,
- d_local_code_shift_chips,
- 3
- );
++ multicorrelator_gpu->set_local_code_and_taps(GPS_L1_CA_CODE_LENGTH_CHIPS, d_ca_code, d_local_code_shift_chips, 3);
d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0;
@@@ -280,17 -278,14 +269,13 @@@
Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::~Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc()
{
d_dump_file.close();
-
+ cudaFreeHost(in_gpu);
- cudaFreeHost(d_carr_sign_gpu);
+ cudaFreeHost(d_corr_outs_gpu);
+ cudaFreeHost(d_local_code_shift_chips);
++ cudaFreeHost(d_ca_code);
- cudaFreeHost(in_gpu);
- cudaFreeHost(d_corr_outs_gpu);
- cudaFreeHost(d_local_code_shift_chips);
- cudaFreeHost(d_ca_code);
-
- multicorrelator_gpu->free_cuda();
- delete(multicorrelator_gpu);
+ multicorrelator_gpu->free_cuda();
+ delete(multicorrelator_gpu);
-
- volk_free(d_ca_code);
-
delete[] d_Prompt_buffer;
}
@@@ -340,17 -335,16 +325,10 @@@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::
float code_phase_step_chips = static_cast<float>(d_code_freq_chips) / static_cast<float>(d_fs_in);
float rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
- memcpy(in_gpu,in,sizeof(gr_complex)*d_current_prn_length_samples);
++ memcpy(in_gpu, in, sizeof(gr_complex) * d_current_prn_length_samples);
cudaProfilerStart();
-- multicorrelator_gpu->Carrier_wipeoff_multicorrelator_resampler_cuda(
- d_corr_outs_gpu,
- in,
- d_rem_carr_phase_rad,
- phase_step_rad,
- code_phase_step_chips,
- rem_code_phase_chips,
- d_current_prn_length_samples,
- 3);
- d_rem_carr_phase_rad,
- phase_step_rad,
- code_phase_step_chips,
- rem_code_phase_chips,
- d_current_prn_length_samples,
- 3);
- cudaProfilerStop();
++ multicorrelator_gpu->Carrier_wipeoff_multicorrelator_resampler_cuda(d_rem_carr_phase_rad, phase_step_rad, code_phase_step_chips, rem_code_phase_chips, d_current_prn_length_samples, 3);
+ cudaProfilerStop();
// ################## PLL ##########################################################
// PLL discriminator
diff --cc src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
index 018fd64,7090977..e34204b
--- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
+++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
@@@ -337,10 -337,10 +337,10 @@@ int gps_l2_m_dll_pll_tracking_cc::gener
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
- float carr_error_hz=0;
- float carr_error_filt_hz=0;
- float code_error_chips=0;
- float code_error_filt_chips=0;
- double carr_error_hz=0;
- double carr_error_filt_hz=0;
- double code_error_chips=0;
- double code_error_filt_chips=0;
++ double carr_error_hz = 0;
++ double carr_error_filt_hz = 0;
++ double code_error_chips = 0;
++ double code_error_filt_chips = 0;
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data = Gnss_Synchro();
@@@ -585,40 -585,40 +585,40 @@@
tmp_L = std::abs<float>(*d_Late);
try
{
- // EPR
- d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
- // PROMPT I and Q (to analyze navigation symbols)
- d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
- // PRN start sample stamp
- //tmp_float=(float)d_sample_counter;
- d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
- // accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
-
- // carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
-
- //PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
-
- //DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
-
- // CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
-
- // AUX vars (for debug purposes)
- tmp_double = d_rem_code_phase_samples;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
- tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
- d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+ // EPR
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
+ // PROMPT I and Q (to analyze navigation symbols)
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
+ d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
+ // PRN start sample stamp
+ //tmp_float=(float)d_sample_counter;
+ d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
+ // accumulated carrier phase
- d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
++ d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
+
+ // carrier and code frequency
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
++ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
++ d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
+
+ //PLL commands
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
++ d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
++ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
+
+ //DLL commands
- d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
++ d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
++ d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
+
+ // CN0 and carrier lock test
- d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
- d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
++ d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
++ d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
+
+ // AUX vars (for debug purposes)
- tmp_float = d_rem_code_phase_samples;
- d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
++ tmp_double = d_rem_code_phase_samples;
++ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
+ tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
+ d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
catch (std::ifstream::failure& e)
{
diff --cc src/algorithms/tracking/libs/cpu_multicorrelator.cc
index 0000000,0ed2ac9..befac31
mode 000000,100644..100644
--- a/src/algorithms/tracking/libs/cpu_multicorrelator.cc
+++ b/src/algorithms/tracking/libs/cpu_multicorrelator.cc
@@@ -1,0 -1,167 +1,163 @@@
+ /*!
+ * \file cpu_multicorrelator.cc
+ * \brief High optimized CPU vector multiTAP correlator class
+ * \authors <ul>
+ * <li> Javier Arribas, 2015. jarribas(at)cttc.es
+ * </ul>
+ *
+ * Class that implements a high optimized vector multiTAP correlator class for CPUs
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ * Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * 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/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+ #include "cpu_multicorrelator.h"
-
++#include <cmath>
+ #include <iostream>
-#include <volk/volk.h>
+ #include <gnuradio/fxpt.h> // fixed point sine and cosine
-#include <cmath>
++#include <volk/volk.h>
++
+
+ bool cpu_multicorrelator::init(
+ int max_signal_length_samples,
+ int n_correlators
+ )
+ {
-
+ // ALLOCATE MEMORY FOR INTERNAL vectors
+ size_t size = max_signal_length_samples * sizeof(std::complex<float>);
+
- // NCO signal
- d_nco_in=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
++ // NCO signal
++ d_nco_in = static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+
- // Doppler-free signal
- d_sig_doppler_wiped=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
++ // Doppler-free signal
++ d_sig_doppler_wiped = static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
+
- d_local_codes_resampled=new std::complex<float>*[n_correlators];
- for (int n=0;n<n_correlators;n++)
- {
- d_local_codes_resampled[n]=static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
- }
- d_n_correlators=n_correlators;
- return true;
++ d_local_codes_resampled = new std::complex<float>*[n_correlators];
++ for (int n = 0; n < n_correlators; n++)
++ {
++ d_local_codes_resampled[n] = static_cast<std::complex<float>*>(volk_malloc(size, volk_get_alignment()));
++ }
++ d_n_correlators = n_correlators;
++ return true;
+ }
+
++
+ bool cpu_multicorrelator::set_local_code_and_taps(
+ int code_length_chips,
+ const std::complex<float>* local_code_in,
+ float *shifts_chips
+ )
+ {
-
- d_local_code_in=local_code_in;
- d_shifts_chips=shifts_chips;
- d_code_length_chips=code_length_chips;
- return true;
++ d_local_code_in = local_code_in;
++ d_shifts_chips = shifts_chips;
++ d_code_length_chips = code_length_chips;
++ return true;
+ }
+
-bool cpu_multicorrelator::set_input_output_vectors(
- std::complex<float>* corr_out,
- const std::complex<float>* sig_in
- )
-{
- // Save CPU pointers
- d_sig_in =sig_in;
- d_corr_out = corr_out;
- return true;
+
++bool cpu_multicorrelator::set_input_output_vectors(std::complex<float>* corr_out, const std::complex<float>* sig_in)
++{
++ // Save CPU pointers
++ d_sig_in = sig_in;
++ d_corr_out = corr_out;
++ return true;
+ }
+
++
+ void cpu_multicorrelator::update_local_code(int correlator_length_samples,float rem_code_phase_chips, float code_phase_step_chips)
+ {
+ float local_code_chip_index;
- for (int current_correlator_tap=0; current_correlator_tap<d_n_correlators;current_correlator_tap++)
- {
- for (int n = 0; n < correlator_length_samples; n++)
- {
- // resample code for current tap
- local_code_chip_index= fmod(code_phase_step_chips*static_cast<float>(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips);
- //Take into account that in multitap correlators, the shifts can be negative!
- if (local_code_chip_index<0.0) local_code_chip_index+=d_code_length_chips;
- d_local_codes_resampled[current_correlator_tap][n]=d_local_code_in[static_cast<int>(round(local_code_chip_index))];
-
- }
- }
++ for (int current_correlator_tap = 0; current_correlator_tap < d_n_correlators; current_correlator_tap++)
++ {
++ for (int n = 0; n < correlator_length_samples; n++)
++ {
++ // resample code for current tap
++ local_code_chip_index = fmod(code_phase_step_chips*static_cast<float>(n)+ d_shifts_chips[current_correlator_tap] - rem_code_phase_chips, d_code_length_chips);
++ //Take into account that in multitap correlators, the shifts can be negative!
++ if (local_code_chip_index < 0.0) local_code_chip_index += d_code_length_chips;
++ d_local_codes_resampled[current_correlator_tap][n] = d_local_code_in[static_cast<int>(round(local_code_chip_index))];
++ }
++ }
+ }
+
+
+ void cpu_multicorrelator::update_local_carrier(int correlator_length_samples, float rem_carr_phase_rad, float phase_step_rad)
+ {
+ float sin_f, cos_f;
+ int phase_step_rad_i = gr::fxpt::float_to_fixed(phase_step_rad);
+ int phase_rad_i = gr::fxpt::float_to_fixed(rem_carr_phase_rad);
+
+ for(int i = 0; i < correlator_length_samples; i++)
+ {
+ gr::fxpt::sincos(phase_rad_i, &sin_f, &cos_f);
+ d_nco_in[i] = std::complex<float>(cos_f, -sin_f);
+ phase_rad_i += phase_step_rad_i;
+ }
+ }
+
+ bool cpu_multicorrelator::Carrier_wipeoff_multicorrelator_resampler(
- float rem_carrier_phase_in_rad,
- float phase_step_rad,
++ float rem_carrier_phase_in_rad,
++ float phase_step_rad,
+ float rem_code_phase_chips,
+ float code_phase_step_chips,
- int signal_length_samples)
- {
++ int signal_length_samples)
++{
+
- update_local_carrier(signal_length_samples, rem_carrier_phase_in_rad, phase_step_rad);
- update_local_code(signal_length_samples,rem_code_phase_chips, code_phase_step_chips);
++ update_local_carrier(signal_length_samples, rem_carrier_phase_in_rad, phase_step_rad);
++ update_local_code(signal_length_samples,rem_code_phase_chips, code_phase_step_chips);
+
+ volk_32fc_x2_multiply_32fc(d_sig_doppler_wiped, d_sig_in, d_nco_in, signal_length_samples);
- for (int current_correlator_tap=0; current_correlator_tap<d_n_correlators;current_correlator_tap++)
- {
- volk_32fc_x2_dot_prod_32fc(&d_corr_out[current_correlator_tap], d_sig_doppler_wiped, d_local_codes_resampled[current_correlator_tap], signal_length_samples);
- }
++ for (int current_correlator_tap = 0; current_correlator_tap < d_n_correlators; current_correlator_tap++)
++ {
++ volk_32fc_x2_dot_prod_32fc(&d_corr_out[current_correlator_tap], d_sig_doppler_wiped, d_local_codes_resampled[current_correlator_tap], signal_length_samples);
++ }
+ return true;
+ }
+
+
+ cpu_multicorrelator::cpu_multicorrelator()
+ {
- d_sig_in=NULL;
- d_nco_in=NULL;
- d_sig_doppler_wiped=NULL;
- d_local_code_in=NULL;
- d_shifts_chips=NULL;
- d_corr_out=NULL;
- d_code_length_chips=0;
- d_n_correlators=0;
++ d_sig_in = NULL;
++ d_nco_in = NULL;
++ d_sig_doppler_wiped = NULL;
++ d_local_code_in = NULL;
++ d_shifts_chips = NULL;
++ d_corr_out = NULL;
++ d_code_length_chips = 0;
++ d_n_correlators = 0;
+ }
+
+ bool cpu_multicorrelator::free()
+ {
- // Free memory
- if (d_sig_doppler_wiped!=NULL) volk_free(d_sig_doppler_wiped);
- if (d_nco_in!=NULL) volk_free(d_nco_in);
- for (int n=0;n<d_n_correlators;n++)
- {
- volk_free(d_local_codes_resampled[n]);
- }
++ // Free memory
++ if (d_sig_doppler_wiped != NULL) volk_free(d_sig_doppler_wiped);
++ if (d_nco_in != NULL) volk_free(d_nco_in);
++ for (int n = 0; n < d_n_correlators; n++)
++ {
++ volk_free(d_local_codes_resampled[n]);
++ }
+ delete d_local_codes_resampled;
- return true;
++ return true;
+ }
+
diff --cc src/algorithms/tracking/libs/cpu_multicorrelator.h
index 0000000,ca0f068..1d7b7c6
mode 000000,100644..100644
--- a/src/algorithms/tracking/libs/cpu_multicorrelator.h
+++ b/src/algorithms/tracking/libs/cpu_multicorrelator.h
@@@ -1,0 -1,98 +1,98 @@@
+ /*!
+ * \file cpu_multicorrelator.h
+ * \brief High optimized CPU vector multiTAP correlator class
+ * \authors <ul>
+ * <li> Javier Arribas, 2015. jarribas(at)cttc.es
+ * </ul>
+ *
+ * Class that implements a high optimized vector multiTAP correlator class for CPUs
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ * Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * 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/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
-#ifndef CPU_MULTICORRELATOR_H_
-#define CPU_MULTICORRELATOR_H_
++#ifndef GNSS_SDR_CPU_MULTICORRELATOR_H_
++#define GNSS_SDR_CPU_MULTICORRELATOR_H_
+
+ #include <complex>
+
+ /*!
+ * \brief Class that implements carrier wipe-off and correlators.
+ */
+ class cpu_multicorrelator
+ {
+ public:
- cpu_multicorrelator();
++ cpu_multicorrelator();
+ bool init(
+ int max_signal_length_samples,
+ int n_correlators
+ );
+ bool set_local_code_and_taps(
+ int code_length_chips,
+ const std::complex<float>* local_code_in,
+ float *shifts_chips
+ );
+ bool set_input_output_vectors(
+ std::complex<float>* corr_out,
+ const std::complex<float>* sig_in
+ );
+ void update_local_code(
+ int correlator_length_samples,
+ float rem_code_phase_chips,
+ float code_phase_step_chips
+ );
+
+ void update_local_carrier(
+ int correlator_length_samples,
+ float rem_carr_phase_rad,
+ float phase_step_rad
+ );
+ bool Carrier_wipeoff_multicorrelator_resampler(
+ float rem_carrier_phase_in_rad,
+ float phase_step_rad,
+ float rem_code_phase_chips,
+ float code_phase_step_chips,
+ int signal_length_samples);
+ bool free();
+
+ private:
+ // Allocate the device input vectors
+ const std::complex<float> *d_sig_in;
+ std::complex<float> *d_nco_in;
+ std::complex<float> **d_local_codes_resampled;
+ std::complex<float> *d_sig_doppler_wiped;
+ const std::complex<float> *d_local_code_in;
+ std::complex<float> *d_corr_out;
+
+ float *d_shifts_chips;
+ int d_code_length_chips;
+ int d_n_correlators;
+
+ bool update_local_code();
+ bool update_local_carrier();
+
+ };
+
+
+ #endif /* CPU_MULTICORRELATOR_H_ */
diff --cc src/algorithms/tracking/libs/cuda_multicorrelator.h
index df640f5,fb2c9a9..e55b17d
--- a/src/algorithms/tracking/libs/cuda_multicorrelator.h
+++ b/src/algorithms/tracking/libs/cuda_multicorrelator.h
@@@ -32,8 -32,8 +32,8 @@@
* -------------------------------------------------------------------------
*/
--#ifndef CUDA_MULTICORRELATOR_H_
--#define CUDA_MULTICORRELATOR_H_
++#ifndef GNSS_SDR_CUDA_MULTICORRELATOR_H_
++#define GNSS_SDR_CUDA_MULTICORRELATOR_H_
#ifdef __CUDACC__
@@@ -107,6 -107,6 +107,8 @@@ struct GPU_Complex_Shor
return GPU_Complex_Short(r+a.r, i+a.i);
}
};
++
++
/*!
* \brief Class that implements carrier wipe-off and correlators using NVIDIA CUDA GPU accelerators.
*/
@@@ -168,4 -164,4 +166,4 @@@ private
};
--#endif /* CUDA_MULTICORRELATOR_H_ */
++#endif /* GNSS_SDR_CUDA_MULTICORRELATOR_H_ */
diff --cc src/core/system_parameters/GPS_L1_CA.h
index dea0cbb,cb63bb3..16b6b2a
--- a/src/core/system_parameters/GPS_L1_CA.h
+++ b/src/core/system_parameters/GPS_L1_CA.h
@@@ -67,6 -68,9 +68,9 @@@ const double MAX_TOA_DELAY_MS = 20
//#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here
const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
+
+ // OBSERVABLE HISTORY DEEP FOR INTERPOLATION
-const int GPS_L1_CA_HISTORY_DEEP=100;
++const int GPS_L1_CA_HISTORY_DEEP = 100;
// NAVIGATION MESSAGE DEMODULATION AND DECODING
#define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1}
diff --cc src/core/system_parameters/MATH_CONSTANTS.h
index 9b2f7b6,9b2f7b6..1776c26
--- a/src/core/system_parameters/MATH_CONSTANTS.h
+++ b/src/core/system_parameters/MATH_CONSTANTS.h
@@@ -55,7 -55,7 +55,7 @@@ const double TWO_N2 = (0.25)
const double TWO_N5 = (0.03125); //!< 2^-5
const double TWO_N8 = (0.00390625); //!< 2^-8
const double TWO_N9 = (0.001953125); //!< 2^-9
--
++const double TWO_N10 = (0.0009765625); //!< 2^-10
const double TWO_N11 = (4.882812500000000e-004); //!< 2^-11
const double TWO_N14 = (0.00006103515625); //!< 2^-14
const double TWO_N15 = (0.00003051757813); //!< 2^-15
diff --cc src/core/system_parameters/rtcm.cc
index 6e8c888,0000000..021bdd5
mode 100644,000000..100644
--- a/src/core/system_parameters/rtcm.cc
+++ b/src/core/system_parameters/rtcm.cc
@@@ -1,1834 -1,0 +1,2197 @@@
+/*!
+ * \file rtcm.cc
+ * \brief Implementation of RTCM 3.2 Standard
+ * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ * Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * 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/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+#include "rtcm.h"
+#include <algorithm> // for std::reverse
+#include <cmath> // for std::fmod
+#include <cstdlib> // for strtol
+#include <sstream> // for std::stringstream
+#include <boost/algorithm/string.hpp> // for to_upper_copy
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/dynamic_bitset.hpp>
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include "GPS_L1_CA.h"
++#include "GPS_L2C.h"
++#include "MATH_CONSTANTS.h"
+
+
+
+using google::LogMessage;
+
+DEFINE_int32(RTCM_Ref_Station_ID, 1234, "Reference Station ID in RTCM messages");
+
+
+
+Rtcm::Rtcm()
+{
+ Rtcm::reset_data_fields();
+ preamble = std::bitset<8>("11010011");
+ reserved_field = std::bitset<6>("000000");
+}
+
+
+
+
+// *****************************************************************************************************
+//
+// TRANSPORT LAYER AS DEFINED AT RTCM STANDARD 10403.2
+//
+// *****************************************************************************************************
+
+std::string Rtcm::add_CRC (const std::string& message_without_crc)
+{
+ // ****** Computes Qualcomm CRC-24Q ******
+ crc_24_q_type CRC_RTCM;
+ // 1) Converts the string to a vector of unsigned char:
+ boost::dynamic_bitset<unsigned char> frame_bits(message_without_crc);
+ std::vector<unsigned char> bytes;
+ boost::to_block_range(frame_bits, std::back_inserter(bytes));
+ std::reverse(bytes.begin(),bytes.end());
+
+ // 2) Computes CRC
+ CRC_RTCM.process_bytes(bytes.data(), bytes.size());
+ crc_frame = std::bitset<24>(CRC_RTCM.checksum());
+
+ // 3) Builds the complete message
+ std::string complete_message = message_without_crc + crc_frame.to_string();
+ return bin_to_hex(complete_message);
+}
+
+
+bool Rtcm::check_CRC(const std::string & message)
+{
+ crc_24_q_type CRC_RTCM_CHECK;
+ // Convert message to binary
+ std::string message_bin = Rtcm::hex_to_bin(message);
+ // Check CRC
+ std::string crc = message_bin.substr(message_bin.length() - 24, 24);
+ std::bitset<24> read_crc = std::bitset<24>(crc);
+ std::string msg_without_crc = message_bin.substr(0, message_bin.length() - 24);
+
+ boost::dynamic_bitset<unsigned char> frame_bits(msg_without_crc);
+ std::vector<unsigned char> bytes;
+ boost::to_block_range(frame_bits, std::back_inserter(bytes));
+ std::reverse(bytes.begin(),bytes.end());
+
+ CRC_RTCM_CHECK.process_bytes(bytes.data(), bytes.size());
+ std::bitset<24> computed_crc = std::bitset<24>(CRC_RTCM_CHECK.checksum());
+ if(read_crc == computed_crc)
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+
+std::string Rtcm::bin_to_hex(const std::string& s)
+{
+ std::string s_aux;
+ std::stringstream ss;
+ int remainder = static_cast<int>(std::fmod(s.length(), 4));
+
+ if (remainder != 0)
+ {
+ s_aux.assign(s, 0 , remainder);
+ boost::dynamic_bitset<> rembits(s_aux);
+ unsigned n = rembits.to_ulong();
+ ss << std::hex << n;
+ }
+
+ unsigned int start = std::max(remainder, 0);
+ for(unsigned int i = start; i < s.length() - 1; i = i + 4)
+ {
+ s_aux.assign(s, i, 4);
+ std::bitset<4> bs(s_aux);
+ unsigned n = bs.to_ulong();
+ ss << std::hex << n;
+ }
+ return boost::to_upper_copy(ss.str());
+}
+
+
+std::string Rtcm::hex_to_bin(const std::string& s)
+{
+ std::string s_aux;
+ s_aux.clear();
+ std::stringstream ss;
+ ss << s;
+ std::string s_lower = boost::to_upper_copy(ss.str());
+ for(unsigned int i = 0; i < s.length(); i++)
+ {
+ unsigned long int n;
+ std::istringstream(s_lower.substr(i,1)) >> std::hex >> n;
+ std::bitset<4> bs(n);
+ s_aux += bs.to_string();
+ }
+ return s_aux;
+}
+
+
+unsigned long int Rtcm::bin_to_uint(const std::string& s)
+{
+ if(s.length() > 32)
+ {
+ LOG(WARNING) << "Cannot convert to a unsigned long int";
+ return 0;
+ }
+ unsigned long int reading = strtoul(s.c_str(), NULL, 2);
+ return reading;
+}
+
+
+long int Rtcm::bin_to_int(const std::string& s)
+{
+ if(s.length() > 32)
+ {
+ LOG(WARNING) << "Cannot convert to a long int";
+ return 0;
+ }
+ long int reading;
+
+ // Handle negative numbers
+ if(s.substr(0,1).compare("0"))
+ {
+ // Computing two's complement
+ boost::dynamic_bitset<> original_bitset(s);
+ original_bitset.flip();
+ reading = - (original_bitset.to_ulong() + 1);
+ }
+ else
+ {
+ reading = strtol(s.c_str(), NULL, 2);
+ }
+ return reading;
+}
+
+
+double Rtcm::bin_to_double(const std::string& s)
+{
+ double reading;
+ if(s.length() > 64)
+ {
+ LOG(WARNING) << "Cannot convert to a double";
+ return 0;
+ }
+
+ long long int reading_int;
+
+ // Handle negative numbers
+ if(s.substr(0,1).compare("0"))
+ {
+ // Computing two's complement
+ boost::dynamic_bitset<> original_bitset(s);
+ original_bitset.flip();
+ reading_int = - (original_bitset.to_ulong() + 1);
+ }
+ else
+ {
+ reading_int = strtoll(s.c_str(), NULL, 2);
+ }
+
+ reading = static_cast<double>(reading_int);
+ return reading;
+}
+
+
+unsigned long int Rtcm::hex_to_uint(const std::string& s)
+{
+ if(s.length() > 32)
+ {
+ LOG(WARNING) << "Cannot convert to a unsigned long int";
+ return 0;
+ }
+ unsigned long int reading = strtoul(s.c_str(), NULL, 16);
+ return reading;
+}
+
+
+long int Rtcm::hex_to_int(const std::string& s)
+{
+ if(s.length() > 32)
+ {
+ LOG(WARNING) << "Cannot convert to a long int";
+ return 0;
+ }
+ long int reading = strtol(s.c_str(), NULL, 16);
+ return reading;
+}
+
+
+std::string Rtcm::build_message(std::string data)
+{
+ unsigned int msg_length_bits = data.length();
+ unsigned int msg_length_bytes = std::ceil(static_cast<float>(msg_length_bits) / 8.0);
+ message_length = std::bitset<10>(msg_length_bytes);
+ unsigned int zeros_to_fill = 8 * msg_length_bytes - msg_length_bits;
+ std::string b(zeros_to_fill, '0');
+ std::string msg_content = data + b;
+ std::string msg_without_crc = preamble.to_string() +
+ reserved_field.to_string() +
+ message_length.to_string() +
+ msg_content;
+ return Rtcm::add_CRC(msg_without_crc);
+}
+
+
+
+// *****************************************************************************************************
+//
+// MESSAGES AS DEFINED AT RTCM STANDARD 10403.2
+//
+// *****************************************************************************************************
+
+
+
+// **********************************************
+//
+// MESSAGE TYPE 1001 (GPS L1 OBSERVATIONS)
+//
+// **********************************************
+
+std::bitset<64> Rtcm::get_MT1001_header(const Gps_Ephemeris & gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & pseudoranges,
+ unsigned int ref_id, unsigned int smooth_int, bool sync_flag, bool divergence_free)
+{
+ unsigned int m1001 = 1001;
+ unsigned int reference_station_id = ref_id; // Max: 4095
+ const std::map<int, Gnss_Synchro> pseudoranges_ = pseudoranges;
+ bool synchronous_GNSS_flag = sync_flag;
+ bool divergence_free_smoothing_indicator = divergence_free;
+ unsigned int smoothing_interval = smooth_int;
+ Rtcm::set_DF002(m1001);
+ Rtcm::set_DF003(reference_station_id);
+ Rtcm::set_DF004(gps_eph, obs_time);
+ Rtcm::set_DF005(synchronous_GNSS_flag);
+ Rtcm::set_DF006(pseudoranges_);
+ Rtcm::set_DF007(divergence_free_smoothing_indicator);
+ Rtcm::set_DF008(smoothing_interval);
+
+ std::string header = DF002.to_string() +
+ DF003.to_string() +
+ DF004.to_string() +
+ DF005.to_string() +
+ DF006.to_string() +
+ DF007.to_string() +
+ DF008.to_string();
+
+ std::bitset<64> header_msg(header);
+ return header_msg;
+}
+
+
+std::bitset<58> Rtcm::get_MT1001_sat_content(const Gnss_Synchro & gnss_synchro)
+{
+ Gnss_Synchro gnss_synchro_ = gnss_synchro;
+ bool code_indicator = false; // code indicator 0: C/A code 1: P(Y) code direct
+ Rtcm::set_DF009(gnss_synchro_);
+ Rtcm::set_DF010(code_indicator); // code indicator 0: C/A code 1: P(Y) code direct
+ Rtcm::set_DF011(gnss_synchro_);
-
- long int gps_L1_phaserange_minus_L1_pseudorange;
- long int phaserange_m = (gnss_synchro.Carrier_phase_rads * GPS_C_m_s) / (GPS_TWO_PI * GPS_L1_FREQ_HZ);
- gps_L1_phaserange_minus_L1_pseudorange = phaserange_m; // TODO
- DF012 = std::bitset<20>(gps_L1_phaserange_minus_L1_pseudorange);
++ Rtcm::set_DF012(gnss_synchro_);
+
+ unsigned int lock_time_indicator = 0; // TODO
+ DF013 = std::bitset<7>(lock_time_indicator);
+
+ std::string content = DF009.to_string() +
+ DF010.to_string() +
+ DF011.to_string() +
+ DF012.to_string() +
+ DF013.to_string();
+
+ std::bitset<58> content_msg(content);
+ return content_msg;
+}
+
+
+std::string Rtcm::print_MT1001(const Gps_Ephemeris & gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & pseudoranges)
+{
+ unsigned int ref_id = static_cast<unsigned int>(FLAGS_RTCM_Ref_Station_ID);
+ unsigned int smooth_int = 0;
+ bool sync_flag = false;
+ bool divergence_free = false;
+
+ std::bitset<64> header = Rtcm::get_MT1001_header(gps_eph, obs_time, pseudoranges, ref_id, smooth_int, sync_flag, divergence_free);
+ std::string data = header.to_string();
+
+ std::map<int, Gnss_Synchro>::const_iterator pseudoranges_iter;
+ for(pseudoranges_iter = pseudoranges.begin();
+ pseudoranges_iter != pseudoranges.end();
+ pseudoranges_iter++)
+ {
+ std::bitset<58> content = Rtcm::get_MT1001_sat_content(pseudoranges_iter->second);
+ data += content.to_string();
+ }
+
+ return Rtcm::build_message(data);
+}
+
+
+
+
+// **********************************************
+//
+// MESSAGE TYPE 1005 (STATION DESCRIPTION)
+//
+// **********************************************
+
+
+/* Stationary Antenna Reference Point, No Height Information
+ * Reference Station Id = 2003
+ GPS Service supported, but not GLONASS or Galileo
+ ARP ECEF-X = 1114104.5999 meters
+ ARP ECEF-Y = -4850729.7108 meters
+ ARP ECEF-Z = 3975521.4643 meters
+ Expected output: D3 00 13 3E D7 D3 02 02 98 0E DE EF 34 B4 BD 62
+ AC 09 41 98 6F 33 36 0B 98
+ */
+std::bitset<152> Rtcm::get_MT1005_test ()
+{
+ unsigned int mt1005 = 1005;
+ unsigned int reference_station_id = 2003; // Max: 4095
+ double ECEF_X = 1114104.5999; // units: m
+ double ECEF_Y = -4850729.7108; // units: m
+ double ECEF_Z = 3975521.4643; // units: m
+
+ std::bitset<1> DF001_;
+
+ Rtcm::set_DF002(mt1005);
+ Rtcm::set_DF003(reference_station_id);
+ Rtcm::set_DF021();
+ Rtcm::set_DF022(true); // GPS
+ Rtcm::set_DF023(false); // Glonass
+ Rtcm::set_DF024(false); // Galileo
+ DF141 = std::bitset<1>("0"); // 0: Real, physical reference station
+ DF001_ = std::bitset<1>("0"); // Reserved, set to 0
+ Rtcm::set_DF025(ECEF_X);
+ DF142 = std::bitset<1>("0"); // Single Receiver Oscillator Indicator
+ Rtcm::set_DF026(ECEF_Y);
+ DF364 = std::bitset<2>("00"); // Quarter Cycle Indicator
+ Rtcm::set_DF027(ECEF_Z);
+
+ std::string message = DF002.to_string() +
+ DF003.to_string() +
+ DF021.to_string() +
+ DF022.to_string() +
+ DF023.to_string() +
+ DF024.to_string() +
+ DF141.to_string() +
+ DF025.to_string() +
+ DF142.to_string() +
+ DF001_.to_string() +
+ DF026.to_string() +
+ DF364.to_string() +
+ DF027.to_string() ;
+
+ std::bitset<152> test_msg(message);
+ return test_msg;
+}
+
+
+std::string Rtcm::print_MT1005( unsigned int ref_id, double ecef_x, double ecef_y, double ecef_z, bool gps, bool glonass, bool galileo, bool non_physical, bool single_oscillator, unsigned int quarter_cycle_indicator)
+{
+ unsigned int msg_number = 1005;
+ std::bitset<1> DF001_;
+
+ Rtcm::set_DF002(msg_number);
+ Rtcm::set_DF003(ref_id);
+ Rtcm::set_DF021();
+ Rtcm::set_DF022(gps);
+ Rtcm::set_DF023(glonass);
+ Rtcm::set_DF024(galileo);
+ DF141 = std::bitset<1>(non_physical);
+ DF001_ = std::bitset<1>("0");
+ Rtcm::set_DF025(ecef_x);
+ DF142 = std::bitset<1>(single_oscillator);
+ Rtcm::set_DF026(ecef_y);
+ DF364 = std::bitset<2>(quarter_cycle_indicator);
+ Rtcm::set_DF027(ecef_z);
+
+ std::string data = DF002.to_string() +
+ DF003.to_string() +
+ DF021.to_string() +
+ DF022.to_string() +
+ DF023.to_string() +
+ DF024.to_string() +
+ DF141.to_string() +
+ DF025.to_string() +
+ DF142.to_string() +
+ DF001_.to_string() +
+ DF026.to_string() +
+ DF364.to_string() +
+ DF027.to_string() ;
+
+ std::string message = build_message(data);
+ return message;
+}
+
+
+int Rtcm::read_MT1005(const std::string & message, unsigned int & ref_id, double & ecef_x, double & ecef_y, double & ecef_z, bool & gps, bool & glonass, bool & galileo)
+{
+ // Convert message to binary
+ std::string message_bin = Rtcm::hex_to_bin(message);
+
+ if(!Rtcm::check_CRC(message) )
+ {
+ LOG(WARNING) << " Bad CRC detected in RTCM message MT1005";
+ return 1;
+ }
+
+ // Check than the message number is correct
+ unsigned int preamble_length = 8;
+ unsigned int reserved_field_length = 6;
+ unsigned int index = preamble_length + reserved_field_length;
+
+ unsigned int read_message_length = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+ index += 10;
+ if (read_message_length != 19)
+ {
+ LOG(WARNING) << " Message MT1005 with wrong length (19 bytes expected, " << read_message_length << " received)";
+ return 1;
+ }
+
+ unsigned int msg_number = 1005;
+ Rtcm::set_DF002(msg_number);
+ std::bitset<12> read_msg_number(message_bin.substr(index, 12));
+ index += 12;
+
+ if (DF002 != read_msg_number)
+ {
+ LOG(WARNING) << " This is not a MT1005 message";
+ return 1;
+ }
+
+
+ ref_id = Rtcm::bin_to_uint(message_bin.substr(index, 12));
+ index += 12;
+
+ index += 6; // ITRF year
+ gps = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+ index += 1;
+
+ glonass = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+ index += 1;
+
+ galileo = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+ index += 1;
+
+ index += 1; // ref_station_indicator
+
+ ecef_x = Rtcm::bin_to_double(message_bin.substr(index, 38)) / 10000.0;
+ index += 38;
+
+ index += 1; // single rx oscillator
+ index += 1; // reserved
+
+ ecef_y = Rtcm::bin_to_double(message_bin.substr(index, 38)) / 10000.0;
+ index += 38;
+
+ index += 2; // quarter cycle indicator
+ ecef_z = Rtcm::bin_to_double(message_bin.substr(index, 38)) / 10000.0;
+
+ return 0;
+}
+
+
+std::string Rtcm::print_MT1005_test()
+{
+ std::bitset<152> mt1005 = get_MT1005_test();
+ return Rtcm::build_message(mt1005.to_string());
+}
+
+
+
+// **********************************************
+//
+// MESSAGE TYPE 1019 (GPS EPHEMERIS)
+//
+// **********************************************
+
+std::string Rtcm::print_MT1019(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int msg_number = 1019;
+
+ Rtcm::set_DF002(msg_number);
+ Rtcm::set_DF009(gps_eph);
+ Rtcm::set_DF076(gps_eph);
+ Rtcm::set_DF077(gps_eph);
+ Rtcm::set_DF078(gps_eph);
+ Rtcm::set_DF079(gps_eph);
+ Rtcm::set_DF071(gps_eph);
+ Rtcm::set_DF081(gps_eph);
+ Rtcm::set_DF082(gps_eph);
+ Rtcm::set_DF083(gps_eph);
+ Rtcm::set_DF084(gps_eph);
+ Rtcm::set_DF085(gps_eph);
+ Rtcm::set_DF086(gps_eph);
+ Rtcm::set_DF087(gps_eph);
+ Rtcm::set_DF088(gps_eph);
+ Rtcm::set_DF089(gps_eph);
+ Rtcm::set_DF090(gps_eph);
+ Rtcm::set_DF091(gps_eph);
+ Rtcm::set_DF092(gps_eph);
+ Rtcm::set_DF093(gps_eph);
+ Rtcm::set_DF094(gps_eph);
+ Rtcm::set_DF095(gps_eph);
+ Rtcm::set_DF096(gps_eph);
+ Rtcm::set_DF097(gps_eph);
+ Rtcm::set_DF098(gps_eph);
+ Rtcm::set_DF099(gps_eph);
+ Rtcm::set_DF100(gps_eph);
+ Rtcm::set_DF101(gps_eph);
+ Rtcm::set_DF102(gps_eph);
+ Rtcm::set_DF103(gps_eph);
+ Rtcm::set_DF137(gps_eph);
+
+ std::string data;
+ data.clear();
+ data = DF002.to_string() +
+ DF009.to_string() +
+ DF076.to_string() +
+ DF077.to_string() +
+ DF078.to_string() +
+ DF079.to_string() +
+ DF071.to_string() +
+ DF081.to_string() +
+ DF082.to_string() +
+ DF083.to_string() +
+ DF084.to_string() +
+ DF085.to_string() +
+ DF086.to_string() +
+ DF087.to_string() +
+ DF088.to_string() +
+ DF089.to_string() +
+ DF090.to_string() +
+ DF091.to_string() +
+ DF092.to_string() +
+ DF093.to_string() +
+ DF094.to_string() +
+ DF095.to_string() +
+ DF096.to_string() +
+ DF097.to_string() +
+ DF098.to_string() +
+ DF099.to_string() +
+ DF100.to_string() +
+ DF101.to_string() +
+ DF102.to_string() +
+ DF103.to_string() +
+ DF137.to_string();
+
+ if (data.length() != 488)
+ {
+ LOG(WARNING) << "Bad-formatted RTCM MT1019 (488 bits expected, found " << data.length() << ")";
+ }
+
+ message1019_content = std::bitset<488>(data);
+ std::string message = build_message(data);
+ return message;
+}
+
+
+int Rtcm::read_MT1019(const std::string & message, Gps_Ephemeris & gps_eph)
+{
+ // Convert message to binary
+ std::string message_bin = Rtcm::hex_to_bin(message);
+
+ if(!Rtcm::check_CRC(message) )
+ {
+ LOG(WARNING) << " Bad CRC detected in RTCM message MT1019";
+ return 1;
+ }
+
+ unsigned int preamble_length = 8;
+ unsigned int reserved_field_length = 6;
+ unsigned int index = preamble_length + reserved_field_length;
+
+ unsigned int read_message_length = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+ index += 10;
+
+ if (read_message_length != 61)
+ {
+ LOG(WARNING) << " Message MT1019 seems too long (61 bytes expected, " << read_message_length << " received)";
+ return 1;
+ }
+
+ // Check than the message number is correct
+ unsigned int read_msg_number = Rtcm::bin_to_uint(message_bin.substr(index, 12));
+ index += 12;
+
+ if (1019 != read_msg_number)
+ {
+ LOG(WARNING) << " This is not a MT1019 message";
+ return 1;
+ }
+
+ // Fill Gps Ephemeris with message data content
+ gps_eph.i_satellite_PRN = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 6)));
+ index += 6;
+
+ gps_eph.i_GPS_week = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+ index += 10;
+
+ gps_eph.i_SV_accuracy = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 4)));
+ index += 4;
+
+ gps_eph.i_code_on_L2 = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 2)));
+ index += 2;
+
+ gps_eph.d_IDOT = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 14))) * I_DOT_LSB;
+ index += 14;
+
+ gps_eph.d_IODE_SF2 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 8)));
+ gps_eph.d_IODE_SF3 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 8)));
+ index += 8;
+
+ gps_eph.d_Toc = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 16))) * T_OC_LSB;
+ index += 16;
+
+ gps_eph.d_A_f2 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 8))) * A_F2_LSB;
+ index += 8;
+
+ gps_eph.d_A_f1 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * A_F1_LSB;
+ index += 16;
+
+ gps_eph.d_A_f0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 22))) * A_F0_LSB;
+ index += 22;
+
+ gps_eph.d_IODC = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+ index += 10;
+
+ gps_eph.d_Crs = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_RS_LSB;
+ index += 16;
+
+ gps_eph.d_Delta_n = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * DELTA_N_LSB;
+ index += 16;
+
+ gps_eph.d_M_0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * M_0_LSB;
+ index += 32;
+
+ gps_eph.d_Cuc = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_UC_LSB;
+ index += 16;
+
+ gps_eph.d_e_eccentricity = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 32))) * E_LSB;
+ index += 32;
+
+ gps_eph.d_Cus = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_US_LSB;
+ index += 16;
+
+ gps_eph.d_sqrt_A = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 32))) * SQRT_A_LSB;
+ index += 32;
+
+ gps_eph.d_Toe = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 16))) * T_OE_LSB;
+ index += 16;
+
+ gps_eph.d_Cic = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_IC_LSB;
+ index += 16;
+
+ gps_eph.d_OMEGA0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * OMEGA_0_LSB;
+ index += 32;
+
+ gps_eph.d_Cis = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_IS_LSB;
+ index += 16;
+
+ gps_eph.d_i_0 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * I_0_LSB;
+ index += 32;
+
+ gps_eph.d_Crc = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_RC_LSB;
+ index += 16;
+
+ gps_eph.d_OMEGA = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * OMEGA_LSB;
+ index += 32;
+
+ gps_eph.d_OMEGA_DOT = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 24))) * OMEGA_DOT_LSB;
+ index += 24;
+
+ gps_eph.d_TGD = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 8))) * T_GD_LSB;
+ index += 8;
+
+ gps_eph.i_SV_health = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 6)));
+ index += 6;
+
+ gps_eph.b_L2_P_data_flag = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+ index += 1;
+
+ gps_eph.b_fit_interval_flag = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+
+ return 0;
+}
+
+
+
+// **********************************************
+//
+// MESSAGE TYPE 1045 (GALILEO EPHEMERIS)
+//
+// **********************************************
+
+std::string Rtcm::print_MT1045(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned int msg_number = 1045;
+
+ Rtcm::set_DF002(msg_number);
+ Rtcm::set_DF252(gal_eph);
+ Rtcm::set_DF289(gal_eph);
+ Rtcm::set_DF290(gal_eph);
+ Rtcm::set_DF291(gal_eph);
+ Rtcm::set_DF293(gal_eph);
+ Rtcm::set_DF294(gal_eph);
+ Rtcm::set_DF295(gal_eph);
+ Rtcm::set_DF296(gal_eph);
+ Rtcm::set_DF297(gal_eph);
+ Rtcm::set_DF298(gal_eph);
+ Rtcm::set_DF299(gal_eph);
+ Rtcm::set_DF300(gal_eph);
+ Rtcm::set_DF301(gal_eph);
+ Rtcm::set_DF302(gal_eph);
+ Rtcm::set_DF303(gal_eph);
+ Rtcm::set_DF304(gal_eph);
+ Rtcm::set_DF305(gal_eph);
+ Rtcm::set_DF306(gal_eph);
+ Rtcm::set_DF307(gal_eph);
+ Rtcm::set_DF308(gal_eph);
+ Rtcm::set_DF309(gal_eph);
+ Rtcm::set_DF310(gal_eph);
+ Rtcm::set_DF311(gal_eph);
+ Rtcm::set_DF312(gal_eph);
+ Rtcm::set_DF314(gal_eph);
+ Rtcm::set_DF315(gal_eph);
+ unsigned int seven_zero = 0;
+ std::bitset<7> DF001_ = std::bitset<7>(seven_zero);
+
+ std::string data;
+ data.clear();
+ data = DF002.to_string() +
+ DF252.to_string() +
+ DF289.to_string() +
+ DF290.to_string() +
+ DF291.to_string() +
+ DF292.to_string() +
+ DF293.to_string() +
+ DF294.to_string() +
+ DF295.to_string() +
+ DF296.to_string() +
+ DF297.to_string() +
+ DF298.to_string() +
+ DF299.to_string() +
+ DF300.to_string() +
+ DF301.to_string() +
+ DF302.to_string() +
+ DF303.to_string() +
+ DF304.to_string() +
+ DF305.to_string() +
+ DF306.to_string() +
+ DF307.to_string() +
+ DF308.to_string() +
+ DF309.to_string() +
+ DF310.to_string() +
+ DF311.to_string() +
+ DF312.to_string() +
+ DF314.to_string() +
+ DF315.to_string() +
+ DF001_.to_string();
+
+ if (data.length() != 496)
+ {
+ LOG(WARNING) << "Bad-formatted RTCM MT1045 (496 bits expected, found " << data.length() << ")";
+ }
+ message1045_content = std::bitset<496>(data);
+ std::string message = build_message(data);
+ return message;
+}
+
+
+int Rtcm::read_MT1045(const std::string & message, Galileo_Ephemeris & gal_eph)
+{
+ // Convert message to binary
+ std::string message_bin = Rtcm::hex_to_bin(message);
+
+ if(!Rtcm::check_CRC(message) )
+ {
+ LOG(WARNING) << " Bad CRC detected in RTCM message MT1045";
+ return 1;
+ }
+
+ unsigned int preamble_length = 8;
+ unsigned int reserved_field_length = 6;
+ unsigned int index = preamble_length + reserved_field_length;
+
+ unsigned int read_message_length = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+ index += 10;
+
+ if (read_message_length != 62)
+ {
+ LOG(WARNING) << " Message MT1045 seems too long (62 bytes expected, " << read_message_length << " received)";
+ return 1;
+ }
+
+ // Check than the message number is correct
+ unsigned int read_msg_number = Rtcm::bin_to_uint(message_bin.substr(index, 12));
+ index += 12;
+
+ if (1045 != read_msg_number)
+ {
+ LOG(WARNING) << " This is not a MT1045 message";
+ return 1;
+ }
+
+ // Fill Galileo Ephemeris with message data content
+ gal_eph.i_satellite_PRN = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 6)));
+ index += 6;
+
+ gal_eph.WN_5 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 12)));
+ index += 12;
+
+ gal_eph.IOD_nav_1 = static_cast<int>(Rtcm::bin_to_uint(message_bin.substr(index, 10)));
+ index += 10;
+
+ gal_eph.SISA_3 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 8)));
+ index += 8;
+
+ gal_eph.iDot_2 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 14))) * iDot_2_LSB;
+ index += 14;
+
+ gal_eph.t0c_4 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 14))) * t0c_4_LSB;
+ index += 14;
+
+ gal_eph.af2_4 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 6))) * af2_4_LSB;
+ index += 6;
+
+ gal_eph.af1_4 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 21))) * af1_4_LSB;
+ index += 21;
+
+ gal_eph.af0_4 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 31))) * af0_4_LSB;
+ index += 31;
+
+ gal_eph.C_rs_3 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_rs_3_LSB;
+ index += 16;
+
+ gal_eph.delta_n_3 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * delta_n_3_LSB;
+ index += 16;
+
+ gal_eph.M0_1 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * M0_1_LSB;
+ index += 32;
+
+ gal_eph.C_uc_3 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_uc_3_LSB;
+ index += 16;
+
+ gal_eph.e_1 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 32))) * e_1_LSB;
+ index += 32;
+
+ gal_eph.C_us_3 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_us_3_LSB;
+ index += 16;
+
+ gal_eph.A_1 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 32))) * A_1_LSB_gal;
+ index += 32;
+
+ gal_eph.t0e_1 = static_cast<double>(Rtcm::bin_to_uint(message_bin.substr(index, 14))) * t0e_1_LSB;
+ index += 14;
+
+ gal_eph.C_ic_4 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_ic_4_LSB;
+ index += 16;
+
+ gal_eph.OMEGA_0_2 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * OMEGA_0_2_LSB;
+ index += 32;
+
+ gal_eph.C_is_4 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_is_4_LSB;
+ index += 16;
+
+ gal_eph.i_0_2 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * i_0_2_LSB;
+ index += 32;
+
+ gal_eph.C_rc_3 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 16))) * C_rc_3_LSB;
+ index += 16;
+
+ gal_eph.omega_2 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 32))) * omega_2_LSB;
+ index += 32;
+
+ gal_eph.OMEGA_dot_3 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 24))) * OMEGA_dot_3_LSB;
+ index += 24;
+
+ gal_eph.BGD_E1E5a_5 = static_cast<double>(Rtcm::bin_to_int(message_bin.substr(index, 10)));
+ index += 10;
+
+ gal_eph.E5a_HS = static_cast<unsigned int>(Rtcm::bin_to_uint(message_bin.substr(index, 2)));
+ index += 2;
+
+ gal_eph.E5a_DVS = static_cast<bool>(Rtcm::bin_to_uint(message_bin.substr(index, 1)));
+
+ return 0;
+}
+
+
++
++
++// **********************************************
++//
++// MESSAGE TYPE MSM1 (COMPACT PSEUDORANGES)
++//
++// **********************************************
++
++
++std::string Rtcm::print_MSM_1( const Gps_Ephemeris & gps_eph,
++ const Galileo_Ephemeris & gal_eph,
++ double obs_time,
++ const std::map<int, Gnss_Synchro> & pseudoranges,
++ unsigned int ref_id,
++ unsigned int clock_steering_indicator,
++ unsigned int external_clock_indicator,
++ int smooth_int,
++ bool sync_flag,
++ bool divergence_free,
++ bool more_messages)
++{
++ unsigned int msg_number = 1071; /// check for Galileo, it's 1091
++ std::string header = Rtcm::get_MSM_header(msg_number, gps_eph,
++ gal_eph,
++ obs_time,
++ pseudoranges,
++ ref_id,
++ clock_steering_indicator,
++ external_clock_indicator,
++ smooth_int,
++ sync_flag,
++ divergence_free,
++ more_messages);
++
++ std::string sat_data = Rtcm::get_MSM_1_content_sat_data(pseudoranges);
++ std::string signal_data = Rtcm::get_MSM_1_content_signal_data(pseudoranges);
++
++ std::string message = build_message(header + sat_data + signal_data);
++ return message;
++}
++
++std::string Rtcm::get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & gps_eph,
++ const Galileo_Ephemeris & gal_eph,
++ double obs_time,
++ const std::map<int, Gnss_Synchro> & pseudoranges,
++ unsigned int ref_id,
++ unsigned int clock_steering_indicator,
++ unsigned int external_clock_indicator,
++ int smooth_int,
++ bool sync_flag,
++ bool divergence_free,
++ bool more_messages)
++{
++ Rtcm::set_DF002(msg_number);
++ Rtcm::set_DF003(ref_id);
++ if(gps_eph.i_satellite_PRN != 0)
++ {
++ Rtcm::set_DF004(gps_eph, obs_time);
++ }
++ else
++ {
++ Rtcm::set_DF248(gal_eph, obs_time);
++ }
++ Rtcm::set_DF393(more_messages);
++ Rtcm::set_DF409(0); // Issue of Data Station. 0: not utilized
++ std::bitset<7> DF001_ = std::bitset<7>("0000000");
++ Rtcm::set_DF411(clock_steering_indicator);
++ Rtcm::set_DF412(external_clock_indicator);
++ Rtcm::set_DF417(divergence_free);
++ Rtcm::set_DF418(smooth_int);
++ Rtcm::set_DF394(pseudoranges);
++ Rtcm::set_DF395(pseudoranges);
++
++ std::string header = DF002.to_string() + DF003.to_string();
++ if(gps_eph.i_satellite_PRN != 0)
++ {
++ header += DF004.to_string();
++ }
++ else
++ {
++ header += DF248.to_string();
++ }
++ header = header + DF393.to_string() +
++ DF409.to_string() +
++ DF001_.to_string() +
++ DF411.to_string() +
++ DF412.to_string() +
++ DF418.to_string() +
++ DF394.to_string() +
++ DF395.to_string() +
++ Rtcm::set_DF396(pseudoranges);
++
++ return header;
++}
++
++std::string Rtcm::get_MSM_1_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges)
++{
++ std::string sat_data;
++ sat_data.clear();
++
++ Rtcm::set_DF394(pseudoranges);
++ unsigned int num_satellites = DF394.count();
++
++ std::map<int, Gnss_Synchro> pseudoranges_ordered;
++
++ std::map<int, Gnss_Synchro>::const_iterator gnss_synchro_iter;
++ std::vector<unsigned int> pos;
++ std::vector<unsigned int>::iterator it;
++
++ for(gnss_synchro_iter = pseudoranges.begin();
++ gnss_synchro_iter != pseudoranges.end();
++ gnss_synchro_iter++)
++ {
++ pseudoranges_ordered.insert(std::pair<int, Gnss_Synchro>(65 - gnss_synchro_iter->second.PRN, gnss_synchro_iter->second));
++
++ it = std::find (pos.begin(), pos.end(), 65 - gnss_synchro_iter->second.PRN);
++ if (it == pos.end())
++ {
++ pos.push_back(65 - gnss_synchro_iter->second.PRN);
++ }
++ }
++
++ std::sort(pos.begin(), pos.end());
++ std::reverse(pos.begin(), pos.end());
++
++ for(unsigned int Nsat = 1; Nsat < num_satellites; Nsat++)
++ {
++ Rtcm::set_DF398(pseudoranges_ordered.at( pos.at(Nsat-1) ));
++ sat_data += DF398.to_string();
++ }
++
++ return sat_data;
++}
++
++std::string Rtcm::get_MSM_1_content_signal_data(const std::map<int, Gnss_Synchro> & pseudoranges)
++{
++ std::string s("Not implemented");
++ return s;
++}
++
++std::string Rtcm::get_MSM_4_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges)
++{
++ //std::map<int, Gnss_Synchro>::const_iterator gnss_synchro_iter;
++ std::string data("Not implemented");
++ return data;
++}
++
++//std::string Rtcm::get_MSM_1_content(const std::map<int, Gnss_Synchro> & pseudoranges)
++//{
++// std::string DF396 = set_DF396(pseudoranges);
++//}
++
++
+// *****************************************************************************************************
+//
+// DATA FIELDS AS DEFINED AT RTCM STANDARD 10403.2
+//
+// *****************************************************************************************************
+
+int Rtcm::reset_data_fields()
+{
+ //DF001.reset();
+ DF002.reset();
+ DF003.reset();
+ DF004.reset();
+ DF005.reset();
+ DF006.reset();
+ DF007.reset();
+ DF008.reset();
+ DF009.reset();
+ DF010.reset();
+ DF011.reset();
+ DF012.reset();
+ DF013.reset();
+ DF014.reset();
+ DF015.reset();
+
+ // Contents of GPS Satellite Ephemeris Data, Message Type 1019
+ DF071.reset();
+ DF076.reset();
+ DF077.reset();
+ DF078.reset();
+ DF079.reset();
+ DF081.reset();
+ DF082.reset();
+ DF083.reset();
+ DF084.reset();
+ DF085.reset();
+ DF086.reset();
+ DF087.reset();
+
+ DF088.reset();
+ DF089.reset();
+ DF090.reset();
+ DF091.reset();
+ DF092.reset();
+ DF093.reset();
+ DF094.reset();
+ DF095.reset();
+ DF096.reset();
+ DF097.reset();
+ DF098.reset();
+ DF099.reset();
+ DF100.reset();
+ DF101.reset();
+ DF102.reset();
+ DF103.reset();
+ DF137.reset();
+
++ DF248.reset();
++
+ // Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045
+ DF252.reset();
+ DF289.reset();
+ DF290.reset();
+ DF291.reset();
+ DF292.reset();
+ DF293.reset();
+ DF294.reset();
+ DF295.reset();
+ DF296.reset();
+ DF297.reset();
+ DF298.reset();
+ DF299.reset();
+ DF300.reset();
+ DF301.reset();
+ DF302.reset();
+ DF303.reset();
+ DF304.reset();
+ DF305.reset();
+ DF306.reset();
+ DF307.reset();
+ DF308.reset();
+ DF309.reset();
+ DF310.reset();
+ DF311.reset();
+ DF312.reset();
+ DF314.reset();
+ DF315.reset();
+
+ DF364.reset();
+
+ DF393.reset();
+ DF394.reset();
+ DF395.reset();
+
++ DF397.reset();
++ DF398.reset();
++ DF399.reset();
++
+ DF409.reset();
+
+ DF411.reset();
+ DF412.reset();
+ DF417.reset();
+ DF418.reset();
+
+ return 0;
+}
+
+
+int Rtcm::set_DF002(unsigned int message_number)
+{
+ if (message_number > 4095)
+ {
+ LOG(WARNING) << "RTCM message number must be between 0 and 4095, but it has been set to " << message_number;
+ }
+ DF002 = std::bitset<12>(message_number);
+ return 0;
+}
+
+
+int Rtcm::set_DF003(unsigned int ref_station_ID)
+{
+ //unsigned int station_ID = ref_station_ID;
+ if (ref_station_ID > 4095)
+ {
+ LOG(WARNING) << "RTCM reference station ID must be between 0 and 4095, but it has been set to " << ref_station_ID;
+ }
+ DF003 = std::bitset<12>(ref_station_ID);
+ return 0;
+}
+
+
+int Rtcm::set_DF004(const Gps_Ephemeris & gps_eph, double obs_time)
+{
+ // TOW in milliseconds from the beginning of the GPS week, measured in GPS time
+ unsigned long int tow = static_cast<unsigned long int>(std::round((obs_time + 604800 * static_cast<double>(gps_eph.i_GPS_week % 1024)) * 1000));
+ if(tow > 604799999)
+ {
+ LOG(WARNING) << "To large TOW! Set to the last millisecond of the week";
+ tow = 604799999;
+ }
+ DF004 = std::bitset<30>(tow);
+ return 0;
+}
+
+
+int Rtcm::set_DF005(bool sync_flag)
+{
+ // 0 - No further GNSS observables referenced to the same Epoch Time will be transmitted. This enables the receiver to begin processing
+ // the data immediately after decoding the message.
+ // 1 - The next message will contain observables of another GNSS source referenced to the same Epoch Time.
+ DF005 = std::bitset<1>(sync_flag);
+ return 0;
+}
+
+
+int Rtcm::set_DF006(const std::map<int, Gnss_Synchro> & pseudoranges)
+{
+ //Number of satellites observed in current epoch
+ unsigned short int nsats = 0;
+ std::map<int, Gnss_Synchro>::const_iterator pseudoranges_iter;
+ for(pseudoranges_iter = pseudoranges.begin();
+ pseudoranges_iter != pseudoranges.end();
+ pseudoranges_iter++)
+ {
+ nsats++;
+ }
+ if (nsats > 31)
+ {
+ LOG(WARNING) << "The number of processed GPS satellites must be between 0 and 31, but it seems that you are processing " << nsats;
+ nsats = 31;
+ }
+ DF006 = std::bitset<5>(nsats);
+ return 0;
+}
+
+
+int Rtcm::set_DF007(bool divergence_free_smoothing_indicator)
+{
+ // 0 - Divergence-free smoothing not used 1 - Divergence-free smoothing used
+ DF007 = std::bitset<1>(divergence_free_smoothing_indicator);
+ return 0;
+}
+
+
+int Rtcm::set_DF008(short int smoothing_interval)
+{
+ DF008 = std::bitset<3>(smoothing_interval);
+ return 0;
+}
+
+
+int Rtcm::set_DF009(const Gnss_Synchro & gnss_synchro)
+{
+ unsigned int prn_ = gnss_synchro.PRN;
+ if(prn_ > 31)
+ {
+ LOG(WARNING) << "GPS satellite ID must be between 0 and 31, but PRN " << prn_ << " was found";
+ }
+ DF009 = std::bitset<6>(prn_);
+ return 0;
+}
+
+
+int Rtcm::set_DF009(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int prn_ = gps_eph.i_satellite_PRN;
+ if(prn_ > 31)
+ {
+ LOG(WARNING) << "GPS satellite ID must be between 0 and 31, but PRN " << prn_ << " was found";
+ }
+ DF009 = std::bitset<6>(prn_);
+ return 0;
+}
+
+
+int Rtcm::set_DF010(bool code_indicator)
+{
+ DF010 = std::bitset<1>(code_indicator);
+ return 0;
+}
+
+
+int Rtcm::set_DF011(const Gnss_Synchro & gnss_synchro)
+{
+ double ambiguity = std::floor( gnss_synchro.Pseudorange_m / 299792.458 );
+ unsigned long int gps_L1_pseudorange = static_cast<unsigned long int>(std::round(( gnss_synchro.Pseudorange_m - ambiguity * 299792.458) / 0.02 ));
+ DF011 = std::bitset<24>(gps_L1_pseudorange);
+ return 0;
+}
+
+
+int Rtcm::set_DF012(const Gnss_Synchro & gnss_synchro)
+{
+ const double lambda = GPS_C_m_s / GPS_L1_FREQ_HZ;
+ double L1_pseudorange = gnss_synchro.Pseudorange_m;
+ double ambiguity = std::floor( gnss_synchro.Pseudorange_m / 299792.458 );
+ double gps_L1_pseudorange = std::round(( gnss_synchro.Pseudorange_m - ambiguity * 299792.458) / 0.02 );
+ double gps_L1_pseudorange_c = static_cast<double>(gps_L1_pseudorange) * 0.02 + ambiguity * 299792.458;
+ double L1_phaserange_c = gnss_synchro.Carrier_phase_rads * GPS_TWO_PI;
+ double L1_phaserange_c_r = std::fmod(L1_phaserange_c - gps_L1_pseudorange_c / lambda + 1500.0, 3000.0) - 1500.0;
+ long int gps_L1_phaserange_minus_L1_pseudorange = static_cast<long int>(std::round(L1_phaserange_c_r * lambda / 0.0005 ));
+ DF012 = std::bitset<20>(gps_L1_phaserange_minus_L1_pseudorange);
+ return 0;
+}
+
+
+int Rtcm::set_DF014(const Gnss_Synchro & gnss_synchro)
+{
+ unsigned int gps_L1_pseudorange_ambiguity = static_cast<unsigned int>(std::floor(gnss_synchro.Pseudorange_m / 299792.458));
+ DF014 = std::bitset<8>(gps_L1_pseudorange_ambiguity);
+ return 0;
+}
+
+
+int Rtcm::set_DF015(const Gnss_Synchro & gnss_synchro)
+{
+ double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz;
+ if (CN0_dB_Hz_est > 63.75)
+ {
+ CN0_dB_Hz_est = 63.75;
+ }
+ unsigned int CN0_dB_Hz = static_cast<unsigned int>(std::round(CN0_dB_Hz_est / 0.25 ));
+ DF015 = std::bitset<8>(CN0_dB_Hz);
+ return 0;
+}
+
+
+int Rtcm::set_DF021()
+{
+ unsigned short int itfr_year = 0;
+ DF021 = std::bitset<6>(itfr_year);
+ return 0;
+}
+
+
+int Rtcm::set_DF022(bool gps_indicator)
+{
+ DF022 = std::bitset<1>(gps_indicator);
+ return 0;
+}
+
+
+int Rtcm::set_DF023(bool glonass_indicator)
+{
+ DF023 = std::bitset<1>(glonass_indicator);
+ return 0;
+}
+
+
+int Rtcm::set_DF024(bool galileo_indicator)
+{
+ DF024 = std::bitset<1>(galileo_indicator);
+ return 0;
+}
+
+
+int Rtcm::set_DF025(double antenna_ECEF_X_m)
+{
+ long long int ant_ref_x = static_cast<long long int>(std::round( antenna_ECEF_X_m * 10000));
+ DF025 = std::bitset<38>(ant_ref_x);
+ return 0;
+}
+
+
+int Rtcm::set_DF026(double antenna_ECEF_Y_m)
+{
+ long long int ant_ref_y = static_cast<long long int>(std::round( antenna_ECEF_Y_m * 10000));
+ DF026 = std::bitset<38>(ant_ref_y);
+ return 0;
+}
+
+
+int Rtcm::set_DF027(double antenna_ECEF_Z_m)
+{
+ long long int ant_ref_z = static_cast<long long int>(std::round( antenna_ECEF_Z_m * 10000));
+ DF027 = std::bitset<38>(ant_ref_z);
+ return 0;
+}
+
+
+int Rtcm::set_DF071(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int iode = static_cast<unsigned int>(gps_eph.d_IODE_SF2);
+ DF071 = std::bitset<8>(iode);
+ return 0;
+}
+
+
+int Rtcm::set_DF076(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int week_number = static_cast<unsigned int>(gps_eph.i_GPS_week);
+ DF076 = std::bitset<10>(week_number);
+ return 0;
+}
+
+
+int Rtcm::set_DF077(const Gps_Ephemeris & gps_eph)
+{
+ unsigned short int ura = static_cast<unsigned short int>(gps_eph.i_SV_accuracy);
+ DF077 = std::bitset<4>(ura);
+ return 0;
+}
+
+
+int Rtcm::set_DF078(const Gps_Ephemeris & gps_eph)
+{
+ unsigned short int code_on_L2 = static_cast<unsigned short int>(gps_eph.i_code_on_L2);
+ DF078 = std::bitset<2>(code_on_L2);
+ return 0;
+}
+
+
+int Rtcm::set_DF079(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int idot = static_cast<unsigned int>(std::round(gps_eph.d_IDOT / I_DOT_LSB ));
+ DF079 = std::bitset<14>(idot);
+ return 0;
+}
+
+
+int Rtcm::set_DF080(const Gps_Ephemeris & gps_eph)
+{
+ unsigned short int iode = static_cast<unsigned short int>(gps_eph.d_IODE_SF2);
+ DF080 = std::bitset<8>(iode);
+ return 0;
+}
+
+
+int Rtcm::set_DF081(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int toc = static_cast<unsigned int>(std::round(gps_eph.d_Toc / T_OC_LSB ));
+ DF081 = std::bitset<16>(toc);
+ return 0;
+}
+
+
+int Rtcm::set_DF082(const Gps_Ephemeris & gps_eph)
+{
+ short int af2 = static_cast<short int>(std::round(gps_eph.d_A_f2 / A_F2_LSB ));
+ DF082 = std::bitset<8>(af2);
+ return 0;
+}
+
+
+int Rtcm::set_DF083(const Gps_Ephemeris & gps_eph)
+{
+ int af1 = static_cast<int>(std::round(gps_eph.d_A_f1 / A_F1_LSB ));
+ DF083 = std::bitset<16>(af1);
+ return 0;
+}
+
+
+int Rtcm::set_DF084(const Gps_Ephemeris & gps_eph)
+{
+ long int af0 = static_cast<long int>(std::round(gps_eph.d_A_f0 / A_F0_LSB ));
+ DF084 = std::bitset<22>(af0);
+ return 0;
+}
+
+
+int Rtcm::set_DF085(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int iodc = static_cast<unsigned int>(gps_eph.d_IODC);
+ DF085 = std::bitset<10>(iodc);
+ return 0;
+}
+
+
+int Rtcm::set_DF086(const Gps_Ephemeris & gps_eph)
+{
+ int crs = static_cast<int>(std::round(gps_eph.d_Crs / C_RS_LSB ));
+ DF086 = std::bitset<16>(crs);
+ return 0;
+}
+
+
+int Rtcm::set_DF087(const Gps_Ephemeris & gps_eph)
+{
+ int delta_n = static_cast<int>(std::round(gps_eph.d_Delta_n / DELTA_N_LSB ));
+ DF087 = std::bitset<16>(delta_n);
+ return 0;
+}
+
+
+int Rtcm::set_DF088(const Gps_Ephemeris & gps_eph)
+{
+ long int m0 = static_cast<long int>(std::round(gps_eph.d_M_0 / M_0_LSB ));
+ DF088 = std::bitset<32>(m0);
+ return 0;
+}
+
+
+int Rtcm::set_DF089(const Gps_Ephemeris & gps_eph)
+{
+ int cuc = static_cast<int>(std::round(gps_eph.d_Cuc / C_UC_LSB ));
+ DF089 = std::bitset<16>(cuc);
+ return 0;
+}
+
+int Rtcm::set_DF090(const Gps_Ephemeris & gps_eph)
+{
+ unsigned long int ecc = static_cast<unsigned long int>(std::round(gps_eph.d_e_eccentricity / E_LSB ));
+ DF090 = std::bitset<32>(ecc);
+ return 0;
+}
+
+
+int Rtcm::set_DF091(const Gps_Ephemeris & gps_eph)
+{
+ int cus = static_cast<int>(std::round(gps_eph.d_Cus / C_US_LSB ));
+ DF091 = std::bitset<16>(cus);
+ return 0;
+}
+
+
+int Rtcm::set_DF092(const Gps_Ephemeris & gps_eph)
+{
+ unsigned long int sqr_a = static_cast<unsigned long int>(std::round(gps_eph.d_sqrt_A / SQRT_A_LSB ));
+ DF092 = std::bitset<32>(sqr_a);
+ return 0;
+}
+
+
+int Rtcm::set_DF093(const Gps_Ephemeris & gps_eph)
+{
+ unsigned int toe = static_cast<unsigned int>(std::round(gps_eph.d_Toe / T_OE_LSB ));
+ DF093 = std::bitset<16>(toe);
+ return 0;
+}
+
+
+int Rtcm::set_DF094(const Gps_Ephemeris & gps_eph)
+{
+ int cic = static_cast<int>(std::round(gps_eph.d_Cic / C_IC_LSB ));
+ DF094 = std::bitset<16>(cic);
+ return 0;
+}
+
+
+int Rtcm::set_DF095(const Gps_Ephemeris & gps_eph)
+{
+ long int Omega0 = static_cast<long int>(std::round(gps_eph.d_OMEGA0 / OMEGA_0_LSB ));
+ DF095 = std::bitset<32>(Omega0);
+ return 0;
+}
+
+
+int Rtcm::set_DF096(const Gps_Ephemeris & gps_eph)
+{
+ int cis = static_cast<int>(std::round(gps_eph.d_Cis / C_IS_LSB ));
+ DF096 = std::bitset<16>(cis);
+ return 0;
+}
+
+
+int Rtcm::set_DF097(const Gps_Ephemeris & gps_eph)
+{
+ long int i0 = static_cast<long int>(std::round(gps_eph.d_i_0 / I_0_LSB ));
+ DF097 = std::bitset<32>(i0);
+ return 0;
+}
+
+
+int Rtcm::set_DF098(const Gps_Ephemeris & gps_eph)
+{
+ int crc = static_cast<int>(std::round(gps_eph.d_Crc / C_RC_LSB ));
+ DF098 = std::bitset<16>(crc);
+ return 0;
+}
+
+
+int Rtcm::set_DF099(const Gps_Ephemeris & gps_eph)
+{
+ long int omega = static_cast<long int>(std::round(gps_eph.d_OMEGA / OMEGA_LSB ));
+ DF099 = std::bitset<32>(omega);
+ return 0;
+}
+
+
+int Rtcm::set_DF100(const Gps_Ephemeris & gps_eph)
+{
+ long int omegadot = static_cast<long int>(std::round(gps_eph.d_OMEGA_DOT / OMEGA_DOT_LSB ));
+ DF100 = std::bitset<24>(omegadot);
+ return 0;
+}
+
+
+int Rtcm::set_DF101(const Gps_Ephemeris & gps_eph)
+{
+ short int tgd = static_cast<short int>(std::round(gps_eph.d_TGD / T_GD_LSB ));
+ DF101 = std::bitset<8>(tgd);
+ return 0;
+}
+
+
+int Rtcm::set_DF102(const Gps_Ephemeris & gps_eph)
+{
+ unsigned short int sv_heath = static_cast<unsigned short int>(gps_eph.i_SV_health);
+ DF102 = std::bitset<6>(sv_heath);
+ return 0;
+}
+
+
+int Rtcm::set_DF103(const Gps_Ephemeris & gps_eph)
+{
+ DF103 = std::bitset<1>(gps_eph.b_L2_P_data_flag);
+ return 0;
+}
+
+
+
+
+int Rtcm::set_DF137(const Gps_Ephemeris & gps_eph)
+{
+ DF137 = std::bitset<1>(gps_eph.b_fit_interval_flag);
+ return 0;
+}
+
+
++int Rtcm::set_DF248(const Galileo_Ephemeris & gal_eph, double obs_time)
++{
++ // TOW in milliseconds from the beginning of the Galileo week, measured in Galileo time
++ unsigned long int tow = static_cast<unsigned long int>(std::round((obs_time + 604800 * static_cast<double>(gal_eph.WN_5)) * 1000));
++ if(tow > 604799999)
++ {
++ LOG(WARNING) << "To large TOW! Set to the last millisecond of the week";
++ tow = 604799999;
++ }
++ DF248 = std::bitset<30>(tow);
++ return 0;
++}
++
+
+int Rtcm::set_DF252(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned int prn_ = gal_eph.i_satellite_PRN;
+ if(prn_ > 63)
+ {
+ LOG(WARNING) << "Galileo satellite ID must be between 0 and 63, but PRN " << prn_ << " was found";
+ }
+ DF252 = std::bitset<6>(prn_);
+ return 0;
+}
+
+
+int Rtcm::set_DF289(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned int galileo_week_number = static_cast<unsigned int>(gal_eph.WN_5);
+ if(galileo_week_number > 4095)
+ {
+ LOG(WARNING) << "Error decoding Galileo week number (it has a 4096 roll-off, but " << galileo_week_number << " was detected)";
+ }
+ DF289 = std::bitset<12>(galileo_week_number);
+ return 0;
+}
+
+
+int Rtcm::set_DF290(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned int iod_nav = static_cast<unsigned int>(gal_eph.IOD_nav_1);
+ if(iod_nav > 1023)
+ {
+ LOG(WARNING) << "Error decoding Galileo IODnav (it has a max of 1023, but " << iod_nav << " was detected)";
+ }
+ DF290 = std::bitset<10>(iod_nav);
+ return 0;
+}
+
+
+int Rtcm::set_DF291(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned short int SISA = static_cast<unsigned short int>(gal_eph.SISA_3);
+ //SISA = 0; // SIS Accuracy, data content definition not given in Galileo OS SIS ICD, Issue 1.1, Sept 2010
+ DF291 = std::bitset<8>(SISA);
+ return 0;
+}
+
+
+int Rtcm::set_DF292(const Galileo_Ephemeris & gal_eph)
+{
+
+ int idot = static_cast<int>(std::round(gal_eph.iDot_2 / FNAV_idot_2_LSB));
+ DF292 = std::bitset<14>(idot);
+ return 0;
+}
+
+int Rtcm::set_DF293(const Galileo_Ephemeris & gal_eph)
+{
+
+ unsigned int toc = static_cast<unsigned int>(gal_eph.t0c_4);
+ if(toc > 604740)
+ {
+ LOG(WARNING) << "Error decoding Galileo ephemeris time (max of 604740, but " << toc << " was detected)";
+ }
+ DF293 = std::bitset<14>(toc);
+ return 0;
+}
+
+
+int Rtcm::set_DF294(const Galileo_Ephemeris & gal_eph)
+{
+ short int af2 = static_cast<short int>(std::round(gal_eph.af2_4 / FNAV_af2_1_LSB));
+ DF294 = std::bitset<6>(af2);
+ return 0;
+}
+
+
+int Rtcm::set_DF295(const Galileo_Ephemeris & gal_eph)
+{
+ long int af1 = static_cast<long int>(std::round(gal_eph.af1_4 / FNAV_af1_1_LSB));
+ DF295 = std::bitset<21>(af1);
+ return 0;
+}
+
+
+int Rtcm::set_DF296(const Galileo_Ephemeris & gal_eph)
+{
+ long int af0 = static_cast<unsigned int>(std::round(gal_eph.af0_4 / FNAV_af0_1_LSB));
+ DF296 = std::bitset<31>(af0);
+ return 0;
+}
+
+
+int Rtcm::set_DF297(const Galileo_Ephemeris & gal_eph)
+{
+ int crs = static_cast<int>(std::round(gal_eph.C_rs_3 / FNAV_Crs_3_LSB));
+ DF297 = std::bitset<16>(crs);
+ return 0;
+}
+
+
+int Rtcm::set_DF298(const Galileo_Ephemeris & gal_eph)
+{
+ int delta_n = static_cast<int>(std::round(gal_eph.delta_n_3 / FNAV_deltan_3_LSB));
+ DF298 = std::bitset<16>(delta_n);
+ return 0;
+}
+
+
+int Rtcm::set_DF299(const Galileo_Ephemeris & gal_eph)
+{
+ long int m0 = static_cast<long int>(std::round(gal_eph.M0_1 / FNAV_M0_2_LSB));
+ DF299 = std::bitset<32>(m0);
+ return 0;
+}
+
+
+int Rtcm::set_DF300(const Galileo_Ephemeris & gal_eph)
+{
+ int cuc = static_cast<unsigned int>(std::round(gal_eph.C_uc_3 / FNAV_Cuc_3_LSB));
+ DF300 = std::bitset<16>(cuc);
+ return 0;
+}
+
+int Rtcm::set_DF301(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned long int ecc = static_cast<unsigned long int>(std::round(gal_eph.e_1 / FNAV_e_2_LSB));
+ DF301 = std::bitset<32>(ecc);
+ return 0;
+}
+
+
+int Rtcm::set_DF302(const Galileo_Ephemeris & gal_eph)
+{
+ int cus = static_cast<int>(std::round(gal_eph.C_us_3 / FNAV_Cus_3_LSB));
+ DF302 = std::bitset<16>(cus);
+ return 0;
+}
+
+
+int Rtcm::set_DF303(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned long int sqr_a = static_cast<unsigned long int>(std::round(gal_eph.A_1 / FNAV_a12_2_LSB));
+ DF303 = std::bitset<32>(sqr_a);
+ return 0;
+}
+
+
+
+int Rtcm::set_DF304(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned int toe = static_cast<unsigned int>(std::round(gal_eph.t0e_1 / FNAV_t0e_3_LSB));
+ DF304 = std::bitset<14>(toe);
+ return 0;
+}
+
+
+int Rtcm::set_DF305(const Galileo_Ephemeris & gal_eph)
+{
+ int cic = static_cast<int>(std::round(gal_eph.C_ic_4 / FNAV_Cic_4_LSB));
+ DF305 = std::bitset<16>(cic);
+ return 0;
+}
+
+
+int Rtcm::set_DF306(const Galileo_Ephemeris & gal_eph)
+{
+ long int Omega0 = static_cast<long int>(std::round(gal_eph.OMEGA_0_2 / FNAV_omega0_2_LSB));
+ DF306 = std::bitset<32>(Omega0);
+ return 0;
+}
+
+
+int Rtcm::set_DF307(const Galileo_Ephemeris & gal_eph)
+{
+ int cis = static_cast<int>(std::round(gal_eph.C_is_4 / FNAV_Cis_4_LSB));
+ DF307 = std::bitset<16>(cis);
+ return 0;
+}
+
+
+int Rtcm::set_DF308(const Galileo_Ephemeris & gal_eph)
+{
+ long int i0 = static_cast<long int>(std::round(gal_eph.i_0_2 / FNAV_i0_3_LSB));
+ DF308 = std::bitset<32>(i0);
+ return 0;
+}
+
+
+int Rtcm::set_DF309(const Galileo_Ephemeris & gal_eph)
+{
+ int crc = static_cast<unsigned int>(std::round(gal_eph.C_rc_3 / FNAV_Crc_3_LSB));
+ DF309 = std::bitset<16>(crc);
+ return 0;
+}
+
+
+int Rtcm::set_DF310(const Galileo_Ephemeris & gal_eph)
+{
+ int omega = static_cast<int>(std::round(gal_eph.omega_2 / FNAV_omega0_2_LSB));
+ DF310 = std::bitset<32>(omega);
+ return 0;
+}
+
+
+int Rtcm::set_DF311(const Galileo_Ephemeris & gal_eph)
+{
+ long int Omegadot = static_cast<long int>(std::round(gal_eph.OMEGA_dot_3 / FNAV_omegadot_2_LSB));
+ DF311 = std::bitset<24>(Omegadot);
+ return 0;
+}
+
+
+int Rtcm::set_DF312(const Galileo_Ephemeris & gal_eph)
+{
+ int bdg_E1_E5a = static_cast<int>(std::round(gal_eph.BGD_E1E5a_5 / FNAV_BGD_1_LSB));
+ DF312 = std::bitset<10>(bdg_E1_E5a);
+ return 0;
+}
+
+
+int Rtcm::set_DF313(const Galileo_Ephemeris & gal_eph)
+{
+ unsigned int bdg_E5b_E1 = static_cast<unsigned int>(std::round(gal_eph.BGD_E1E5b_5 ));
+ //bdg_E5b_E1 = 0; //reserved
+ DF313 = std::bitset<10>(bdg_E5b_E1);
+ return 0;
+}
+
+
+int Rtcm::set_DF314(const Galileo_Ephemeris & gal_eph)
+{
+ DF314 = std::bitset<2>(gal_eph.E5a_HS);
+ return 0;
+}
+
+
+int Rtcm::set_DF315(const Galileo_Ephemeris & gal_eph)
+{
+ DF315 = std::bitset<1>(gal_eph.E5a_DVS);
+ return 0;
+}
+
+
+
+int Rtcm::set_DF393(bool more_messages)
+{
+ DF393 = std::bitset<1>(more_messages);
+ return 0;
+}
+
+
+int Rtcm::set_DF394(const std::map<int, Gnss_Synchro> & gnss_synchro)
+{
+ DF394.reset();
+ std::map<int, Gnss_Synchro>::const_iterator gnss_synchro_iter;
+ unsigned int mask_position;
+ for(gnss_synchro_iter = gnss_synchro.begin();
+ gnss_synchro_iter != gnss_synchro.end();
+ gnss_synchro_iter++)
+ {
+ mask_position = 65 - gnss_synchro_iter->second.PRN;
+ DF394.set(mask_position, true);
+ }
+ return 0;
+}
+
+
+int Rtcm::set_DF395(const std::map<int, Gnss_Synchro> & gnss_synchro)
+{
+ DF395.reset();
+ std::map<int, Gnss_Synchro>::const_iterator gnss_synchro_iter;
+ std::string sig;
+ unsigned int mask_position;
+ for(gnss_synchro_iter = gnss_synchro.begin();
+ gnss_synchro_iter != gnss_synchro.end();
+ gnss_synchro_iter++)
+ {
+ std::string sig_(gnss_synchro_iter->second.Signal);
+ sig = sig_.substr(0,2);
+
+ std::string sys(gnss_synchro_iter->second.System, 1);
+
+ if ((sig.compare("1C") == 0) && (sys.compare("G") == 0 ) )
+ {
+ mask_position = 33 - 2;
+ DF395.set(mask_position, true);
+ }
+ if ((sig.compare("2S") == 0) && (sys.compare("G") == 0 ) )
+ {
+ mask_position = 33 - 15;
+ DF395.set(mask_position, true);
+ }
+
+ if ((sig.compare("5X") == 0) && (sys.compare("G") == 0 ) )
+ {
+ mask_position = 33 - 24;
+ DF395.set(mask_position, true);
+ }
+ if ((sig.compare("1B") == 0) && (sys.compare("E") == 0 ) )
+ {
+ mask_position = 33 - 4;
+ DF395.set(mask_position, true);
+ }
+
+ if ((sig.compare("5X") == 0) && (sys.compare("E") == 0 ) )
+ {
+ mask_position = 33 - 24;
+ DF395.set(mask_position, true);
+ }
+ if ((sig.compare("7X") == 0) && (sys.compare("E") == 0 ) )
+ {
+ mask_position = 33 - 16;
+ DF395.set(mask_position, true);
+ }
+ }
+ return 0;
+}
+
++std::string Rtcm::set_DF396(const std::map<int, Gnss_Synchro> & pseudoranges)
++{
++ std::string DF396;
++ std::map<int, Gnss_Synchro>::const_iterator pseudoranges_iter;
++ Rtcm::set_DF394(pseudoranges);
++ Rtcm::set_DF395(pseudoranges);
++ unsigned int num_signals = DF395.count();
++ unsigned int num_satellites = DF394.count();
++ std::vector<std::vector<bool> > matrix( num_signals, std::vector<bool>(num_satellites) );
++
++ unsigned int element = 0;
++ std::string sig;
++ // fill matrix
++ for(unsigned int row = 0; row < num_signals - 1; row++)
++ {
++ for (unsigned int signal_id = 1; signal_id < 32; signal_id++)
++ {
++ bool we_have_signal_for_this_sat = false;
++
++ if(static_cast<bool>(DF395.test(signal_id)))
++ {
++ for(pseudoranges_iter = pseudoranges.begin();
++ pseudoranges_iter != pseudoranges.end();
++ pseudoranges_iter++)
++ {
++ std::string sig_(pseudoranges_iter->second.Signal);
++ sig = sig_.substr(0,2);
++ std::string sys(pseudoranges_iter->second.System, 1);
++ bool this_sat = static_cast<bool>(DF394.test(65 - pseudoranges_iter->second.PRN));
++ if (this_sat)
++ {
++ if( (signal_id == 2) && (sig.compare("1C") == 0) && (sys.compare("G") == 0 ) )
++ {
++ we_have_signal_for_this_sat = true;
++ }
++
++ if( (signal_id == 4) && (sig.compare("1B") == 0) && (sys.compare("E") == 0 ) )
++ {
++ we_have_signal_for_this_sat = true;
++ }
++
++ if( (signal_id == 15) && (sig.compare("2S") == 0) && (sys.compare("G") == 0 ) )
++ {
++ we_have_signal_for_this_sat = true;
++ }
++
++ if( (signal_id == 24) && (sig.compare("5X") == 0) && (sys.compare("E") == 0 ) )
++ {
++ we_have_signal_for_this_sat = true;
++ }
++
++ matrix[row].push_back(we_have_signal_for_this_sat);
++ }
++ }
++ }
++
++ }
++ }
++
++ // write matrix
++ DF396.clear();
++ std::stringstream ss;
++ for(unsigned int row = 0; row < num_signals - 1; row++)
++ {
++ for(unsigned int col = 0; col < num_satellites - 1; col++)
++ {
++ ss << std::boolalpha << matrix[row].at(col);
++ DF396 += ss.str();
++ }
++ }
++
++ return DF396;
++}
++
++
++
++int Rtcm::set_DF397(const Gnss_Synchro & gnss_synchro)
++{
++ double meters_to_miliseconds = GPS_C_m_s * 0.001;
++ double rough_range_ms = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
++
++ unsigned int int_ms = 0;
++
++ if (rough_range_ms == 0.0)
++ {
++ int_ms = 255;
++ }
++ else if((rough_range_ms < 0.0) || (rough_range_ms > meters_to_miliseconds * 255.0))
++ {
++ int_ms = 255;
++ }
++ else
++ {
++ int_ms = static_cast<unsigned int>(std::floor(rough_range_ms / meters_to_miliseconds / TWO_N10) + 0.5) >> 10;
++ }
++
++ DF397 = std::bitset<8>(int_ms);
++ return 0;
++}
++
++
++
++int Rtcm::set_DF398(const Gnss_Synchro & gnss_synchro)
++{
++ double meters_to_miliseconds = GPS_C_m_s * 0.001;
++ double rough_range_ms = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
++ DF398 = std::bitset<10>(static_cast<unsigned int>(rough_range_ms));
++ return 0;
++}
++
++int Rtcm::set_DF399(const Gnss_Synchro & gnss_synchro)
++{
++ double lambda;
++ std::string sig_(gnss_synchro.Signal);
++ std::string sig = sig_.substr(0,2);
++
++ if (sig.compare("1C") == 0 )
++ {
++ lambda = GPS_C_m_s / GPS_L1_FREQ_HZ;
++ }
++ if (sig.compare("2S") == 0 )
++ {
++ lambda = GPS_C_m_s / GPS_L2_FREQ_HZ;
++ }
++
++ if (sig.compare("5X") == 0 )
++ {
++ lambda = GPS_C_m_s / Galileo_E5a_FREQ_HZ;
++ }
++ if (sig.compare("1B") == 0 )
++ {
++ lambda = GPS_C_m_s / Galileo_E1_FREQ_HZ;
++ }
++
++ double rough_phase_range_ms = std::round(- gnss_synchro.Carrier_Doppler_hz / lambda);
++ DF399 = std::bitset<14>(static_cast<int>(rough_phase_range_ms));
++ return 0;
++}
++
+
+int Rtcm::set_DF409(unsigned int iods)
+{
+ DF409 = std::bitset<3>(iods);
+ return 0;
+}
+
++int Rtcm::set_DF411(unsigned int clock_steering_indicator)
++{
++ DF411 = std::bitset<2>(clock_steering_indicator);
++ return 0;
++}
++
++int Rtcm::set_DF412(unsigned int external_clock_indicator)
++{
++ DF412 = std::bitset<2>(external_clock_indicator);
++ return 0;
++}
++
+
+int Rtcm::set_DF417(bool using_divergence_free_smoothing)
+{
+ DF417 = std::bitset<1>(using_divergence_free_smoothing);
+ return 0;
+}
++
++int Rtcm::set_DF418(int carrier_smoothing_interval_s)
++{
++ unsigned int smoothing_int = abs(carrier_smoothing_interval_s);
++ if(carrier_smoothing_interval_s < 0)
++ {
++ DF418 = std::bitset<3>("111");
++ }
++ else
++ {
++ if(carrier_smoothing_interval_s == 0)
++ {
++ DF418 = std::bitset<3>("000");
++ }
++ else if(carrier_smoothing_interval_s < 30)
++ {
++ DF418 = std::bitset<3>("001");
++ }
++ else if(carrier_smoothing_interval_s < 60)
++ {
++ DF418 = std::bitset<3>("010");
++ }
++ else if(carrier_smoothing_interval_s < 120)
++ {
++ DF418 = std::bitset<3>("011");
++ }
++ else if(carrier_smoothing_interval_s < 240)
++ {
++ DF418 = std::bitset<3>("100");
++ }
++ else if(carrier_smoothing_interval_s < 480)
++ {
++ DF418 = std::bitset<3>("101");
++ }
++ else
++ {
++ DF418 = std::bitset<3>("110");
++ }
++ }
++ return 0;
++}
++
diff --cc src/core/system_parameters/rtcm.h
index b17f3cd,0000000..1784594
mode 100644,000000..100644
--- a/src/core/system_parameters/rtcm.h
+++ b/src/core/system_parameters/rtcm.h
@@@ -1,421 -1,0 +1,462 @@@
+/*!
+ * \file rtcm.h
+ * \brief Interface for the RTCM 3.2 Standard
+ * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es
+ *
+ * -------------------------------------------------------------------------
+ *
+ * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
+ *
+ * GNSS-SDR is a software defined Global Navigation
+ * Satellite Systems receiver
+ *
+ * This file is part of GNSS-SDR.
+ *
+ * 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/>.
+ *
+ * -------------------------------------------------------------------------
+ */
+
+
+#ifndef GNSS_SDR_RTCM_H_
+#define GNSS_SDR_RTCM_H_
+
+
+#include <bitset>
+#include <map>
+#include <string>
+#include <vector>
+#include <boost/crc.hpp>
+#include "gnss_synchro.h"
+#include "galileo_fnav_message.h"
+#include "gps_navigation_message.h"
+
++
+/*!
+ * \brief This class implements the generation and reading of some Message Types
+ * defined in the RTCM 3.2 Standard.
+ */
+class Rtcm
+{
+public:
+ Rtcm(); //<! Default constructor
+
+ std::string print_MT1001(const Gps_Ephemeris& gps_eph, double obs_time, const std::map<int, Gnss_Synchro> & pseudoranges);
+
+ /*!
+ * \brief Prints message type 1005 (Stationary Antenna Reference Point)
+ */
+ std::string print_MT1005(unsigned int ref_id, double ecef_x, double ecef_y, double ecef_z, bool gps, bool glonass, bool galileo, bool non_physical, bool single_oscillator, unsigned int quarter_cycle_indicator);
+
+ /*!
+ * \brief Verifies and reads messages of type 1005 (Stationary Antenna Reference Point). Returns 1 if anything goes wrong, 0 otherwise.
+ */
+ int read_MT1005(const std::string & message, unsigned int & ref_id, double & ecef_x, double & ecef_y, double & ecef_z, bool & gps, bool & glonass, bool & galileo);
+
+ /*!
+ * \brief Prints message type 1019 (GPS Ephemeris), should be broadcast in the event that
+ * the IODC does not match the IODE, and every 2 minutes.
+ */
+ std::string print_MT1019(const Gps_Ephemeris & gps_eph);
+
+ /*!
+ * \brief Verifies and reads messages of type 1019 (GPS Ephemeris). Returns 1 if anything goes wrong, 0 otherwise.
+ */
+ int read_MT1019(const std::string & message, Gps_Ephemeris & gps_eph);
+
+ /*!
+ * \brief Prints message type 1045 (Galileo Ephemeris), should be broadcast every 2 minutes
+ */
+ std::string print_MT1045(const Galileo_Ephemeris & gal_eph);
+
+ /*!
+ * \brief Verifies and reads messages of type 1045 (Galileo Ephemeris). Returns 1 if anything goes wrong, 0 otherwise.
+ */
+ int read_MT1045(const std::string & message, Galileo_Ephemeris & gal_eph);
+
++ std::string print_MSM_1( const Gps_Ephemeris & gps_eph,
++ const Galileo_Ephemeris & gal_eph,
++ double obs_time,
++ const std::map<int, Gnss_Synchro> & pseudoranges,
++ unsigned int ref_id,
++ unsigned int clock_steering_indicator,
++ unsigned int external_clock_indicator,
++ int smooth_int,
++ bool sync_flag,
++ bool divergence_free,
++ bool more_messages);
++
+ std::string bin_to_hex(const std::string& s); //<! Returns a string of hexadecimal symbols from a string of binary symbols
+ std::string hex_to_bin(const std::string& s); //<! Returns a string of binary symbols from a string of hexadecimal symbols
+
+ unsigned long int bin_to_uint(const std::string& s); //<! Returns an unsigned long int from a string of binary symbols
+ long int bin_to_int(const std::string& s); //<! Returns a long int from a string of binary symbols
+
+ unsigned long int hex_to_uint(const std::string& s); //<! Returns an unsigned long int from a string of hexadecimal symbols
+ long int hex_to_int(const std::string& s); //<! Returns a long int from a string of hexadecimal symbols
+
+ double bin_to_double(const std::string& s); //<! Returns double from a string of binary symbols
+ std::string print_MT1005_test(); //<! For testing purposes
+
+ bool check_CRC(const std::string & message); //<! Checks that the CRC of a RTCM package is correct
+
+private:
+ //
+ // Messages
+ //
+ std::bitset<64> message1001_header;
+ std::bitset<58> message1001_content;
+ std::bitset<64> message1002_header;
+ std::bitset<74> message1002_content;
+ std::bitset<488> message1019_content;
+ std::bitset<496> message1045_content;
+ std::bitset<169> MSM_header; // 169+X
+ std::vector<std::bitset<18> > MSM4_content; // 18 * Nsat
+ std::vector<std::bitset<36> > MSM5_content; // 36 * Nsat
+
+ std::bitset<64> get_MT1001_header(const Gps_Ephemeris & gps_eph,
+ double obs_time,
+ const std::map<int, Gnss_Synchro> & pseudoranges,
+ unsigned int ref_id,
+ unsigned int smooth_int,
+ bool sync_flag,
+ bool divergence_free);
+
+ std::bitset<58> get_MT1001_sat_content(const Gnss_Synchro & gnss_synchro);
+
+ std::bitset<152> get_MT1005_test();
+
++ std::string get_MSM_header(unsigned int msg_number, const Gps_Ephemeris & gps_eph,
++ const Galileo_Ephemeris & gal_eph,
++ double obs_time,
++ const std::map<int, Gnss_Synchro> & pseudoranges,
++ unsigned int ref_id,
++ unsigned int clock_steering_indicator,
++ unsigned int external_clock_indicator,
++ int smooth_int,
++ bool sync_flag,
++ bool divergence_free,
++ bool more_messages);
++
++ std::string get_MSM_1_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges);
++ std::string get_MSM_1_content_signal_data(const std::map<int, Gnss_Synchro> & pseudoranges);
++
++ std::string get_MSM_4_content_sat_data(const std::map<int, Gnss_Synchro> & pseudoranges);
++
+ //
+ // Transport Layer
+ //
+ std::bitset<8> preamble;
+ std::bitset<6> reserved_field;
+ std::bitset<10> message_length;
+ std::bitset<24> crc_frame;
+ typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type;
+ std::string add_CRC(const std::string& m);
+ std::string build_message(std::string data); // adds 0s to complete a byte and adds the CRC
+
+ //
+ // Data Fields
+ //
+ int reset_data_fields();
+
+ std::bitset<12> DF002;
+ int set_DF002(unsigned int message_number);
+
+ std::bitset<12> DF003;
+ int set_DF003(unsigned int ref_station_ID);
+
+ std::bitset<30> DF004;
+ int set_DF004(const Gps_Ephemeris & gps_eph, double obs_time);
+
+ std::bitset<1> DF005;
+ int set_DF005(bool sync_flag);
+
+ std::bitset<5> DF006;
+ int set_DF006(const std::map<int, Gnss_Synchro> & pseudoranges);
+
+ std::bitset<1> DF007;
+ int set_DF007(bool divergence_free_smoothing_indicator); // 0 - Divergence-free smoothing not used 1 - Divergence-free smoothing used
+
+ std::bitset<3> DF008;
+ int set_DF008(short int smoothing_interval);
+
+ std::bitset<6> DF009;
+ int set_DF009(const Gnss_Synchro & gnss_synchro);
+ int set_DF009(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<1> DF010;
+ int set_DF010(bool code_indicator);
+
+ std::bitset<24> DF011;
+ int set_DF011(const Gnss_Synchro & gnss_synchro);
+
+ std::bitset<20> DF012;
+ int set_DF012(const Gnss_Synchro & gnss_synchro);
+
+ std::bitset<7> DF013;
+ std::bitset<8> DF014;
+ int set_DF014(const Gnss_Synchro & gnss_synchro);
+
+ std::bitset<8> DF015;
+ int set_DF015(const Gnss_Synchro & gnss_synchro);
+
+
+ std::bitset<6> DF021;
+ int set_DF021();
+
+ std::bitset<1> DF022;
+ int set_DF022(bool gps_indicator);
+
+ std::bitset<1> DF023;
+ int set_DF023(bool glonass_indicator);
+
+ std::bitset<1> DF024;
+ int set_DF024(bool galileo_indicator);
+
+ std::bitset<38> DF025;
+ int set_DF025(double antenna_ECEF_X_m);
+
+ std::bitset<38> DF026;
+ int set_DF026(double antenna_ECEF_Y_m);
+
+ std::bitset<38> DF027;
+ int set_DF027(double antenna_ECEF_Z_m);
+
+ // Contents of GPS Satellite Ephemeris Data, Message Type 1019
+ std::bitset<8> DF071;
+ int set_DF071(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<10> DF076;
+ int set_DF076(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<4> DF077;
+ int set_DF077(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<2> DF078;
+ int set_DF078(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<14> DF079;
+ int set_DF079(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<8> DF080;
+ int set_DF080(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF081;
+ int set_DF081(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<8> DF082;
+ int set_DF082(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF083;
+ int set_DF083(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<22> DF084;
+ int set_DF084(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<10> DF085;
+ int set_DF085(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF086;
+ int set_DF086(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF087;
+ int set_DF087(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<32> DF088;
+ int set_DF088(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF089;
+ int set_DF089(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<32> DF090;
+ int set_DF090(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF091;
+ int set_DF091(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<32> DF092;
+ int set_DF092(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF093;
+ int set_DF093(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF094;
+ int set_DF094(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<32> DF095;
+ int set_DF095(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF096;
+ int set_DF096(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<32> DF097;
+ int set_DF097(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<16> DF098;
+ int set_DF098(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<32> DF099;
+ int set_DF099(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<24> DF100;
+ int set_DF100(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<8> DF101;
+ int set_DF101(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<6> DF102;
+ int set_DF102(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<1> DF103;
+ int set_DF103(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<1> DF137;
+ int set_DF137(const Gps_Ephemeris & gps_eph);
+
+
+
+ std::bitset<1> DF141;
+ int set_DF141(const Gps_Ephemeris & gps_eph);
+
+ std::bitset<1> DF142;
+ int set_DF142(const Gps_Ephemeris & gps_eph);
+
++ std::bitset<30> DF248;
++ int set_DF248(const Galileo_Ephemeris & gal_eph, double obs_time);
++
+ // Contents of Galileo F/NAV Satellite Ephemeris Data, Message Type 1045
+ std::bitset<6> DF252;
+ int set_DF252(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<12> DF289;
+ int set_DF289(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<10> DF290;
+ int set_DF290(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<8> DF291;
+ int set_DF291(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<14> DF292;
+ int set_DF292(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<14> DF293;
+ int set_DF293(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<6> DF294;
+ int set_DF294(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<21> DF295;
+ int set_DF295(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<31> DF296;
+ int set_DF296(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<16> DF297;
+ int set_DF297(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<16> DF298;
+ int set_DF298(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<32> DF299;
+ int set_DF299(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<16> DF300;
+ int set_DF300(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<32> DF301;
+ int set_DF301(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<16> DF302;
+ int set_DF302(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<32> DF303;
+ int set_DF303(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<14> DF304;
+ int set_DF304(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<16> DF305;
+ int set_DF305(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<32> DF306;
+ int set_DF306(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<16> DF307;
+ int set_DF307(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<32> DF308;
+ int set_DF308(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<16> DF309;
+ int set_DF309(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<32> DF310;
+ int set_DF310(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<24> DF311;
+ int set_DF311(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<10> DF312;
+ int set_DF312(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<10> DF313;
+ int set_DF313(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<2> DF314;
+ int set_DF314(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<1> DF315;
+ int set_DF315(const Galileo_Ephemeris & gal_eph);
+
+ std::bitset<2> DF364;
+
+ // Content of message header for MSM1, MSM2, MSM3, MSM4, MSM5, MSM6 and MSM7
+ std::bitset<1> DF393;
+ int set_DF393(bool more_messages); //1 indicates that more MSMs follow for given physical time and reference station ID
+
+ std::bitset<64> DF394;
- int set_DF394(const std::map<int, Gnss_Synchro> & gnss_synchro);
++ int set_DF394(const std::map<int, Gnss_Synchro> & pseudoranges);
+
+ std::bitset<32> DF395;
- int set_DF395(const std::map<int, Gnss_Synchro> & gnss_synchro);
++ int set_DF395(const std::map<int, Gnss_Synchro> & pseudoranges);
++
++ std::string set_DF396(const std::map<int, Gnss_Synchro> & pseudoranges);
++
++ std::bitset<8> DF397;
++ int set_DF397(const Gnss_Synchro & gnss_synchro);
++
++ std::bitset<10> DF398;
++ int set_DF398(const Gnss_Synchro & gnss_synchro);
++
++ std::bitset<14> DF399;
++ int set_DF399(const Gnss_Synchro & gnss_synchro);
+
- //std::bitset<1> DF396; //variable
+ std::bitset<3> DF409;
+ int set_DF409(unsigned int iods);
+
+ std::bitset<2> DF411;
++ int set_DF411(unsigned int clock_steering_indicator);
++
+ std::bitset<2> DF412;
++ int set_DF412(unsigned int external_clock_indicator);
++
+ std::bitset<1> DF417;
+ int set_DF417(bool using_divergence_free_smoothing);
+
+ std::bitset<3> DF418;
-
- // Content of Satellite data for MSM4 and MSM6
- std::vector<std::bitset<8> > DF397; // 8*NSAT
- std::vector<std::bitset<10> > DF398; // 10*NSAT
-
- // Content of Satellite data for MSM5 and MSM7
- std::vector<std::bitset<14> > DF399; // 14*NSAT
++ int set_DF418(int carrier_smoothing_interval_s);
+};
+
+#endif
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/pkg-hamradio/gnss-sdr.git
More information about the pkg-hamradio-commits
mailing list