1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-13 05:37:20 +00:00

Improve headers, large data members first. Improve nav message interfaces

This commit is contained in:
Carles Fernandez
2020-06-23 09:47:58 +02:00
parent 92f013c6b9
commit 037a1fcb5f
89 changed files with 1891 additions and 1600 deletions

View File

@@ -160,51 +160,6 @@ private:
bool save_gnss_synchro_map_xml(const std::string& file_name); // debug helper function
bool load_gnss_synchro_map_xml(const std::string& file_name); // debug helper function
bool d_dump;
bool d_dump_mat;
bool d_rinex_output_enabled;
bool d_rinex_header_written;
bool d_rinex_header_updated;
bool d_geojson_output_enabled;
bool d_gpx_output_enabled;
bool d_kml_output_enabled;
bool d_nmea_output_file_enabled;
bool d_first_fix;
bool d_xml_storage;
bool d_flag_monitor_pvt_enabled;
bool d_show_local_time_zone;
bool d_waiting_obs_block_rx_clock_offset_correction_msg;
bool d_enable_rx_clock_correction;
bool d_rtcm_writing_started;
bool d_rtcm_enabled;
int32_t d_rinexobs_rate_ms;
int32_t d_rtcm_MT1045_rate_ms; // Galileo Broadcast Ephemeris
int32_t d_rtcm_MT1019_rate_ms; // GPS Broadcast Ephemeris (orbits)
int32_t d_rtcm_MT1020_rate_ms; // GLONASS Broadcast Ephemeris (orbits)
int32_t d_rtcm_MT1077_rate_ms; // The type 7 Multiple Signal Message format for the USAs GPS system, popular
int32_t d_rtcm_MT1087_rate_ms; // GLONASS MSM7. The type 7 Multiple Signal Message format for the Russian GLONASS system
int32_t d_rtcm_MT1097_rate_ms; // Galileo MSM7. The type 7 Multiple Signal Message format for Europes Galileo system
int32_t d_rtcm_MSM_rate_ms;
int32_t d_kml_rate_ms;
int32_t d_gpx_rate_ms;
int32_t d_geojson_rate_ms;
int32_t d_nmea_rate_ms;
int32_t d_last_status_print_seg; // for status printer
int32_t d_output_rate_ms;
int32_t d_display_rate_ms;
int32_t d_report_rate_ms;
int32_t d_max_obs_block_rx_clock_offset_ms;
uint32_t d_nchannels;
uint32_t d_type_of_rx;
double d_rinex_version;
double d_rx_time;
key_t d_sysv_msg_key;
int d_sysv_msqid;
std::unique_ptr<Rinex_Printer> d_rp;
std::unique_ptr<Kml_Printer> d_kml_dump;
std::unique_ptr<Gpx_Printer> d_gpx_dump;
@@ -246,6 +201,51 @@ private:
std::map<int, Gnss_Synchro> d_gnss_observables_map_t1;
boost::posix_time::time_duration d_utc_diff_time;
double d_rinex_version;
double d_rx_time;
key_t d_sysv_msg_key;
int d_sysv_msqid;
int32_t d_rinexobs_rate_ms;
int32_t d_rtcm_MT1045_rate_ms; // Galileo Broadcast Ephemeris
int32_t d_rtcm_MT1019_rate_ms; // GPS Broadcast Ephemeris (orbits)
int32_t d_rtcm_MT1020_rate_ms; // GLONASS Broadcast Ephemeris (orbits)
int32_t d_rtcm_MT1077_rate_ms; // The type 7 Multiple Signal Message format for the USAs GPS system, popular
int32_t d_rtcm_MT1087_rate_ms; // GLONASS MSM7. The type 7 Multiple Signal Message format for the Russian GLONASS system
int32_t d_rtcm_MT1097_rate_ms; // Galileo MSM7. The type 7 Multiple Signal Message format for Europes Galileo system
int32_t d_rtcm_MSM_rate_ms;
int32_t d_kml_rate_ms;
int32_t d_gpx_rate_ms;
int32_t d_geojson_rate_ms;
int32_t d_nmea_rate_ms;
int32_t d_last_status_print_seg; // for status printer
int32_t d_output_rate_ms;
int32_t d_display_rate_ms;
int32_t d_report_rate_ms;
int32_t d_max_obs_block_rx_clock_offset_ms;
uint32_t d_nchannels;
uint32_t d_type_of_rx;
bool d_dump;
bool d_dump_mat;
bool d_rinex_output_enabled;
bool d_rinex_header_written;
bool d_rinex_header_updated;
bool d_geojson_output_enabled;
bool d_gpx_output_enabled;
bool d_kml_output_enabled;
bool d_nmea_output_file_enabled;
bool d_first_fix;
bool d_xml_storage;
bool d_flag_monitor_pvt_enabled;
bool d_show_local_time_zone;
bool d_waiting_obs_block_rx_clock_offset_correction_msg;
bool d_enable_rx_clock_correction;
bool d_rtcm_writing_started;
bool d_rtcm_enabled;
};
#endif // GNSS_SDR_RTKLIB_PVT_GS_H

View File

@@ -24,7 +24,6 @@
#include <fstream>
#include <memory>
#include <string>
class Pvt_Solution;
@@ -45,9 +44,9 @@ public:
private:
std::ofstream geojson_file;
bool first_pos;
std::string filename_;
std::string geojson_base_path;
bool first_pos;
};
#endif

View File

@@ -24,7 +24,6 @@
#include <fstream>
#include <memory>
#include <string>
class Pvt_Solution;
@@ -45,10 +44,10 @@ public:
private:
std::ofstream gpx_file;
bool positions_printed;
std::string gpx_filename;
std::string indent;
std::string gpx_base_path;
bool positions_printed;
};
#endif

View File

@@ -23,7 +23,6 @@
#define GNSS_SDR_KML_PRINTER_H
#include <fstream> // for ofstream
#include <memory> // for shared_ptr
#include <string>
class Pvt_Solution;
@@ -45,12 +44,12 @@ public:
private:
std::ofstream kml_file;
std::ofstream tmp_file;
bool positions_printed;
std::string kml_filename;
std::string kml_base_path;
std::string tmp_file_str;
unsigned int point_id;
std::string indent;
unsigned int point_id;
bool positions_printed;
};
#endif

View File

@@ -46,23 +46,17 @@ public:
*/
Nmea_Printer(const std::string& filename, bool flag_nmea_output_file, bool flag_nmea_tty_port, std::string nmea_dump_devname, const std::string& base_path = ".");
/*!
* \brief Print NMEA PVT and satellite info to the initialized device
*/
bool Print_Nmea_Line(const Rtklib_Solver* pvt_data, bool print_average_values);
/*!
* \brief Default destructor.
*/
~Nmea_Printer();
/*!
* \brief Print NMEA PVT and satellite info to the initialized device
*/
bool Print_Nmea_Line(const Rtklib_Solver* pvt_data, bool print_average_values);
private:
std::string nmea_filename; // String with the NMEA log filename
std::string nmea_base_path;
std::ofstream nmea_file_descriptor; // Output file stream for NMEA log file
std::string nmea_devname;
int nmea_dev_descriptor; // NMEA serial device descriptor (i.e. COM port)
const Rtklib_Solver* d_PVT_data;
int init_serial(const std::string& serial_device); // serial port control
void close_serial();
std::string get_GPGGA(); // fix data
@@ -73,6 +67,17 @@ private:
std::string longitude_to_hm(double longitude);
std::string latitude_to_hm(double lat);
char checkSum(const std::string& sentence);
const Rtklib_Solver* d_PVT_data;
std::ofstream nmea_file_descriptor; // Output file stream for NMEA log file
std::string nmea_filename; // String with the NMEA log filename
std::string nmea_base_path;
std::string nmea_devname;
int nmea_dev_descriptor; // NMEA serial device descriptor (i.e. COM port)
bool print_avg_pos;
bool d_flag_nmea_output_file;
};

View File

@@ -27,44 +27,15 @@
class Pvt_Conf
{
public:
uint32_t type_of_receiver;
int32_t output_rate_ms;
int32_t display_rate_ms;
int32_t kml_rate_ms;
int32_t gpx_rate_ms;
int32_t geojson_rate_ms;
int32_t nmea_rate_ms;
Pvt_Conf();
int32_t rinex_version;
int32_t rinexobs_rate_ms;
std::string rinex_name;
std::map<int, int> rtcm_msg_rate_ms;
bool dump;
bool dump_mat;
std::string rinex_name;
std::string dump_filename;
bool flag_nmea_tty_port;
std::string nmea_dump_filename;
std::string nmea_dump_devname;
bool flag_rtcm_server;
bool flag_rtcm_tty_port;
uint16_t rtcm_tcp_port;
uint16_t rtcm_station_id;
std::string rtcm_dump_devname;
bool output_enabled;
bool rinex_output_enabled;
bool gpx_output_enabled;
bool geojson_output_enabled;
bool nmea_output_file_enabled;
bool kml_output_enabled;
bool xml_output_enabled;
bool rtcm_output_file_enabled;
int32_t max_obs_block_rx_clock_offset_ms;
std::string output_path;
std::string rinex_output_path;
std::string gpx_output_path;
@@ -73,17 +44,41 @@ public:
std::string kml_output_path;
std::string xml_output_path;
std::string rtcm_output_file_path;
bool monitor_enabled;
bool protobuf_enabled;
std::string udp_addresses;
uint32_t type_of_receiver;
int32_t output_rate_ms;
int32_t display_rate_ms;
int32_t kml_rate_ms;
int32_t gpx_rate_ms;
int32_t geojson_rate_ms;
int32_t nmea_rate_ms;
int32_t rinex_version;
int32_t rinexobs_rate_ms;
int32_t max_obs_block_rx_clock_offset_ms;
int udp_port;
uint16_t rtcm_tcp_port;
uint16_t rtcm_station_id;
bool flag_nmea_tty_port;
bool flag_rtcm_server;
bool flag_rtcm_tty_port;
bool output_enabled;
bool rinex_output_enabled;
bool gpx_output_enabled;
bool geojson_output_enabled;
bool nmea_output_file_enabled;
bool kml_output_enabled;
bool xml_output_enabled;
bool rtcm_output_file_enabled;
bool monitor_enabled;
bool protobuf_enabled;
bool enable_rx_clock_correction;
bool show_local_time_zone;
bool pre_2009_file;
Pvt_Conf();
bool dump;
bool dump_mat;
};
#endif

View File

@@ -132,6 +132,14 @@ public:
protected:
bool d_pre_2009_file; // Flag to correct week rollover in post processing mode for signals older than 2009
private:
arma::vec d_rx_pos;
arma::vec d_rx_vel;
boost::posix_time::ptime d_position_UTC_time;
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
double d_rx_dt_s; // RX time offset [s]
double d_rx_clock_drift_ppm; // RX clock drift [ppm]
@@ -145,19 +153,11 @@ private:
double d_avg_longitude_d; // Averaged longitude in degrees
double d_avg_height_m; // Averaged height [m]
bool b_valid_position;
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
bool d_flag_averaging;
int d_averaging_depth; // Length of averaging window
arma::vec d_rx_pos;
arma::vec d_rx_vel;
boost::posix_time::ptime d_position_UTC_time;
int d_valid_observations;
bool b_valid_position;
bool d_flag_averaging;
};
#endif

View File

@@ -11888,13 +11888,13 @@ boost::posix_time::ptime Rinex_Printer::compute_UTC_time(const Gps_Navigation_Me
{
// if we are processing a file -> wait to leap second to resolve the ambiguity else take the week from the local system time
// idea: resolve the ambiguity with the leap second
const double utc_t = nav_msg.utc_time(nav_msg.d_TOW);
boost::posix_time::time_duration t = boost::posix_time::milliseconds(static_cast<int64_t>((utc_t + 604800 * static_cast<double>(nav_msg.i_GPS_week)) * 1000));
const double utc_t = nav_msg.utc_time(nav_msg.get_TOW());
boost::posix_time::time_duration t = boost::posix_time::milliseconds(static_cast<int64_t>((utc_t + 604800 * static_cast<double>(nav_msg.get_GPS_week())) * 1000));
// Handle week rollover
if (pre_2009_file_ == false)
{
// Handle week rollover (valid from 2009 to 2029)
if (nav_msg.i_GPS_week < 512)
if (nav_msg.get_GPS_week() < 512)
{
boost::posix_time::ptime p_time(boost::gregorian::date(2019, 4, 7), t);
return p_time;

View File

@@ -440,10 +440,6 @@ public:
void set_pre_2009_file(bool pre_2009_file);
private:
int version; // RINEX version (2 for 2.10/2.11 and 3 for 3.01)
int numberTypesObservations; // Number of available types of observable in the system. Should be public?
bool pre_2009_file_;
/*
* Generation of RINEX signal strength indicators
*/
@@ -477,8 +473,6 @@ private:
*/
void lengthCheck(const std::string& line);
double fake_cnav_iode;
/*
* If the string is bigger than length, truncate it from the right.
* otherwise, add pad characters to its right.
@@ -653,6 +647,11 @@ private:
inline std::string asString(const X x);
inline std::string asFixWidthString(const int x, const int width, char fill_digit);
double fake_cnav_iode;
int version; // RINEX version (2 for 2.10/2.11 and 3 for 3.01)
int numberTypesObservations; // Number of available types of observable in the system. Should be public?
bool pre_2009_file_;
};

View File

@@ -146,17 +146,18 @@ public:
uint32_t lock_time(const Glonass_Gnav_Ephemeris& eph, double obs_time, const Gnss_Synchro& gnss_synchro);
private:
std::string rtcm_filename; // String with the RTCM log filename
std::string rtcm_base_path;
std::ofstream rtcm_file_descriptor; // Output file stream for RTCM log file
std::string rtcm_devname;
uint16_t port;
uint16_t station_id;
int32_t rtcm_dev_descriptor; // RTCM serial device descriptor (i.e. COM port)
int32_t init_serial(const std::string& serial_device); // serial port control
void close_serial();
std::shared_ptr<Rtcm> rtcm;
bool Print_Message(const std::string& message);
std::shared_ptr<Rtcm> rtcm;
std::ofstream rtcm_file_descriptor; // Output file stream for RTCM log file
std::string rtcm_filename; // String with the RTCM log filename
std::string rtcm_base_path;
std::string rtcm_devname;
int32_t rtcm_dev_descriptor; // RTCM serial device descriptor (i.e. COM port)
uint16_t port;
uint16_t station_id;
bool d_rtcm_file_dump;
};

View File

@@ -73,14 +73,15 @@ public:
bool get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_map, bool flag_averaging);
sol_t pvt_sol{};
std::array<ssat_t, MAXSAT> pvt_ssat{};
double get_hdop() const override;
double get_vdop() const override;
double get_pdop() const override;
double get_gdop() const override;
Monitor_Pvt get_monitor_pvt() const;
sol_t pvt_sol{};
std::array<ssat_t, MAXSAT> pvt_ssat{};
std::map<int, Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int, Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris
std::map<int, Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris
@@ -106,16 +107,17 @@ public:
std::map<int, Beidou_Dnav_Almanac> beidou_dnav_almanac_map;
private:
rtk_t rtk_{};
Monitor_Pvt monitor_pvt{};
bool save_matfile();
std::array<obsd_t, MAXOBS> obs_data{};
std::array<double, 4> dop_{};
rtk_t rtk_{};
Monitor_Pvt monitor_pvt{};
std::string d_dump_filename;
std::ofstream d_dump_file;
int d_nchannels; // Number of available channels for positioning
bool d_flag_dump_enabled;
bool d_flag_dump_mat_enabled;
bool save_matfile();
};
#endif // GNSS_SDR_RTKLIB_SOLVER_H

View File

@@ -234,37 +234,6 @@ private:
float first_vs_second_peak_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step);
float max_to_input_power_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step);
bool d_active;
bool d_worker_active;
bool d_cshort;
bool d_step_two;
bool d_use_CFAR_algorithm_flag;
bool d_dump;
int32_t d_state;
int32_t d_positive_acq;
uint32_t d_channel;
uint32_t d_samplesPerChip;
uint32_t d_doppler_step;
int32_t d_doppler_center;
int32_t d_doppler_bias;
uint32_t d_num_noncoherent_integrations_counter;
uint32_t d_fft_size;
uint32_t d_consumed_samples;
uint32_t d_num_doppler_bins;
uint32_t d_num_doppler_bins_step2;
uint32_t d_dump_channel;
uint32_t d_buffer_count;
uint64_t d_sample_counter;
int64_t d_dump_number;
float d_threshold;
float d_mag;
float d_input_power;
float d_test_statistics;
float d_doppler_center_step_two;
std::string d_dump_filename;
std::unique_ptr<gr::fft::fft_complex> d_fft_if;
std::unique_ptr<gr::fft::fft_complex> d_ifft;
std::weak_ptr<ChannelFsm> d_channel_fsm;
volk_gnsssdr::vector<volk_gnsssdr::vector<float>> d_magnitude_grid;
volk_gnsssdr::vector<float> d_tmp_buffer;
volk_gnsssdr::vector<std::complex<float>> d_input_signal;
@@ -273,10 +242,47 @@ private:
volk_gnsssdr::vector<std::complex<float>> d_fft_codes;
volk_gnsssdr::vector<std::complex<float>> d_data_buffer;
volk_gnsssdr::vector<lv_16sc_t> d_data_buffer_sc;
std::string d_dump_filename;
std::unique_ptr<gr::fft::fft_complex> d_fft_if;
std::unique_ptr<gr::fft::fft_complex> d_ifft;
std::weak_ptr<ChannelFsm> d_channel_fsm;
Acq_Conf d_acq_parameters;
Gnss_Synchro* d_gnss_synchro;
arma::fmat d_grid;
arma::fmat d_narrow_grid;
int64_t d_dump_number;
uint64_t d_sample_counter;
float d_threshold;
float d_mag;
float d_input_power;
float d_test_statistics;
float d_doppler_center_step_two;
int32_t d_state;
int32_t d_positive_acq;
int32_t d_doppler_center;
int32_t d_doppler_bias;
uint32_t d_channel;
uint32_t d_samplesPerChip;
uint32_t d_doppler_step;
uint32_t d_num_noncoherent_integrations_counter;
uint32_t d_fft_size;
uint32_t d_consumed_samples;
uint32_t d_num_doppler_bins;
uint32_t d_num_doppler_bins_step2;
uint32_t d_dump_channel;
uint32_t d_buffer_count;
bool d_active;
bool d_worker_active;
bool d_cshort;
bool d_step_two;
bool d_use_CFAR_algorithm_flag;
bool d_dump;
};
#endif // GNSS_SDR_PCPS_ACQUISITION_H

View File

@@ -42,21 +42,21 @@ class Gnss_Synchro;
typedef struct
{
/* pcps acquisition configuration */
uint32_t doppler_max;
int64_t fs_in;
int32_t samples_per_code;
int32_t code_length;
uint32_t select_queue_Fpga;
std::string device_name;
int64_t fs_in;
float doppler_step2;
uint32_t* all_fft_codes; // pointer to memory that contains all the code ffts
uint32_t doppler_max;
uint32_t select_queue_Fpga;
uint32_t downsampling_factor;
uint32_t total_block_exp;
uint32_t excludelimit;
bool make_2_steps;
uint32_t num_doppler_bins_step2;
float doppler_step2;
bool repeat_satellite;
uint32_t max_num_acqs;
int32_t samples_per_code;
int32_t code_length;
bool make_2_steps;
bool repeat_satellite;
} pcpsconf_fpga_t;
class pcps_acquisition_fpga;
@@ -195,12 +195,28 @@ private:
void acquisition_core(uint32_t num_doppler_bins, uint32_t doppler_step, int32_t doppler_min);
float first_vs_second_peak_statistic(uint32_t& indext, int32_t& doppler, uint32_t num_doppler_bins, int32_t doppler_max, int32_t doppler_step);
bool d_active;
bool d_make_2_steps;
pcpsconf_fpga_t d_acq_parameters;
std::shared_ptr<Fpga_Acquisition> d_acquisition_fpga;
std::weak_ptr<ChannelFsm> d_channel_fsm;
Gnss_Synchro* d_gnss_synchro;
uint64_t d_sample_counter;
float d_threshold;
float d_mag;
float d_input_power;
float d_test_statistics;
float d_doppler_step2;
float d_doppler_center_step_two;
int32_t d_doppler_center;
int32_t d_state;
uint32_t d_doppler_index;
uint32_t d_channel;
uint32_t d_doppler_step;
int32_t d_doppler_center;
uint32_t d_doppler_max;
uint32_t d_fft_size;
uint32_t d_num_doppler_bins;
@@ -209,18 +225,9 @@ private:
uint32_t d_total_block_exp;
uint32_t d_num_doppler_bins_step2;
uint32_t d_max_num_acqs;
int32_t d_state;
uint64_t d_sample_counter;
float d_threshold;
float d_mag;
float d_input_power;
float d_test_statistics;
float d_doppler_step2;
float d_doppler_center_step_two;
std::shared_ptr<Fpga_Acquisition> d_acquisition_fpga;
std::weak_ptr<ChannelFsm> d_channel_fsm;
pcpsconf_fpga_t d_acq_parameters;
Gnss_Synchro* d_gnss_synchro;
bool d_active;
bool d_make_2_steps;
};
#endif // GNSS_SDR_PCPS_ACQUISITION_FPGA_H

View File

@@ -33,21 +33,33 @@ public:
void SetFromConfiguration(ConfigurationInterface *configuration, const std::string &role, double chip_rate, double opt_freq);
/* PCPS Acquisition configuration */
std::string item_type;
std::string dump_filename;
int64_t fs_in;
int64_t resampled_fs;
size_t it_size;
float doppler_step;
float samples_per_ms;
float doppler_step2;
float pfa;
float pfa2;
float samples_per_code;
float resampler_ratio;
uint32_t sampled_ms;
uint32_t ms_per_code;
uint32_t samples_per_chip;
uint32_t chips_per_second;
uint32_t max_dwells;
uint32_t num_doppler_bins_step2;
uint32_t resampler_latency_samples;
uint32_t dump_channel;
int32_t doppler_max;
int32_t doppler_min;
float doppler_step;
uint32_t num_doppler_bins_step2;
float doppler_step2;
float pfa;
float pfa2;
int64_t fs_in;
float samples_per_ms;
float samples_per_code;
bool bit_transition_flag;
bool use_CFAR_algorithm_flag;
bool dump;
@@ -55,13 +67,6 @@ public:
bool blocking_on_standby; // enable it only for unit testing to avoid sample consume on idle status
bool make_2_steps;
bool use_automatic_resampler;
float resampler_ratio;
int64_t resampled_fs;
uint32_t resampler_latency_samples;
std::string dump_filename;
uint32_t dump_channel;
size_t it_size;
std::string item_type;
private:
void SetDerivedParams();

View File

@@ -148,6 +148,12 @@ private:
static const uint32_t SELECT_ALL_CODE_BITS = 0xFFFFFFFF; // Select a 20 bit word
static const uint32_t SHL_CODE_BITS = 65536; // shift left by 10 bits
// FPGA private functions
void fpga_acquisition_test_register(void);
void read_result_valid(uint32_t *result_valid);
std::string d_device_name; // HW device name
int64_t d_fs_in;
// data related to the hardware module and the driver
int32_t d_fd; // driver descriptor
@@ -158,13 +164,9 @@ private:
uint32_t d_nsamples_total; // number of samples including padding
uint32_t d_nsamples; // number of samples not including padding
uint32_t d_select_queue; // queue selection
std::string d_device_name; // HW device name
uint32_t d_doppler_max; // max doppler
uint32_t d_doppler_step; // doppler step
uint32_t d_PRN; // PRN
// FPGA private functions
void fpga_acquisition_test_register(void);
void read_result_valid(uint32_t *result_valid);
};
#endif // GNSS_SDR_FPGA_ACQUISITION_H

View File

@@ -76,15 +76,7 @@ private:
void smooth_pseudoranges(std::vector<Gnss_Synchro>& data);
int32_t save_matfile() const;
bool d_T_rx_TOW_set; // rx time follow GPST
bool d_dump;
bool d_dump_mat;
uint32_t d_T_rx_TOW_ms;
uint32_t d_T_rx_step_ms;
uint32_t d_T_status_report_timer_ms;
uint32_t d_nchannels_in;
uint32_t d_nchannels_out;
double d_smooth_filter_M;
Obs_Conf d_conf;
enum StringValue_
{
@@ -102,6 +94,10 @@ private:
};
std::map<std::string, StringValue_> d_mapStringValues;
std::unique_ptr<Gnss_circular_deque<Gnss_Synchro>> d_gnss_synchro_history; // Tracking observable history
boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history
std::vector<bool> d_channel_last_pll_lock;
std::vector<double> d_channel_last_pseudorange_smooth;
std::vector<double> d_channel_last_carrier_phase_rads;
@@ -110,11 +106,17 @@ private:
std::ofstream d_dump_file;
std::unique_ptr<Gnss_circular_deque<Gnss_Synchro>> d_gnss_synchro_history; // Tracking observable history
double d_smooth_filter_M;
boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history
uint32_t d_T_rx_TOW_ms;
uint32_t d_T_rx_step_ms;
uint32_t d_T_status_report_timer_ms;
uint32_t d_nchannels_in;
uint32_t d_nchannels_out;
Obs_Conf d_conf;
bool d_T_rx_TOW_set; // rx time follow GPST
bool d_dump;
bool d_dump_mat;
};
#endif // GNSS_SDR_HYBRID_OBSERVABLES_GS_H

View File

@@ -23,11 +23,11 @@
Obs_Conf::Obs_Conf()
{
enable_carrier_smoothing = false;
dump_filename = "obs_dump.dat";
smoothing_factor = FLAGS_carrier_smoothing_factor;
nchannels_in = 0;
nchannels_out = 0;
enable_carrier_smoothing = false;
dump = false;
dump_mat = false;
dump_filename = "obs_dump.dat";
}

View File

@@ -27,15 +27,15 @@
class Obs_Conf
{
public:
bool enable_carrier_smoothing;
Obs_Conf();
std::string dump_filename;
int32_t smoothing_factor;
uint32_t nchannels_in;
uint32_t nchannels_out;
bool enable_carrier_smoothing;
bool dump;
bool dump_mat;
std::string dump_filename;
Obs_Conf();
};
#endif

View File

@@ -226,7 +226,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
}
// 3. Check operation executed correctly
if (d_nav.flag_crc_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel
<< " from satellite " << d_satellite;
@@ -281,8 +281,8 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell
// Update satellite information for DNAV decoder
sat_prn = d_satellite.get_PRN();
d_nav.i_satellite_PRN = sat_prn;
d_nav.i_signal_type = 1; // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)
d_nav.set_satellite_PRN(sat_prn);
d_nav.set_signal_type(1); // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)
// Update tel dec parameters for D2 NAV Messages
if (sat_prn > 0 and sat_prn < 6)
@@ -448,7 +448,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
// call the decoder
decode_subframe(d_subframe_symbols.data());
if (d_nav.flag_crc_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -505,7 +505,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
// call the decoder
decode_subframe(d_subframe_symbols.data());
if (d_nav.flag_crc_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -532,17 +532,17 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
}
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true)
if (this->d_flag_preamble == true and d_nav.get_flag_new_SOW_available() == true)
// update TOW at the preamble instant
{
// Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.d_SOW + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
// check TOW update consistency
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// compute new TOW
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
flag_SOW_set = true;
d_nav.flag_new_SOW_available = false;
d_nav.set_flag_new_SOW_available(false);
if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(d_symbol_duration_ms))
{

View File

@@ -83,42 +83,45 @@ private:
// Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{};
int32_t d_symbols_per_preamble;
int32_t d_samples_per_preamble;
int32_t d_preamble_period_samples;
std::array<float, BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS> d_subframe_symbols{};
uint32_t d_required_symbols;
// Storage for incoming data
boost::circular_buffer<float> d_symbol_history;
// Variables for internal functionality
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_stat; // Status of decoder
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_preamble; // Flag indicating when preamble was found
int32_t d_CRC_error_counter; // Number of failed CRC operations
bool flag_SOW_set; // Indicates when time of week is set
// Navigation Message variable
Beidou_Dnav_Navigation_Message d_nav;
// Satellite Information and logging capacity
Gnss_Satellite d_satellite;
std::string d_dump_filename;
std::ofstream d_dump_file;
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
int32_t d_channel;
int32_t d_symbols_per_preamble;
int32_t d_samples_per_preamble;
int32_t d_preamble_period_samples;
int32_t d_CRC_error_counter; // Number of failed CRC operations
uint32_t d_required_symbols;
uint32_t d_stat; // Status of decoder
// Values to populate gnss synchronization structure
uint64_t d_last_valid_preamble;
uint32_t d_symbol_duration_ms;
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_TOW_at_current_symbol_ms;
uint64_t d_last_valid_preamble;
bool flag_SOW_set; // Indicates when time of week is set
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_preamble; // Flag indicating when preamble was found
bool d_flag_valid_word;
bool d_sent_tlm_failed_msg;
bool Flag_valid_word;
// Satellite Information and logging capacity
Gnss_Satellite d_satellite;
int32_t d_channel;
bool d_dump;
std::string d_dump_filename;
std::ofstream d_dump_file;
};
#endif // GNSS_SDR_BEIDOU_B1I_TELEMETRY_DECODER_GS_H

View File

@@ -227,7 +227,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
}
// 3. Check operation executed correctly
if (d_nav.flag_crc_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel
<< " from satellite " << d_satellite;
@@ -299,8 +299,8 @@ void beidou_b3i_telemetry_decoder_gs::set_satellite(
// Update satellite information for DNAV decoder
sat_prn = d_satellite.get_PRN();
d_nav.i_satellite_PRN = sat_prn;
d_nav.i_signal_type = 5; // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)
d_nav.set_satellite_PRN(sat_prn);
d_nav.set_signal_type(5); // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)
// Update tel dec parameters for D2 NAV Messages
if (sat_prn > 0 and sat_prn < 6)
@@ -474,7 +474,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
// call the decoder
decode_subframe(d_subframe_symbols.data());
if (d_nav.flag_crc_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -534,7 +534,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
// call the decoder
decode_subframe(d_subframe_symbols.data());
if (d_nav.flag_crc_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -563,17 +563,17 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
}
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true)
if (this->d_flag_preamble == true and d_nav.get_flag_new_SOW_available() == true)
// update TOW at the preamble instant
{
// Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.d_SOW + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
// check TOW update consistency
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// compute new TOW
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
flag_SOW_set = true;
d_nav.flag_new_SOW_available = false;
d_nav.set_flag_new_SOW_available(false);
if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(d_symbol_duration_ms))
{

View File

@@ -81,42 +81,43 @@ private:
// Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{};
int32_t d_symbols_per_preamble;
int32_t d_samples_per_preamble;
int32_t d_preamble_period_samples;
std::array<float, BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS> d_subframe_symbols{};
uint32_t d_required_symbols;
// Storage for incoming data
boost::circular_buffer<float> d_symbol_history;
// Variables for internal functionality
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_stat; // Status of decoder
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_preamble; // Flag indicating when preamble was found
int32_t d_CRC_error_counter; // Number of failed CRC operations
bool flag_SOW_set; // Indicates when time of week is set
// Navigation Message variable
Beidou_Dnav_Navigation_Message d_nav;
Gnss_Satellite d_satellite;
std::string d_dump_filename;
std::ofstream d_dump_file;
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_required_symbols;
uint32_t d_stat; // Status of decoder
int32_t d_channel;
int32_t d_CRC_error_counter; // Number of failed CRC operations
int32_t d_symbols_per_preamble;
int32_t d_samples_per_preamble;
int32_t d_preamble_period_samples;
// Values to populate gnss synchronization structure
uint64_t d_last_valid_preamble;
uint32_t d_symbol_duration_ms;
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_TOW_at_current_symbol_ms;
uint64_t d_last_valid_preamble;
bool flag_SOW_set; // Indicates when time of week is set
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_preamble; // Flag indicating when preamble was found
bool d_flag_valid_word;
bool d_sent_tlm_failed_msg;
bool Flag_valid_word;
// Satellite Information and logging capacity
Gnss_Satellite d_satellite;
int32_t d_channel;
bool d_dump;
std::string d_dump_filename;
std::ofstream d_dump_file;
};
#endif // GNSS_SDR_BEIDOU_B3I_TELEMETRY_DECODER_GS_H

View File

@@ -252,7 +252,7 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
{
// DECODE COMPLETE WORD (even + odd) and TEST CRC
d_inav_nav.split_page(page_String, flag_even_word_arrived);
if (d_inav_nav.flag_CRC_test == true)
if (d_inav_nav.get_flag_CRC_test() == true)
{
DLOG(INFO) << "Galileo E1 CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
@@ -290,7 +290,7 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_inav_nav.get_utc_model());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
d_delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G_10 + 604800 * (fmod((d_inav_nav.WN_0 - tmp_obj->WN_0G_10), 64)));
d_delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G_10 + 604800 * (std::fmod(static_cast<float>(d_inav_nav.get_Galileo_week() - tmp_obj->WN_0G_10), 64.0)));
DLOG(INFO) << "delta_t=" << d_delta_t << "[s]";
}
if (d_inav_nav.have_new_almanac() == true)
@@ -301,7 +301,7 @@ void galileo_telemetry_decoder_gs::decode_INAV_word(float *page_part_symbols, in
std::cout << "Galileo E1 I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << std::endl;
DLOG(INFO) << "Current parameters:";
DLOG(INFO) << "d_TOW_at_current_symbol_ms=" << d_TOW_at_current_symbol_ms;
DLOG(INFO) << "d_nav.WN_0=" << d_inav_nav.WN_0;
DLOG(INFO) << "d_nav.WN_0=" << d_inav_nav.get_Galileo_week();
}
}
@@ -341,7 +341,7 @@ void galileo_telemetry_decoder_gs::decode_FNAV_word(float *page_symbols, int32_t
// DECODE COMPLETE WORD (even + odd) and TEST CRC
d_fnav_nav.split_page(page_String);
if (d_fnav_nav.flag_CRC_test == true)
if (d_fnav_nav.get_flag_CRC_test() == true)
{
DLOG(INFO) << "Galileo E5a CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
@@ -595,7 +595,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
break;
}
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (d_inav_nav.flag_CRC_test == true or d_fnav_nav.flag_CRC_test == true)
if (d_inav_nav.get_flag_CRC_test() == true or d_fnav_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -617,8 +617,8 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
d_stat = 0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
d_fnav_nav.flag_TOW_set = false;
d_inav_nav.flag_TOW_set = false;
d_fnav_nav.set_flag_TOW_set(false);
d_inav_nav.set_flag_TOW_set(false);
}
}
}
@@ -635,22 +635,22 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
{
case 1: // INAV
{
if (d_inav_nav.flag_TOW_set == true)
if (d_inav_nav.is_TOW_set() == true)
{
if (d_inav_nav.flag_TOW_5 == true) // page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
if (d_inav_nav.is_TOW5_set() == true) // page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
// TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.TOW_5 * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW5() * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
d_inav_nav.flag_TOW_5 = false;
d_inav_nav.set_TOW5_flag(false);
}
else if (d_inav_nav.flag_TOW_6 == true) // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
else if (d_inav_nav.is_TOW5_set() == true) // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
// TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.TOW_6 * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.get_TOW6() * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
d_inav_nav.flag_TOW_6 = false;
d_inav_nav.set_TOW6_flag(false);
}
else
{
@@ -662,35 +662,35 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
}
case 2: // FNAV
{
if (d_fnav_nav.flag_TOW_set == true)
if (d_fnav_nav.is_TOW_set() == true)
{
if (d_fnav_nav.flag_TOW_1 == true)
if (d_fnav_nav.is_TOW1_set() == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_1 * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.get_TOW1() * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_1 = false;
d_fnav_nav.set_TOW1_flag(false);
}
else if (d_fnav_nav.flag_TOW_2 == true)
else if (d_fnav_nav.is_TOW2_set() == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_2 * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.get_TOW2() * 1000.0);
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_2 = false;
d_fnav_nav.set_TOW2_flag(false);
}
else if (d_fnav_nav.flag_TOW_3 == true)
else if (d_fnav_nav.is_TOW3_set() == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_3 * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.get_TOW3() * 1000.0);
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_3 = false;
d_fnav_nav.set_TOW3_flag(false);
}
else if (d_fnav_nav.flag_TOW_4 == true)
else if (d_fnav_nav.is_TOW4_set() == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_4 * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.get_TOW4() * 1000.0);
// d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_FNAV_CODES_PER_SYMBOL * GALILEO_E5A_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_4 = false;
d_fnav_nav.set_TOW4_flag(false);
}
else
{
@@ -707,7 +707,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
{
case 1: // INAV
{
if (d_inav_nav.flag_TOW_set == true)
if (d_inav_nav.is_TOW_set() == true)
{
d_TOW_at_current_symbol_ms += d_PRN_code_period_ms;
}
@@ -715,7 +715,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
}
case 2: // FNAV
{
if (d_fnav_nav.flag_TOW_set == true)
if (d_fnav_nav.is_TOW_set() == true)
{
d_TOW_at_current_symbol_ms += d_PRN_code_period_ms;
}
@@ -728,11 +728,11 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
{
case 1: // INAV
{
if (d_inav_nav.flag_TOW_set)
if (d_inav_nav.is_TOW_set())
{
if (d_inav_nav.flag_GGTO_1 == true and d_inav_nav.flag_GGTO_2 == true and d_inav_nav.flag_GGTO_3 == true and d_inav_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
if (d_inav_nav.get_flag_GGTO()) // all GGTO parameters arrived
{
d_delta_t = d_inav_nav.A_0G_10 + d_inav_nav.A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - d_inav_nav.t_0G_10 + 604800.0 * (fmod((d_inav_nav.WN_0 - d_inav_nav.WN_0G_10), 64.0)));
d_delta_t = d_inav_nav.get_A0G() + d_inav_nav.get_A1G() * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - d_inav_nav.get_t0G() + 604800.0 * (std::fmod(static_cast<float>(d_inav_nav.get_Galileo_week() - d_inav_nav.get_WN0G()), 64.0)));
}
current_symbol.Flag_valid_word = true;
@@ -742,7 +742,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
case 2: // FNAV
{
if (d_fnav_nav.flag_TOW_set)
if (d_fnav_nav.is_TOW_set())
{
current_symbol.Flag_valid_word = true;
}
@@ -750,7 +750,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
}
}
if (d_inav_nav.flag_TOW_set or d_fnav_nav.flag_TOW_set)
if (d_inav_nav.is_TOW_set() or d_fnav_nav.is_TOW_set())
{
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// todo: Galileo to GPS time conversion should be moved to observable block.

View File

@@ -78,24 +78,42 @@ private:
galileo_telemetry_decoder_gs(const Gnss_Satellite &satellite, int frame_type, bool dump);
const int32_t d_nn = 2; // Coding rate 1/n
const int32_t d_KK = 7; // Constraint Length
void viterbi_decoder(float *page_part_symbols, int32_t *page_part_bits);
void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out);
void decode_INAV_word(float *page_part_symbols, int32_t frame_length);
void decode_FNAV_word(float *page_symbols, int32_t frame_length);
bool d_sent_tlm_failed_msg;
bool d_flag_frame_sync;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_parity;
bool d_flag_preamble;
bool d_dump;
// vars for Viterbi decoder
std::vector<int32_t> d_preamble_samples;
std::vector<float> d_page_part_symbols;
std::vector<int32_t> d_out0;
std::vector<int32_t> d_out1;
std::vector<int32_t> d_state0;
std::vector<int32_t> d_state1;
std::string d_dump_filename;
std::ofstream d_dump_file;
boost::circular_buffer<float> d_symbol_history;
Gnss_Satellite d_satellite;
// navigation message vars
Galileo_Navigation_Message d_inav_nav;
Galileo_Fnav_Message d_fnav_nav;
double d_delta_t; // GPS-GALILEO time offset
uint64_t d_sample_counter;
uint64_t d_preamble_index;
uint64_t d_last_valid_preamble;
const int32_t d_nn = 2; // Coding rate 1/n
const int32_t d_KK = 7; // Constraint Length
int32_t d_mm = d_KK - 1;
int32_t d_codelength;
int32_t d_datalength;
int32_t d_frame_type;
int32_t d_bits_per_preamble;
int32_t d_samples_per_preamble;
@@ -109,32 +127,14 @@ private:
uint32_t d_stat;
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_TOW_at_current_symbol_ms;
uint64_t d_sample_counter;
uint64_t d_preamble_index;
uint64_t d_last_valid_preamble;
uint32_t d_max_symbols_without_valid_frame;
double d_delta_t; // GPS-GALILEO time offset
// vars for Viterbi decoder
std::vector<int32_t> d_out0;
std::vector<int32_t> d_out1;
std::vector<int32_t> d_state0;
std::vector<int32_t> d_state1;
std::vector<int32_t> d_preamble_samples;
std::vector<float> d_page_part_symbols;
std::string d_dump_filename;
std::ofstream d_dump_file;
boost::circular_buffer<float> d_symbol_history;
Gnss_Satellite d_satellite;
// navigation message vars
Galileo_Navigation_Message d_inav_nav;
Galileo_Fnav_Message d_fnav_nav;
bool d_sent_tlm_failed_msg;
bool d_flag_frame_sync;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_parity;
bool d_flag_preamble;
bool d_dump;
};
#endif // GNSS_SDR_GALILEO_TELEMETRY_DECODER_GS_H

View File

@@ -165,7 +165,7 @@ void glonass_l1_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
d_nav.string_decoder(data_bits);
// 3. Check operation executed correctly
if (d_nav.flag_CRC_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
LOG(INFO) << "GLONASS GNAV CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
@@ -177,7 +177,7 @@ void glonass_l1_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
if (d_nav.have_new_ephemeris() == true)
{
// get object for this SV (mandatory)
d_nav.gnav_ephemeris.i_satellite_freq_channel = d_satellite.get_rf_link();
d_nav.set_rf_link(d_satellite.get_rf_link());
std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite;
@@ -193,19 +193,20 @@ void glonass_l1_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
}
if (d_nav.have_new_almanac() == true)
{
uint32_t slot_nbr = d_nav.i_alm_satellite_slot_number;
std::shared_ptr<Glonass_Gnav_Almanac> tmp_obj = std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac(slot_nbr));
uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number();
std::shared_ptr<Glonass_Gnav_Almanac>
tmp_obj = std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac(slot_nbr));
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Almanac have been received in channel" << d_channel << " in slot number " << slot_nbr;
std::cout << "New GLONASS L1 GNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << std::endl;
}
// 5. Update satellite information on system
if (d_nav.flag_update_slot_number == true)
if (d_nav.get_flag_update_slot_number() == true)
{
LOG(INFO) << "GLONASS GNAV Slot Number Identified in channel " << d_channel;
d_satellite.update_PRN(d_nav.gnav_ephemeris.d_n);
d_satellite.what_block(d_satellite.get_system(), d_nav.gnav_ephemeris.d_n);
d_nav.flag_update_slot_number = false;
d_satellite.update_PRN(d_nav.get_ephemeris().d_n);
d_satellite.what_block(d_satellite.get_system(), d_nav.get_ephemeris().d_n);
d_nav.set_flag_update_slot_number(false);
}
}
@@ -342,7 +343,7 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
// call the decoder
decode_string(string_symbols.data(), string_length);
if (d_nav.flag_CRC_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -370,25 +371,25 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_TOW_new == true)
if (this->d_flag_preamble == true and d_nav.get_flag_TOW_new() == true)
// update TOW at the preamble instant
{
d_TOW_at_current_symbol = floor((d_nav.gnav_ephemeris.d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_nav.flag_TOW_new = false;
d_TOW_at_current_symbol = floor((d_nav.get_ephemeris().d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_nav.set_flag_TOW_new(false);
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L1_CA_CODE_PERIOD_S;
}
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.get_flag_CRC_test() == true)
// if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
// {
// delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64)));
// }
if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true)
if (d_flag_frame_sync == true and d_nav.is_flag_TOW_set() == true)
{
current_symbol.Flag_valid_word = true;
}

View File

@@ -81,43 +81,46 @@ private:
glonass_l1_ca_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump);
const std::array<uint16_t, GLONASS_GNAV_PREAMBLE_LENGTH_BITS> d_preambles_bits{GLONASS_GNAV_PREAMBLE};
const int32_t d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS;
void decode_string(const double *symbols, int32_t frame_length);
// Help with coherent tracking
double d_preamble_time_samples;
// Preamble decoding
const std::array<uint16_t, GLONASS_GNAV_PREAMBLE_LENGTH_BITS> d_preambles_bits{GLONASS_GNAV_PREAMBLE};
std::array<int32_t, GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS> d_preambles_symbols{};
const int32_t d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS;
// Storage for incoming data
boost::circular_buffer<Gnss_Synchro> d_symbol_history;
// Variables for internal functionality
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_stat; // Status of decoder
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_parity; // Flag indicating when parity check was achieved (crc check)
bool d_flag_preamble; // Flag indicating when preamble was found
int32_t d_CRC_error_counter; // Number of failed CRC operations
bool flag_TOW_set; // Indicates when time of week is set
double delta_t; // GPS-GLONASS time offset
// Navigation Message variable
Glonass_Gnav_Navigation_Message d_nav;
// Values to populate gnss synchronization structure
double d_TOW_at_current_symbol;
bool Flag_valid_word;
// Satellite Information and logging capacity
Gnss_Satellite d_satellite;
int32_t d_channel;
bool d_dump;
std::string d_dump_filename;
std::ofstream d_dump_file;
double d_preamble_time_samples;
double d_TOW_at_current_symbol;
double delta_t; // GPS-GLONASS time offset
// Variables for internal functionality
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_stat; // Status of decoder
int32_t d_CRC_error_counter; // Number of failed CRC operations
int32_t d_channel;
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_parity; // Flag indicating when parity check was achieved (crc check)
bool d_flag_preamble; // Flag indicating when preamble was found
bool flag_TOW_set; // Indicates when time of week is set
bool Flag_valid_word;
bool d_dump;
};
#endif // GNSS_SDR_GLONASS_L1_CA_TELEMETRY_DECODER_GS_H

View File

@@ -165,7 +165,7 @@ void glonass_l2_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
d_nav.string_decoder(data_bits);
// 3. Check operation executed correctly
if (d_nav.flag_CRC_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
LOG(INFO) << "GLONASS GNAV CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
@@ -177,7 +177,7 @@ void glonass_l2_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
if (d_nav.have_new_ephemeris() == true)
{
// get object for this SV (mandatory)
d_nav.gnav_ephemeris.i_satellite_freq_channel = d_satellite.get_rf_link();
d_nav.set_rf_link(d_satellite.get_rf_link());
std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Ephemeris have been received in channel" << d_channel << " from satellite " << d_satellite;
@@ -193,19 +193,19 @@ void glonass_l2_ca_telemetry_decoder_gs::decode_string(const double *frame_symbo
}
if (d_nav.have_new_almanac() == true)
{
uint32_t slot_nbr = d_nav.i_alm_satellite_slot_number;
uint32_t slot_nbr = d_nav.get_alm_satellite_slot_number();
std::shared_ptr<Glonass_Gnav_Almanac> tmp_obj = std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac(slot_nbr));
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
LOG(INFO) << "GLONASS GNAV Almanac have been received in channel" << d_channel << " in slot number " << slot_nbr;
std::cout << TEXT_CYAN << "New GLONASS L2 GNAV almanac received in channel " << d_channel << " from satellite " << d_satellite << TEXT_RESET << std::endl;
}
// 5. Update satellite information on system
if (d_nav.flag_update_slot_number == true)
if (d_nav.get_flag_update_slot_number() == true)
{
LOG(INFO) << "GLONASS GNAV Slot Number Identified in channel " << d_channel;
d_satellite.update_PRN(d_nav.gnav_ephemeris.d_n);
d_satellite.what_block(d_satellite.get_system(), d_nav.gnav_ephemeris.d_n);
d_nav.flag_update_slot_number = false;
d_satellite.update_PRN(d_nav.get_ephemeris().d_n);
d_satellite.what_block(d_satellite.get_system(), d_nav.get_ephemeris().d_n);
d_nav.set_flag_update_slot_number(false);
}
}
@@ -342,7 +342,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
// call the decoder
decode_string(string_symbols.data(), string_length);
if (d_nav.flag_CRC_test == true)
if (d_nav.get_flag_CRC_test() == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
@@ -370,25 +370,25 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_TOW_new == true)
if (this->d_flag_preamble == true and d_nav.get_flag_TOW_new() == true)
// update TOW at the preamble instant
{
d_TOW_at_current_symbol = floor((d_nav.gnav_ephemeris.d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_nav.flag_TOW_new = false;
d_TOW_at_current_symbol = floor((d_nav.get_ephemeris().d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_nav.set_flag_TOW_new(false);
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L2_CA_CODE_PERIOD_S;
}
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.get_flag_CRC_test() == true)
// if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
// {
// delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64)));
// }
if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true)
if (d_flag_frame_sync == true and d_nav.is_flag_TOW_set() == true)
{
current_symbol.Flag_valid_word = true;
}

View File

@@ -78,43 +78,42 @@ private:
glonass_l2_ca_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump);
void decode_string(const double *symbols, int32_t frame_length);
// Help with coherent tracking
double d_preamble_time_samples;
// Preamble decoding
const std::array<uint16_t, GLONASS_GNAV_PREAMBLE_LENGTH_BITS> d_preambles_bits{GLONASS_GNAV_PREAMBLE};
std::array<int32_t, GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS> d_preambles_symbols{};
const int32_t d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS;
void decode_string(const double *symbols, int32_t frame_length);
// Storage for incoming data
boost::circular_buffer<Gnss_Synchro> d_symbol_history;
// Variables for internal functionality
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_stat; // Status of decoder
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_parity; // Flag indicating when parity check was achieved (crc check)
bool d_flag_preamble; // Flag indicating when preamble was found
int32_t d_CRC_error_counter; // Number of failed CRC operations
bool flag_TOW_set; // Indicates when time of week is set
double delta_t; // GPS-GLONASS time offset
std::array<int32_t, GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS> d_preambles_symbols{};
// Navigation Message variable
Glonass_Gnav_Navigation_Message d_nav;
// Values to populate gnss synchronization structure
double d_TOW_at_current_symbol;
bool Flag_valid_word;
// Satellite Information and logging capacity
Gnss_Satellite d_satellite;
int32_t d_channel;
bool d_dump;
std::string d_dump_filename;
std::ofstream d_dump_file;
double d_preamble_time_samples;
double delta_t; // GPS-GLONASS time offset
double d_TOW_at_current_symbol;
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_stat; // Status of decoder
int32_t d_CRC_error_counter; // Number of failed CRC operations
int32_t d_channel;
bool Flag_valid_word;
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
bool d_flag_parity; // Flag indicating when parity check was achieved (crc check)
bool d_flag_preamble; // Flag indicating when preamble was found
bool flag_TOW_set; // Indicates when time of week is set
bool d_dump;
};
#endif // GNSS_SDR_GLONASS_L2_CA_TELEMETRY_DECODER_GS_H

View File

@@ -172,7 +172,7 @@ void gps_l1_ca_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satelli
d_nav = Gps_Navigation_Message();
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
d_nav.i_satellite_PRN = d_satellite.get_PRN();
d_nav.set_satellite_PRN(d_satellite.get_PRN());
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
}
@@ -180,7 +180,7 @@ void gps_l1_ca_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satelli
void gps_l1_ca_telemetry_decoder_gs::set_channel(int32_t channel)
{
d_channel = channel;
d_nav.i_channel_ID = channel;
d_nav.set_channel(channel);
DLOG(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
@@ -274,7 +274,7 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe()
std::cout << "New GPS NAV message received in channel " << this->d_channel << ": "
<< "subframe "
<< subframe_ID << " from satellite "
<< Gnss_Satellite(std::string("GPS"), d_nav.i_satellite_PRN) << std::endl;
<< Gnss_Satellite(std::string("GPS"), d_nav.get_satellite_PRN()) << std::endl;
switch (subframe_ID)
{
@@ -287,12 +287,12 @@ bool gps_l1_ca_telemetry_decoder_gs::decode_subframe()
}
break;
case 4: // Possible IONOSPHERE and UTC model update (page 18)
if (d_nav.flag_iono_valid == true)
if (d_nav.get_flag_iono_valid() == true)
{
std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.flag_utc_model_valid == true)
if (d_nav.get_flag_utc_model_valid() == true)
{
std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
@@ -474,15 +474,15 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__
// 2. Add the telemetry decoder information
if (d_flag_preamble == true)
{
if (!(d_nav.d_TOW == 0))
if (!(d_nav.get_TOW() == 0))
{
d_TOW_at_current_symbol_ms = static_cast<uint32_t>(d_nav.d_TOW * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.d_TOW * 1000.0);
d_TOW_at_current_symbol_ms = static_cast<uint32_t>(d_nav.get_TOW() * 1000.0);
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.get_TOW() * 1000.0);
d_flag_TOW_set = true;
}
else
{
DLOG(INFO) << "Received GPS L1 TOW equal to zero at sat " << d_nav.i_satellite_PRN;
DLOG(INFO) << "Received GPS L1 TOW equal to zero at sat " << d_nav.get_satellite_PRN();
}
}
else

View File

@@ -76,13 +76,19 @@ private:
bool gps_word_parityCheck(uint32_t gpsword);
bool decode_subframe();
bool d_flag_frame_sync;
bool d_flag_parity;
bool d_flag_preamble;
bool d_sent_tlm_failed_msg;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_TOW_set;
bool d_dump;
Gps_Navigation_Message d_nav;
Gnss_Satellite d_satellite;
std::array<int32_t, GPS_CA_PREAMBLE_LENGTH_BITS> d_preamble_samples{};
std::string d_dump_filename;
std::ofstream d_dump_file;
boost::circular_buffer<float> d_symbol_history;
uint64_t d_sample_counter;
uint64_t d_preamble_index;
uint64_t d_last_valid_preamble;
int32_t d_bits_per_preamble;
int32_t d_samples_per_preamble;
@@ -98,19 +104,13 @@ private:
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_TOW_at_current_symbol_ms;
uint64_t d_sample_counter;
uint64_t d_preamble_index;
uint64_t d_last_valid_preamble;
std::array<int32_t, GPS_CA_PREAMBLE_LENGTH_BITS> d_preamble_samples{};
std::string d_dump_filename;
std::ofstream d_dump_file;
boost::circular_buffer<float> d_symbol_history;
Gps_Navigation_Message d_nav;
Gnss_Satellite d_satellite;
bool d_flag_frame_sync;
bool d_flag_parity;
bool d_flag_preamble;
bool d_sent_tlm_failed_msg;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_TOW_set;
bool d_dump;
};
#endif // GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_GS_H

View File

@@ -75,31 +75,31 @@ private:
gps_l2c_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump);
bool d_dump;
bool d_sent_tlm_failed_msg;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_valid_word;
int32_t d_channel;
int32_t d_state;
int32_t d_crc_error_count;
uint64_t d_sample_counter;
uint64_t d_last_valid_preamble;
uint32_t d_max_symbols_without_valid_frame;
double d_TOW_at_current_symbol;
double d_TOW_at_Preamble;
std::string d_dump_filename;
std::ofstream d_dump_file;
Gnss_Satellite d_satellite;
cnav_msg_decoder_t d_cnav_decoder{};
Gps_CNAV_Navigation_Message d_CNAV_Message;
std::string d_dump_filename;
std::ofstream d_dump_file;
double d_TOW_at_current_symbol;
double d_TOW_at_Preamble;
uint64_t d_sample_counter;
uint64_t d_last_valid_preamble;
int32_t d_channel;
int32_t d_state;
int32_t d_crc_error_count;
uint32_t d_max_symbols_without_valid_frame;
bool d_dump;
bool d_sent_tlm_failed_msg;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_valid_word;
};

View File

@@ -74,27 +74,28 @@ private:
gps_l5_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump);
bool d_dump;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_valid_word;
bool d_sent_tlm_failed_msg;
int32_t d_channel;
uint32_t d_TOW_at_current_symbol_ms;
uint32_t d_TOW_at_Preamble_ms;
uint64_t d_sample_counter;
uint64_t d_last_valid_preamble;
uint32_t d_max_symbols_without_valid_frame;
std::string d_dump_filename;
std::ofstream d_dump_file;
cnav_msg_decoder_t d_cnav_decoder{};
Gnss_Satellite d_satellite;
Gps_CNAV_Navigation_Message d_CNAV_Message;
std::string d_dump_filename;
std::ofstream d_dump_file;
uint64_t d_sample_counter;
uint64_t d_last_valid_preamble;
int32_t d_channel;
uint32_t d_TOW_at_current_symbol_ms;
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_max_symbols_without_valid_frame;
bool d_flag_PLL_180_deg_phase_locked;
bool d_flag_valid_word;
bool d_sent_tlm_failed_msg;
bool d_dump;
};

View File

@@ -88,41 +88,30 @@ private:
bool acquire_secondary();
int32_t save_matfile();
bool d_pull_in_transitory;
bool d_corrected_doppler;
bool d_interchange_iq;
bool d_veml;
bool d_cloop;
bool d_secondary;
bool d_dump;
bool d_dump_mat;
bool d_acc_carrier_phase_initialized;
bool d_enable_extended_integration;
Cpu_Multicorrelator_Real_Codes d_multicorrelator_cpu;
Cpu_Multicorrelator_Real_Codes d_correlator_data_cpu; // for data channel
int32_t d_symbols_per_bit;
int32_t d_preamble_length_symbols;
int32_t d_state;
int32_t d_correlation_length_ms;
int32_t d_n_correlator_taps;
int32_t d_current_prn_length_samples;
int32_t d_extend_correlation_symbols_count;
int32_t d_current_symbol;
int32_t d_current_data_symbol;
int32_t d_cn0_estimation_counter;
int32_t d_carrier_lock_fail_counter;
int32_t d_code_lock_fail_counter;
Dll_Pll_Conf d_trk_parameters;
uint32_t d_channel;
uint32_t d_secondary_code_length;
uint32_t d_data_secondary_code_length;
uint32_t d_code_length_chips;
uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled)
Exponential_Smoother d_cn0_smoother;
Exponential_Smoother d_carrier_lock_test_smoother;
uint64_t d_sample_counter;
uint64_t d_acq_sample_stamp;
Tracking_loop_filter d_code_loop_filter;
Tracking_FLL_PLL_filter d_carrier_loop_filter;
float *d_prompt_data_shift;
float d_rem_carr_phase_rad;
Gnss_Synchro *d_acquisition_gnss_synchro;
volk_gnsssdr::vector<float> d_tracking_code;
volk_gnsssdr::vector<float> d_data_code;
volk_gnsssdr::vector<float> d_local_code_shift_chips;
volk_gnsssdr::vector<gr_complex> d_correlator_outs;
volk_gnsssdr::vector<gr_complex> d_Prompt_Data;
volk_gnsssdr::vector<gr_complex> d_Prompt_buffer;
boost::circular_buffer<float> d_dll_filt_history;
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
double d_signal_carrier_freq;
double d_code_period;
@@ -175,30 +164,41 @@ private:
std::ofstream d_dump_file;
boost::circular_buffer<float> d_dll_filt_history;
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
uint64_t d_sample_counter;
uint64_t d_acq_sample_stamp;
volk_gnsssdr::vector<float> d_tracking_code;
volk_gnsssdr::vector<float> d_data_code;
volk_gnsssdr::vector<float> d_local_code_shift_chips;
volk_gnsssdr::vector<gr_complex> d_correlator_outs;
volk_gnsssdr::vector<gr_complex> d_Prompt_Data;
volk_gnsssdr::vector<gr_complex> d_Prompt_buffer;
float *d_prompt_data_shift;
float d_rem_carr_phase_rad;
Cpu_Multicorrelator_Real_Codes d_multicorrelator_cpu;
Cpu_Multicorrelator_Real_Codes d_correlator_data_cpu; // for data channel
int32_t d_symbols_per_bit;
int32_t d_preamble_length_symbols;
int32_t d_state;
int32_t d_correlation_length_ms;
int32_t d_n_correlator_taps;
int32_t d_current_prn_length_samples;
int32_t d_extend_correlation_symbols_count;
int32_t d_current_symbol;
int32_t d_current_data_symbol;
int32_t d_cn0_estimation_counter;
int32_t d_carrier_lock_fail_counter;
int32_t d_code_lock_fail_counter;
Dll_Pll_Conf d_trk_parameters;
uint32_t d_channel;
uint32_t d_secondary_code_length;
uint32_t d_data_secondary_code_length;
uint32_t d_code_length_chips;
uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled)
Exponential_Smoother d_cn0_smoother;
Exponential_Smoother d_carrier_lock_test_smoother;
Gnss_Synchro *d_acquisition_gnss_synchro;
Tracking_loop_filter d_code_loop_filter;
Tracking_FLL_PLL_filter d_carrier_loop_filter;
bool d_pull_in_transitory;
bool d_corrected_doppler;
bool d_interchange_iq;
bool d_veml;
bool d_cloop;
bool d_secondary;
bool d_dump;
bool d_dump_mat;
bool d_acc_carrier_phase_initialized;
bool d_enable_extended_integration;
};
#endif // GNSS_SDR_DLL_PLL_VEML_TRACKING_H

View File

@@ -117,50 +117,41 @@ private:
void log_data();
int32_t save_matfile();
bool d_veml;
bool d_cloop;
bool d_secondary;
bool d_enable_extended_integration;
bool d_dump;
bool d_dump_mat;
bool d_pull_in_transitory;
bool d_corrected_doppler;
bool d_interchange_iq;
bool d_acc_carrier_phase_initialized;
bool d_worker_is_done;
bool d_extended_correlation_in_fpga;
bool d_current_extended_correlation_in_fpga;
bool d_stop_tracking;
bool d_sc_demodulate_enabled;
Dll_Pll_Conf_Fpga d_trk_parameters;
int32_t d_symbols_per_bit;
int32_t d_state;
int32_t d_extend_correlation_symbols_count;
int32_t d_current_symbol;
int32_t d_current_data_symbol;
int32_t d_current_integration_length_samples;
int32_t d_cn0_estimation_counter;
int32_t d_carrier_lock_fail_counter;
int32_t d_code_lock_fail_counter;
int32_t d_correlation_length_ms;
int32_t d_n_correlator_taps;
int32_t d_next_integration_length_samples;
int32_t d_extend_fpga_integration_periods;
Exponential_Smoother d_cn0_smoother;
Exponential_Smoother d_carrier_lock_test_smoother;
uint32_t d_channel;
uint32_t d_secondary_code_length;
uint32_t d_data_secondary_code_length;
uint32_t d_code_length_chips;
uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled)
uint32_t d_fpga_integration_period;
uint32_t d_current_fpga_integration_period;
Gnss_Synchro *d_acquisition_gnss_synchro;
uint64_t d_sample_counter;
uint64_t d_acq_sample_stamp;
uint64_t d_sample_counter_next;
Tracking_loop_filter d_code_loop_filter;
float *d_prompt_data_shift;
float d_rem_carr_phase_rad;
Tracking_FLL_PLL_filter d_carrier_loop_filter;
volk_gnsssdr::vector<float> d_local_code_shift_chips;
volk_gnsssdr::vector<gr_complex> d_correlator_outs;
volk_gnsssdr::vector<gr_complex> d_Prompt_Data;
volk_gnsssdr::vector<gr_complex> d_Prompt_buffer;
boost::circular_buffer<float> d_dll_filt_history;
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
std::string d_systemName;
std::string d_signal_type;
std::string *d_secondary_code_string;
std::string *d_data_secondary_code_string;
std::string d_signal_pretty_name;
std::string d_dump_filename;
std::ofstream d_dump_file;
std::shared_ptr<Fpga_Multicorrelator_8sc> d_multicorrelator_fpga;
boost::condition_variable d_m_condition;
boost::mutex d_mutex;
double d_signal_carrier_freq;
double d_code_period;
@@ -205,41 +196,50 @@ private:
gr_complex d_VL_accu;
gr_complex d_P_data_accu;
std::string d_systemName;
std::string d_signal_type;
std::string *d_secondary_code_string;
std::string *d_data_secondary_code_string;
std::string d_signal_pretty_name;
std::string d_dump_filename;
uint64_t d_sample_counter;
uint64_t d_acq_sample_stamp;
uint64_t d_sample_counter_next;
std::ofstream d_dump_file;
float *d_prompt_data_shift;
float d_rem_carr_phase_rad;
std::shared_ptr<Fpga_Multicorrelator_8sc> d_multicorrelator_fpga;
int32_t d_symbols_per_bit;
int32_t d_state;
int32_t d_extend_correlation_symbols_count;
int32_t d_current_symbol;
int32_t d_current_data_symbol;
int32_t d_current_integration_length_samples;
int32_t d_cn0_estimation_counter;
int32_t d_carrier_lock_fail_counter;
int32_t d_code_lock_fail_counter;
int32_t d_correlation_length_ms;
int32_t d_n_correlator_taps;
int32_t d_next_integration_length_samples;
int32_t d_extend_fpga_integration_periods;
volk_gnsssdr::vector<float> d_local_code_shift_chips;
volk_gnsssdr::vector<gr_complex> d_correlator_outs;
volk_gnsssdr::vector<gr_complex> d_Prompt_Data;
volk_gnsssdr::vector<gr_complex> d_Prompt_buffer;
uint32_t d_channel;
uint32_t d_secondary_code_length;
uint32_t d_data_secondary_code_length;
uint32_t d_code_length_chips;
uint32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled)
uint32_t d_fpga_integration_period;
uint32_t d_current_fpga_integration_period;
boost::circular_buffer<float> d_dll_filt_history;
boost::circular_buffer<std::pair<double, double>> d_code_ph_history;
boost::circular_buffer<std::pair<double, double>> d_carr_ph_history;
boost::circular_buffer<gr_complex> d_Prompt_circular_buffer;
boost::condition_variable d_m_condition;
boost::mutex d_mutex;
Dll_Pll_Conf_Fpga d_trk_parameters;
Exponential_Smoother d_cn0_smoother;
Exponential_Smoother d_carrier_lock_test_smoother;
Gnss_Synchro *d_acquisition_gnss_synchro;
Tracking_loop_filter d_code_loop_filter;
Tracking_FLL_PLL_filter d_carrier_loop_filter;
bool d_veml;
bool d_cloop;
bool d_secondary;
bool d_enable_extended_integration;
bool d_dump;
bool d_dump_mat;
bool d_pull_in_transitory;
bool d_corrected_doppler;
bool d_interchange_iq;
bool d_acc_carrier_phase_initialized;
bool d_worker_is_done;
bool d_extended_correlation_in_fpga;
bool d_current_extended_correlation_in_fpga;
bool d_stop_tracking;
bool d_sc_demodulate_enabled;
};
#endif // GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H

View File

@@ -45,8 +45,8 @@ public:
private:
// Allocate the device input vectors
const std::complex<float> *d_sig_in;
std::complex<float> **d_local_codes_resampled;
const std::complex<float> *d_local_code_in;
std::complex<float> **d_local_codes_resampled;
std::complex<float> *d_corr_out;
float *d_shifts_chips;
int d_code_length_chips;

View File

@@ -45,8 +45,8 @@ public:
private:
// Allocate the device input vectors
const lv_16sc_t *d_sig_in;
lv_16sc_t **d_local_codes_resampled;
const lv_16sc_t *d_local_code_in;
lv_16sc_t **d_local_codes_resampled;
lv_16sc_t *d_corr_out;
float *d_shifts_chips;
int d_code_length_chips;

View File

@@ -48,13 +48,13 @@ public:
private:
// Allocate the device input vectors
const std::complex<float> *d_sig_in;
float **d_local_codes_resampled;
const float *d_local_code_in;
std::complex<float> *d_corr_out;
float **d_local_codes_resampled;
float *d_shifts_chips;
bool d_use_high_dynamics_resampler;
int d_code_length_chips;
int d_n_correlators;
bool d_use_high_dynamics_resampler;
};

View File

@@ -33,18 +33,10 @@ public:
void SetFromConfiguration(ConfigurationInterface *configuration, const std::string &role);
/* DLL/PLL tracking configuration */
int32_t fll_filter_order;
bool enable_fll_pull_in;
bool enable_fll_steady_state;
uint32_t pull_in_time_s;
uint32_t bit_synchronization_time_limit_s;
int32_t pll_filter_order;
int32_t dll_filter_order;
double fs_in;
uint32_t vector_length;
bool dump;
bool dump_mat;
std::string item_type;
std::string dump_filename;
double fs_in;
double carrier_lock_th;
float pll_pull_in_bw_hz;
float dll_pull_in_bw_hz;
float fll_bw_hz;
@@ -59,24 +51,32 @@ public:
float slope;
float spc;
float y_intercept;
float cn0_smoother_alpha;
float carrier_lock_test_smoother_alpha;
uint32_t pull_in_time_s;
uint32_t bit_synchronization_time_limit_s;
uint32_t vector_length;
uint32_t smoother_length;
int32_t fll_filter_order;
int32_t pll_filter_order;
int32_t dll_filter_order;
int32_t extend_correlation_symbols;
bool carrier_aiding;
bool high_dyn;
std::string item_type;
int32_t cn0_samples;
int32_t cn0_smoother_samples;
float cn0_smoother_alpha;
int32_t carrier_lock_test_smoother_samples;
float carrier_lock_test_smoother_alpha;
int32_t cn0_min;
int32_t max_code_lock_fail;
int32_t max_carrier_lock_fail;
uint32_t smoother_length;
double carrier_lock_th;
char signal[3]{};
char system;
bool enable_fll_pull_in;
bool enable_fll_steady_state;
bool track_pilot;
bool enable_doppler_correction;
char system;
char signal[3]{};
bool carrier_aiding;
bool high_dyn;
bool dump;
bool dump_mat;
};
#endif

View File

@@ -35,19 +35,12 @@ public:
void SetFromConfiguration(ConfigurationInterface* configuration, const std::string& role);
/* DLL/PLL tracking configuration */
int32_t fll_filter_order;
bool enable_fll_pull_in;
bool enable_fll_steady_state;
uint32_t pull_in_time_s; // signed integer, when pull in time is not yet reached it has to be compared against a negative number
uint32_t bit_synchronization_time_limit_s;
int32_t pll_filter_order;
int32_t dll_filter_order;
std::string device_name;
std::string dump_filename;
double fs_in;
uint32_t vector_length;
bool dump;
bool dump_mat;
std::string dump_filename;
double carrier_lock_th;
float pll_pull_in_bw_hz;
float dll_pull_in_bw_hz;
float fll_bw_hz;
@@ -62,36 +55,47 @@ public:
float slope;
float spc;
float y_intercept;
int32_t extend_correlation_symbols;
bool carrier_aiding;
bool high_dyn;
int32_t cn0_samples;
int32_t cn0_min;
int32_t max_code_lock_fail;
int32_t max_carrier_lock_fail;
int32_t cn0_smoother_samples;
float cn0_smoother_alpha;
int32_t carrier_lock_test_smoother_samples;
float carrier_lock_test_smoother_alpha;
// int32_t max_lock_fail;
uint32_t pull_in_time_s; // signed integer, when pull in time is not yet reached it has to be compared against a negative number
uint32_t bit_synchronization_time_limit_s;
uint32_t vector_length;
uint32_t smoother_length;
double carrier_lock_th;
bool track_pilot;
bool enable_doppler_correction;
char system;
char signal[3];
std::string device_name;
uint32_t dev_file_num;
uint32_t num_prev_assigned_ch;
uint32_t code_length_chips;
uint32_t code_samples_per_chip;
int32_t* ca_codes;
int32_t* data_codes;
bool extended_correlation_in_fpga;
uint32_t extend_fpga_integration_periods;
uint32_t fpga_integration_period;
int32_t fll_filter_order;
int32_t pll_filter_order;
int32_t dll_filter_order;
int32_t extend_correlation_symbols;
int32_t cn0_samples;
int32_t cn0_min;
int32_t max_code_lock_fail;
int32_t max_carrier_lock_fail;
int32_t cn0_smoother_samples;
int32_t carrier_lock_test_smoother_samples;
// int32_t max_lock_fail;
int32_t* ca_codes;
int32_t* data_codes;
char signal[3];
char system;
bool extended_correlation_in_fpga;
bool track_pilot;
bool enable_doppler_correction;
bool enable_fll_pull_in;
bool enable_fll_steady_state;
bool carrier_aiding;
bool high_dyn;
bool dump;
bool dump_mat;
};
#endif

View File

@@ -36,8 +36,12 @@
class Exponential_Smoother
{
public:
Exponential_Smoother(); //!< Constructor
~Exponential_Smoother() = default; //!< Destructor
Exponential_Smoother(); //!< Constructor
~Exponential_Smoother() = default; //!< Destructor
Exponential_Smoother(Exponential_Smoother&&) = default; //!< Move operator
Exponential_Smoother& operator=(Exponential_Smoother&& /*other*/) = default; //!< Move assignment operator
void set_alpha(float alpha); //!< 0 < alpha < 1. The higher, the most responsive, but more variance. Default value: 0.001
void set_samples_for_initialization(int num_samples); //!< Number of samples averaged for initialization. Default value: 200
void reset();
@@ -45,18 +49,17 @@ public:
void set_offset(float offset);
float smooth(float raw);
double smooth(double raw);
Exponential_Smoother(Exponential_Smoother&&) = default; //!< Move operator
Exponential_Smoother& operator=(Exponential_Smoother&& /*other*/) = default; //!< Move assignment operator
private:
std::vector<float> init_buffer_;
float alpha_; // takes value 0.0001 if not set
int samples_for_initialization_;
float one_minus_alpha_;
float old_value_;
float min_value_;
float offset_;
bool initializing_;
int samples_for_initialization_;
int init_counter_;
std::vector<float> init_buffer_;
bool initializing_;
};
#endif // GNSS_SDR_EXPONENTIAL_SMOOTHER_H

View File

@@ -193,10 +193,36 @@ private:
static const uint32_t local_code_fpga_clear_address_counter = 0x10000000;
static const uint32_t test_register_track_writeval = 0x55AA;
// private functions
uint32_t fpga_acquisition_test_register(uint32_t writeval);
void fpga_configure_tracking_gps_local_code(int32_t PRN);
void fpga_compute_code_shift_parameters();
void fpga_configure_code_parameters_in_fpga();
void fpga_compute_signal_parameters_in_fpga();
void fpga_configure_signal_parameters_in_fpga();
void fpga_launch_multicorrelator_fpga();
void read_tracking_gps_results();
void close_device(void);
void write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr);
volk_gnsssdr::vector<uint32_t> d_initial_index;
volk_gnsssdr::vector<uint32_t> d_initial_interp_counter;
uint64_t d_initial_sample_counter;
gr_complex *d_corr_out;
gr_complex *d_Prompt_Data;
float *d_shifts_chips;
float *d_prompt_data_shift;
float d_rem_code_phase_chips;
float d_code_phase_step_chips;
float d_code_phase_rate_step_chips;
float d_rem_carrier_phase_in_rad;
float d_phase_step_rad;
float d_carrier_phase_rate_step_rad;
uint32_t d_code_length_chips;
uint32_t d_code_length_samples;
uint32_t d_n_correlators; // number of correlators
@@ -208,24 +234,13 @@ private:
// configuration data received from the interface
uint32_t d_channel; // channel number
uint32_t d_correlator_length_samples;
float d_rem_code_phase_chips;
float d_code_phase_step_chips;
float d_code_phase_rate_step_chips;
float d_rem_carrier_phase_in_rad;
float d_phase_step_rad;
float d_carrier_phase_rate_step_rad;
uint32_t d_code_samples_per_chip;
bool d_track_pilot;
// configuration data computed in the format that the FPGA expects
volk_gnsssdr::vector<uint32_t> d_initial_index;
volk_gnsssdr::vector<uint32_t> d_initial_interp_counter;
uint32_t d_code_phase_step_chips_num;
uint32_t d_code_phase_rate_step_chips_num;
int32_t d_rem_carr_phase_rad_int;
int32_t d_phase_step_rad_int;
int32_t d_carrier_phase_rate_step_rad_int;
uint64_t d_initial_sample_counter;
// driver
std::string d_device_name;
@@ -239,19 +254,9 @@ private:
// secondary code configuration
uint32_t d_secondary_code_0_length;
uint32_t d_secondary_code_1_length;
bool d_secondary_code_enabled;
// private functions
uint32_t fpga_acquisition_test_register(uint32_t writeval);
void fpga_configure_tracking_gps_local_code(int32_t PRN);
void fpga_compute_code_shift_parameters();
void fpga_configure_code_parameters_in_fpga();
void fpga_compute_signal_parameters_in_fpga();
void fpga_configure_signal_parameters_in_fpga();
void fpga_launch_multicorrelator_fpga();
void read_tracking_gps_results();
void close_device(void);
void write_secondary_code(uint32_t secondary_code_length, std::string *secondary_code_string, uint32_t reg_addr);
bool d_track_pilot;
bool d_secondary_code_enabled;
};
#endif // GNSS_SDR_FPGA_MULTICORRELATOR_H

View File

@@ -37,15 +37,18 @@
class Tracking_2nd_DLL_filter
{
public:
Tracking_2nd_DLL_filter();
~Tracking_2nd_DLL_filter() = default;
explicit Tracking_2nd_DLL_filter(float pdi_code);
void set_DLL_BW(float dll_bw_hz); //!< Set DLL filter bandwidth [Hz]
void set_pdi(float pdi_code); //!< Set Summation interval for code [s]
void initialize(); //!< Start tracking with acquisition information
float get_code_nco(float DLL_discriminator); //!< Numerically controlled oscillator
explicit Tracking_2nd_DLL_filter(float pdi_code);
Tracking_2nd_DLL_filter();
~Tracking_2nd_DLL_filter() = default;
private:
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k);
// PLL filter parameters
float d_tau1_code = 0.0;
float d_tau2_code = 0.0;
@@ -54,7 +57,6 @@ private:
float d_dlldampingratio = 0.0;
float d_old_code_error = 0.0;
float d_old_code_nco = 0.0;
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k);
};
#endif

View File

@@ -36,15 +36,17 @@
class Tracking_2nd_PLL_filter
{
public:
Tracking_2nd_PLL_filter();
~Tracking_2nd_PLL_filter() = default;
explicit Tracking_2nd_PLL_filter(float pdi_carr);
void set_PLL_BW(float pll_bw_hz); //!< Set PLL loop bandwidth [Hz]
void set_pdi(float pdi_carr); //!< Set Summation interval for code [s]
void initialize();
float get_carrier_nco(float PLL_discriminator);
explicit Tracking_2nd_PLL_filter(float pdi_carr);
Tracking_2nd_PLL_filter();
~Tracking_2nd_PLL_filter() = default;
private:
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k);
// PLL filter parameters
float d_tau1_carr = 0.0;
float d_tau2_carr = 0.0;
@@ -53,7 +55,6 @@ private:
float d_plldampingratio = 0.0;
float d_old_carr_error = 0.0;
float d_old_carr_nco = 0.0;
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta, float k);
};
#endif

View File

@@ -26,15 +26,14 @@
class Tracking_FLL_PLL_filter
{
public:
Tracking_FLL_PLL_filter();
~Tracking_FLL_PLL_filter() = default;
void set_params(float fll_bw_hz, float pll_bw_hz, int order);
void initialize(float d_acq_carrier_doppler_hz);
float get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s);
Tracking_FLL_PLL_filter();
~Tracking_FLL_PLL_filter() = default;
private:
// FLL + PLL filter parameters
int d_order;
float d_pll_w;
float d_pll_w0p3;
float d_pll_w0f2;
@@ -45,6 +44,7 @@ private:
float d_pll_w0p2;
float d_pll_b3;
float d_pll_w0p;
int d_order;
};
#endif

View File

@@ -33,11 +33,11 @@ Tracking_loop_filter::Tracking_loop_filter(float update_interval,
float noise_bandwidth,
int loop_order,
bool include_last_integrator)
: d_loop_order(loop_order),
: d_noise_bandwidth(noise_bandwidth),
d_update_interval(update_interval),
d_loop_order(loop_order),
d_current_index(0),
d_include_last_integrator(include_last_integrator),
d_noise_bandwidth(noise_bandwidth),
d_update_interval(update_interval)
d_include_last_integrator(include_last_integrator)
{
d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);
d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);
@@ -46,11 +46,11 @@ Tracking_loop_filter::Tracking_loop_filter(float update_interval,
Tracking_loop_filter::Tracking_loop_filter()
: d_loop_order(2),
: d_noise_bandwidth(15.0),
d_update_interval(0.001),
d_loop_order(2),
d_current_index(0),
d_include_last_integrator(false),
d_noise_bandwidth(15.0),
d_update_interval(0.001)
d_include_last_integrator(false)
{
d_inputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);
d_outputs.resize(MAX_LOOP_HISTORY_LENGTH, 0.0);

View File

@@ -57,6 +57,9 @@ public:
float apply(float current_input);
private:
// Compute the filter coefficients:
void update_coefficients();
// Store the last inputs and outputs:
std::vector<float> d_inputs;
std::vector<float> d_outputs;
@@ -65,15 +68,6 @@ private:
std::vector<float> d_input_coefficients;
std::vector<float> d_output_coefficients;
// The loop order:
int d_loop_order;
// The current index in the i/o arrays:
int d_current_index;
// Should the last integrator be included?
bool d_include_last_integrator;
// The noise bandwidth (in Hz)
// Note this is an approximation only valid when the product of this
// number and the update interval (T) is small.
@@ -82,8 +76,14 @@ private:
// Loop update interval
float d_update_interval;
// Compute the filter coefficients:
void update_coefficients();
// The loop order:
int d_loop_order;
// The current index in the i/o arrays:
int d_current_index;
// Should the last integrator be included?
bool d_include_last_integrator;
};
#endif