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
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
89 changed files with 1891 additions and 1600 deletions

View File

@ -41,7 +41,7 @@
<xs:element type="xs:int" name="TOW_5"/>
<xs:element type="xs:float" name="Galileo_satClkDrift"/>
<xs:element type="xs:float" name="Galileo_dtr"/>
<xs:element type="xs:byte" name="flag_all_ephemeris"/>
<xs:element type="xs:byte" name="IOD_ephemeris"/>
<xs:element type="xs:byte" name="IOD_nav_1"/>
<xs:element type="xs:byte" name="SISA_3"/>
@ -53,6 +53,7 @@
<xs:element type="xs:byte" name="E1B_DVS_5"/>
<xs:element type="xs:float" name="BGD_E1E5a_5"/>
<xs:element type="xs:float" name="BGD_E1E5b_5"/>
<xs:element type="xs:byte" name="flag_all_ephemeris"/>
</xs:sequence>
<xs:attribute type="xs:byte" name="class_id" use="optional"/>
<xs:attribute type="xs:byte" name="tracking_level" use="optional"/>

View File

@ -10,13 +10,13 @@
<xs:element type="xs:float" name="ai0_5"/>
<xs:element type="xs:float" name="ai1_5"/>
<xs:element type="xs:float" name="ai2_5"/>
<xs:element type="xs:int" name="TOW_5"/>
<xs:element type="xs:short" name="WN_5"/>
<xs:element type="xs:byte" name="Region1_flag_5"/>
<xs:element type="xs:byte" name="Region2_flag_5"/>
<xs:element type="xs:byte" name="Region3_flag_5"/>
<xs:element type="xs:byte" name="Region4_flag_5"/>
<xs:element type="xs:byte" name="Region5_flag_5"/>
<xs:element type="xs:int" name="TOW_5"/>
<xs:element type="xs:short" name="WN_5"/>
</xs:sequence>
<xs:attribute type="xs:byte" name="class_id"/>
<xs:attribute type="xs:byte" name="tracking_level"/>

View File

@ -7,7 +7,6 @@
<xs:element name="GNSS-SDR_utc_model">
<xs:complexType>
<xs:sequence>
<xs:element type="xs:byte" name="valid"/>
<xs:element type="xs:float" name="d_A1"/>
<xs:element type="xs:float" name="d_A0"/>
<xs:element type="xs:int" name="d_t_OT"/>
@ -16,6 +15,7 @@
<xs:element type="xs:short" name="i_WN_LSF"/>
<xs:element type="xs:byte" name="i_DN"/>
<xs:element type="xs:byte" name="d_DeltaT_LSF"/>
<xs:element type="xs:byte" name="valid"/>
</xs:sequence>
<xs:attribute type="xs:byte" name="class_id"/>
<xs:attribute type="xs:byte" name="tracking_level"/>

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

View File

@ -155,6 +155,8 @@ target_link_libraries(core_receiver
Armadillo::armadillo
)
target_include_directories(core_receiver PRIVATE ${CMAKE_SOURCE_DIR}/src/algorithms/libs)
if(ENABLE_ARMA_NO_DEBUG)
target_compile_definitions(core_receiver
PRIVATE -DARMA_NO_BOUND_CHECKING=1

View File

@ -357,6 +357,7 @@ int ControlThread::run()
fpga_helper_thread_.try_join_until(boost::chrono::steady_clock::now() + boost::chrono::milliseconds(1000));
#endif
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// Terminate keyboard thread
pthread_t id = keyboard_thread_.native_handle();
keyboard_thread_.detach();

View File

@ -152,19 +152,14 @@ private:
std::thread sysv_queue_thread_;
std::thread gps_acq_assist_data_collector_thread_;
#ifdef ENABLE_FPGA
boost::thread fpga_helper_thread_;
#endif
std::shared_ptr<GNSSFlowgraph> flowgraph_;
std::shared_ptr<ConfigurationInterface> configuration_;
std::shared_ptr<Concurrent_Queue<pmt::pmt_t>> control_queue_;
bool receiver_on_standby_;
bool pre_2009_file_; // to override the system time to postprocess old gnss records and avoid wrong week rollover
bool stop_;
bool restart_;
bool delete_configuration_;
unsigned int processed_control_messages_;
unsigned int applied_actions_;
int msqid_;
// default filename for assistance data
const std::string eph_default_xml_filename_ = "./gps_ephemeris.xml";
const std::string utc_default_xml_filename_ = "./gps_utc_model.xml";
@ -194,9 +189,15 @@ private:
Agnss_Ref_Location agnss_ref_location_;
Agnss_Ref_Time agnss_ref_time_;
#ifdef ENABLE_FPGA
boost::thread fpga_helper_thread_;
#endif
unsigned int processed_control_messages_;
unsigned int applied_actions_;
int msqid_;
bool receiver_on_standby_;
bool pre_2009_file_; // to override the system time to postprocess old gnss records and avoid wrong week rollover
bool stop_;
bool restart_;
bool delete_configuration_;
};
#endif // GNSS_SDR_CONTROL_THREAD_H

View File

@ -23,9 +23,7 @@
*/
#include "file_configuration.h"
#include "INIReader.h"
#include "in_memory_configuration.h"
#include "string_converter.h"
#include "gnss_sdr_make_unique.h"
#include <glog/logging.h>
#include <utility>
@ -44,6 +42,27 @@ FileConfiguration::FileConfiguration()
}
void FileConfiguration::init()
{
converter_ = std::make_unique<StringConverter>();
overrided_ = std::make_unique<InMemoryConfiguration>();
ini_reader_ = std::make_unique<INIReader>(filename_);
error_ = ini_reader_->ParseError();
if (error_ == 0)
{
DLOG(INFO) << "Configuration file " << filename_ << " opened with no errors";
}
else if (error_ > 0)
{
LOG(WARNING) << "Configuration file " << filename_ << " contains errors in line " << error_;
}
else
{
LOG(WARNING) << "Unable to open configuration file " << filename_;
}
}
std::string FileConfiguration::property(std::string property_name, std::string default_value)
{
if (overrided_->is_present(property_name))
@ -157,24 +176,3 @@ void FileConfiguration::set_property(std::string property_name, std::string valu
{
overrided_->set_property(property_name, value);
}
void FileConfiguration::init()
{
converter_ = std::make_shared<StringConverter>();
overrided_ = std::make_shared<InMemoryConfiguration>();
ini_reader_ = std::make_shared<INIReader>(filename_);
error_ = ini_reader_->ParseError();
if (error_ == 0)
{
DLOG(INFO) << "Configuration file " << filename_ << " opened with no errors";
}
else if (error_ > 0)
{
LOG(WARNING) << "Configuration file " << filename_ << " contains errors in line " << error_;
}
else
{
LOG(WARNING) << "Unable to open configuration file " << filename_;
}
}

View File

@ -25,15 +25,14 @@
#ifndef GNSS_SDR_FILE_CONFIGURATION_H
#define GNSS_SDR_FILE_CONFIGURATION_H
#include "INIReader.h"
#include "configuration_interface.h"
#include "in_memory_configuration.h"
#include "string_converter.h"
#include <cstdint>
#include <memory>
#include <string>
class INIReader;
class StringConverter;
class InMemoryConfiguration;
/*!
* \brief This class is an implementation of the interface ConfigurationInterface
*
@ -64,9 +63,9 @@ public:
private:
void init();
std::string filename_;
std::shared_ptr<INIReader> ini_reader_;
std::shared_ptr<InMemoryConfiguration> overrided_;
std::shared_ptr<StringConverter> converter_;
std::unique_ptr<INIReader> ini_reader_;
std::unique_ptr<InMemoryConfiguration> overrided_;
std::unique_ptr<StringConverter> converter_;
int error_{};
};

View File

@ -176,21 +176,6 @@ private:
std::vector<std::string> split_string(const std::string& s, char delim);
bool connected_;
bool running_;
bool multiband_;
bool enable_monitor_;
int sources_count_;
unsigned int channels_count_;
unsigned int acq_channels_count_;
unsigned int max_acq_channels_;
std::string config_file_;
std::mutex signal_list_mutex_;
std::vector<std::shared_ptr<GNSSBlockInterface>> sig_source_;
std::vector<std::shared_ptr<GNSSBlockInterface>> sig_conditioner_;
std::vector<gr::blocks::null_sink::sptr> null_sinks_;
@ -236,6 +221,21 @@ private:
#if ENABLE_FPGA
gnss_sdr_fpga_sample_counter_sptr ch_out_fpga_sample_counter_;
#endif
std::string config_file_;
std::mutex signal_list_mutex_;
int sources_count_;
unsigned int channels_count_;
unsigned int acq_channels_count_;
unsigned int max_acq_channels_;
bool connected_;
bool running_;
bool multiband_;
bool enable_monitor_;
};
#endif // GNSS_SDR_GNSS_FLOWGRAPH_H

View File

@ -20,14 +20,13 @@
#include "in_memory_configuration.h"
#include "string_converter.h"
#include "gnss_sdr_make_unique.h"
#include <utility>
InMemoryConfiguration::InMemoryConfiguration()
{
std::unique_ptr<StringConverter> converter_aux(new StringConverter);
converter_ = std::move(converter_aux);
converter_ = std::make_unique<StringConverter>();
}

View File

@ -25,14 +25,13 @@
#define GNSS_SDR_IN_MEMORY_CONFIGURATION_H
#include "configuration_interface.h"
#include "string_converter.h"
#include <cstdint>
#include <map>
#include <memory>
#include <string>
class StringConverter;
/*!
* \brief This class is an implementation of the interface ConfigurationInterface.
*

View File

@ -49,21 +49,21 @@ TcpCmdInterface::TcpCmdInterface()
void TcpCmdInterface::register_functions()
{
#if HAS_GENERIC_LAMBDA
functions["status"] = [&](auto &s) { return TcpCmdInterface::status(s); };
functions["standby"] = [&](auto &s) { return TcpCmdInterface::standby(s); };
functions["reset"] = [&](auto &s) { return TcpCmdInterface::reset(s); };
functions["hotstart"] = [&](auto &s) { return TcpCmdInterface::hotstart(s); };
functions["warmstart"] = [&](auto &s) { return TcpCmdInterface::warmstart(s); };
functions["coldstart"] = [&](auto &s) { return TcpCmdInterface::coldstart(s); };
functions["set_ch_satellite"] = [&](auto &s) { return TcpCmdInterface::set_ch_satellite(s); };
functions_["status"] = [&](auto &s) { return TcpCmdInterface::status(s); };
functions_["standby"] = [&](auto &s) { return TcpCmdInterface::standby(s); };
functions_["reset"] = [&](auto &s) { return TcpCmdInterface::reset(s); };
functions_["hotstart"] = [&](auto &s) { return TcpCmdInterface::hotstart(s); };
functions_["warmstart"] = [&](auto &s) { return TcpCmdInterface::warmstart(s); };
functions_["coldstart"] = [&](auto &s) { return TcpCmdInterface::coldstart(s); };
functions_["set_ch_satellite"] = [&](auto &s) { return TcpCmdInterface::set_ch_satellite(s); };
#else
functions["status"] = std::bind(&TcpCmdInterface::status, this, std::placeholders::_1);
functions["standby"] = std::bind(&TcpCmdInterface::standby, this, std::placeholders::_1);
functions["reset"] = std::bind(&TcpCmdInterface::reset, this, std::placeholders::_1);
functions["hotstart"] = std::bind(&TcpCmdInterface::hotstart, this, std::placeholders::_1);
functions["warmstart"] = std::bind(&TcpCmdInterface::warmstart, this, std::placeholders::_1);
functions["coldstart"] = std::bind(&TcpCmdInterface::coldstart, this, std::placeholders::_1);
functions["set_ch_satellite"] = std::bind(&TcpCmdInterface::set_ch_satellite, this, std::placeholders::_1);
functions_["status"] = std::bind(&TcpCmdInterface::status, this, std::placeholders::_1);
functions_["standby"] = std::bind(&TcpCmdInterface::standby, this, std::placeholders::_1);
functions_["reset"] = std::bind(&TcpCmdInterface::reset, this, std::placeholders::_1);
functions_["hotstart"] = std::bind(&TcpCmdInterface::hotstart, this, std::placeholders::_1);
functions_["warmstart"] = std::bind(&TcpCmdInterface::warmstart, this, std::placeholders::_1);
functions_["coldstart"] = std::bind(&TcpCmdInterface::coldstart, this, std::placeholders::_1);
functions_["set_ch_satellite"] = std::bind(&TcpCmdInterface::set_ch_satellite, this, std::placeholders::_1);
#endif
}
@ -355,7 +355,7 @@ void TcpCmdInterface::run_cmd_server(int tcp_port)
}
else
{
response = functions[cmd_vector.at(0)](cmd_vector);
response = functions_[cmd_vector.at(0)](cmd_vector);
}
}
catch (const std::bad_function_call &ex)

View File

@ -56,7 +56,7 @@ public:
private:
std::unordered_map<std::string, std::function<std::string(const std::vector<std::string> &)>>
functions;
functions_;
std::string status(const std::vector<std::string> &commandLine);
std::string reset(const std::vector<std::string> &commandLine);
std::string standby(const std::vector<std::string> &commandLine);
@ -68,15 +68,15 @@ private:
void register_functions();
std::shared_ptr<Concurrent_Queue<pmt::pmt_t>> control_queue_;
bool keep_running_;
time_t receiver_utc_time_;
std::shared_ptr<PvtInterface> PVT_sptr_;
float rx_latitude_;
float rx_longitude_;
float rx_altitude_;
std::shared_ptr<PvtInterface> PVT_sptr_;
time_t receiver_utc_time_;
bool keep_running_;
};
#endif // GNSS_SDR_TCP_CMD_INTERFACE_H

View File

@ -36,10 +36,10 @@ public:
*/
Agnss_Ref_Location() = default;
bool valid{};
double lat{};
double lon{};
double uncertainty{};
bool valid{};
template <class Archive>
@ -52,10 +52,10 @@ public:
if (version)
{
};
archive& make_nvp("valid", valid);
archive& make_nvp("lat", lat);
archive& make_nvp("lon", lon);
archive& make_nvp("uncertainty", uncertainty);
archive& make_nvp("valid", valid);
}
};

View File

@ -36,11 +36,11 @@ public:
*/
Agnss_Ref_Time() = default;
bool valid{};
double d_TOW{};
double d_Week{};
double d_tv_sec{};
double d_tv_usec{};
bool valid{};
template <class Archive>
@ -53,11 +53,11 @@ public:
if (version)
{
};
archive& make_nvp("valid", valid);
archive& make_nvp("d_TOW", d_TOW);
archive& make_nvp("d_Week", d_Week);
archive& make_nvp("d_tv_sec", d_tv_sec);
archive& make_nvp("d_tv_usec", d_tv_usec);
archive& make_nvp("valid", valid);
}
};

View File

@ -41,6 +41,25 @@ public:
*/
Beidou_Dnav_Ephemeris();
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200K)
* and compute the clock bias term including relativistic effect (return value)
*/
double satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_drift(double transmitTime);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
unsigned int i_satellite_PRN{}; //!< SV PRN NUMBER
double d_TOW{}; //!< Time of BEIDOU Week of the ephemeris set (taken from subframes TOW) [s]
double d_Crs{}; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
@ -110,24 +129,6 @@ public:
std::map<int, std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200K)
* and compute the clock bias term including relativistic effect (return value)
*/
double satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_drift(double transmitTime);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
template <class Archive>

View File

@ -33,7 +33,6 @@ class Beidou_Dnav_Iono
public:
Beidou_Dnav_Iono() = default; //!< Default constructor
bool valid{}; //!< Valid flag
// Ionospheric parameters
double d_alpha0{}; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
double d_alpha1{}; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
@ -44,6 +43,8 @@ public:
double d_beta2{}; //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
double d_beta3{}; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]
bool valid{}; //!< Valid flag
template <class Archive>
/*!

View File

@ -41,7 +41,7 @@ Beidou_Dnav_Navigation_Message::Beidou_Dnav_Navigation_Message()
}
void Beidou_Dnav_Navigation_Message::print_beidou_word_bytes(uint32_t BEIDOU_word)
void Beidou_Dnav_Navigation_Message::print_beidou_word_bytes(uint32_t BEIDOU_word) const
{
std::cout << " Word =";
std::cout << std::bitset<32>(BEIDOU_word);
@ -51,7 +51,7 @@ void Beidou_Dnav_Navigation_Message::print_beidou_word_bytes(uint32_t BEIDOU_wor
bool Beidou_Dnav_Navigation_Message::read_navigation_bool(
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits,
const std::vector<std::pair<int32_t, int32_t>>& parameter)
const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value;
@ -69,7 +69,7 @@ bool Beidou_Dnav_Navigation_Message::read_navigation_bool(
uint64_t Beidou_Dnav_Navigation_Message::read_navigation_unsigned(
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits,
const std::vector<std::pair<int32_t, int32_t>>& parameter)
const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
uint64_t value = 0ULL;
int32_t num_of_slices = parameter.size();
@ -90,7 +90,7 @@ uint64_t Beidou_Dnav_Navigation_Message::read_navigation_unsigned(
int64_t Beidou_Dnav_Navigation_Message::read_navigation_signed(
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits,
const std::vector<std::pair<int32_t, int32_t>>& parameter)
const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
int64_t value = 0;
int32_t num_of_slices = parameter.size();
@ -776,7 +776,7 @@ double Beidou_Dnav_Navigation_Message::utc_time(const double beidoutime_correcte
}
Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris()
Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris() const
{
Beidou_Dnav_Ephemeris eph;
@ -826,7 +826,6 @@ Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris()
eph.d_A_f1 = static_cast<double>(read_navigation_signed(subframe_bits, D2_A1)) * D1_A1_LSB;
eph.d_A_f2 = d_A_f2;
eph.d_TGD1 = d_TGD1;
eph.d_TGD2 = d_TGD2;
}

View File

@ -49,199 +49,10 @@ public:
*/
Beidou_Dnav_Navigation_Message();
// System flags for data processing
bool flag_eph_valid{};
bool flag_utc_model_valid{};
bool flag_iono_valid{};
bool flag_d1_sf1{};
bool flag_d1_sf2{};
bool flag_d1_sf3{};
bool flag_d1_sf4{};
bool flag_d1_sf5{};
bool flag_new_SOW_available{};
bool flag_crc_test{};
double d_previous_aode{};
bool flag_d1_sf5_p7{}; //!< D1 NAV Message, Subframe 5, Page 09 decoded indicator
bool flag_d1_sf5_p8{}; //!< D1 NAV Message, Subframe 5, Page 09 decoded indicator
bool flag_d1_sf5_p9{}; //!< D1 NAV Message, Subframe 5, Page 09 decoded indicator
bool flag_d1_sf5_p10{}; //!< D1 NAV Message, Subframe 5, Page 10 decoded indicator
bool flag_sf1_p1{}; //!< D2 NAV Message, Subframe 1, Page 1 decoded indicator
bool flag_sf1_p2{}; //!< D2 NAV Message, Subframe 1, Page 2 decoded indicator
bool flag_sf1_p3{}; //!< D2 NAV Message, Subframe 1, Page 3 decoded indicator
bool flag_sf1_p4{}; //!< D2 NAV Message, Subframe 1, Page 4 decoded indicator
bool flag_sf1_p5{}; //!< D2 NAV Message, Subframe 1, Page 5 decoded indicator
bool flag_sf1_p6{}; //!< D2 NAV Message, Subframe 1, Page 6 decoded indicator
bool flag_sf1_p7{}; //!< D2 NAV Message, Subframe 1, Page 7 decoded indicator
bool flag_sf1_p8{}; //!< D2 NAV Message, Subframe 1, Page 8 decoded indicator
bool flag_sf1_p9{}; //!< D2 NAV Message, Subframe 1, Page 9 decoded indicator
bool flag_sf1_p10{}; //!< D2 NAV Message, Subframe 1, Page 10 decoded indicator
// broadcast orbit 1
double d_SOW{}; //!< Time of BeiDou Week of the ephemeris set (taken from subframes SOW) [s]
double d_SOW_SF1{}; //!< Time of BeiDou Week from HOW word of Subframe 1 [s]
double d_SOW_SF2{}; //!< Time of BeiDou Week from HOW word of Subframe 2 [s]
double d_SOW_SF3{}; //!< Time of BeiDou Week from HOW word of Subframe 3 [s]
double d_SOW_SF4{}; //!< Time of BeiDou Week from HOW word of Subframe 4 [s]
double d_SOW_SF5{}; //!< Time of BeiDou Week from HOW word of Subframe 5 [s]
double d_AODE{};
double d_Crs{}; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
double d_Delta_n{}; //!< Mean Motion Difference From Computed Value [semi-circles/s]
double d_M_0{}; //!< Mean Anomaly at Reference Time [semi-circles]
// broadcast orbit 2
double d_Cuc{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_eccentricity{}; //!< Eccentricity [dimensionless]
double d_Cus{}; //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad]
double d_sqrt_A{}; //!< Square Root of the Semi-Major Axis [sqrt(m)]
// broadcast orbit 3
double d_Toe_sf2{}; //!< Ephemeris data reference time of week in subframe 2, D1 Message
double d_Toe_sf3{}; //!< Ephemeris data reference time of week in subframe 3, D1 Message
double d_Toe{}; //!< Ephemeris data reference time of week in subframe 1, D2 Message
double d_Toc{}; //!< clock data reference time [s]
double d_Cic{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad]
double d_OMEGA0{}; //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
double d_Cis{}; //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad]
// broadcast orbit 4
double d_i_0{}; //!< Inclination Angle at Reference Time [semi-circles]
double d_Crc{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m]
double d_OMEGA{}; //!< Argument of Perigee [semi-cicles]
double d_OMEGA_DOT{}; //!< Rate of Right Ascension [semi-circles/s]
// broadcast orbit 5
double d_IDOT{}; //!< Rate of Inclination Angle [semi-circles/s]
int32_t i_BEIDOU_week{}; //!< BeiDou week number, aka WN [week]
// broadcast orbit 6
int32_t i_SV_accuracy{}; //!< User Range Accuracy (URA) index of the SV
int32_t i_SV_health{};
double d_TGD1{}; //!< Estimated Group Delay Differential in B1 [s]
double d_TGD2{}; //!< Estimated Group Delay Differential in B2 [s]
double d_AODC{}; //!< Age of Data, Clock
// broadcast orbit 7
// int32_t i_AODO{}; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
bool b_fit_interval_flag{}; //!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
double d_spare1{};
double d_spare2{};
double d_A_f0{}; //!< Clock correction parameters. Coefficient 0 of code phase offset model [s]
double d_A_f1{}; //!< Clock correction parameters. Coefficient 1 of code phase offset model [s/s]
double d_A_f2{}; //!< Clock correction parameters. Coefficient 2 of code phase offset model [s/s^2]
// D2 NAV Message Decoding
uint64_t d_A_f1_msb_bits{}; //!< Clock correction parameters, D2 NAV MSB
uint64_t d_A_f1_lsb_bits{}; //!< Clock correction parameters, D2 NAV LSB
uint64_t d_Cuc_msb_bits{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_Cuc_lsb_bits{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_eccentricity_msb{}; //!< Eccentricity [dimensionless]
uint64_t d_eccentricity_lsb{}; //!< Eccentricity [dimensionless]
uint64_t d_Cic_msb_bits{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_Cic_lsb_bits{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_eccentricity_msb_bits{}; //!< Eccentricity [dimensionless]
uint64_t d_eccentricity_lsb_bits{};
uint64_t d_i_0_msb_bits{}; //!< Inclination Angle at Reference Time [semi-circles]
uint64_t d_i_0_lsb_bits{}; //!< Inclination Angle at Reference Time [semi-circles]
uint64_t d_OMEGA_msb_bits{}; //!< Argument of Perigee [semi-cicles]
uint64_t d_OMEGA_lsb_bits{}; //!< Argument of Perigee [semi-cicles]
uint64_t d_OMEGA_DOT_msb_bits{}; //!< Rate of Right Ascension [semi-circles/s]
uint64_t d_OMEGA_DOT_lsb_bits{}; //!< Rate of Right Ascension [semi-circles/s]
// Almanac
double d_Toa{}; //!< Almanac reference time [s]
int32_t i_WN_A{}; //!< Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced
std::map<int32_t, int32_t> almanacHealth; //!< Map that stores the health information stored in the almanac
std::map<int32_t, std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs
// Flags
/*! \brief If true, enhanced level of integrity assurance.
*
* If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.
* That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound
* value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less
* than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of
* integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73
* times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an
* accompanying alert, is less than 1E-8 per hour.
*/
bool b_integrity_status_flag{};
bool b_alert_flag{}; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
bool b_antispoofing_flag{}; //!< If true, the AntiSpoofing mode is ON in that SV
// clock terms
// double d_master_clock{}; // GPS transmission time
double d_satClkCorr{}; // GPS clock error
double d_dtr{}; // relativistic clock correction term
double d_satClkDrift{};
// satellite positions
double d_satpos_X{}; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double d_satpos_Y{}; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double d_satpos_Z{}; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
// satellite identification info
int32_t i_channel_ID{};
int32_t i_signal_type{}; //!< BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)
uint32_t i_satellite_PRN{};
// time synchro
double d_subframe_timestamp_ms{}; // [ms]
// Ionospheric parameters
double d_alpha0{}; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
double d_alpha1{}; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
double d_alpha2{}; //!< Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2]
double d_alpha3{}; //!< Coefficient 3 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^3]
double d_beta0{}; //!< Coefficient 0 of a cubic equation representing the period of the model [s]
double d_beta1{}; //!< Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle]
double d_beta2{}; //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
double d_beta3{}; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]
// UTC parameters
double d_A1UTC{}; //!< 1st order term of a model that relates GPS and UTC time [s/s]
double d_A0UTC{}; //!< Constant of a model that relates GPS and UTC time [s]
double d_DeltaT_LS{}; //!< delta time due to leap seconds [s]. Number of leap seconds since 6-Jan-1980 as transmitted by the GPS almanac.
int32_t i_WN_LSF{}; //!< Week number at the end of which the leap second becomes effective [weeks]
int32_t i_DN{}; //!< Day number (DN) at the end of which the leap second becomes effective [days]
double d_DeltaT_LSF{}; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
double d_A1GPS{};
double d_A0GPS{};
double d_A1GAL{};
double d_A0GAL{};
double d_A1GLO{};
double d_A0GLO{};
double d_SQRT_A_ALMANAC{};
double d_A1_ALMANAC{};
double d_A0_ALMANAC{};
double d_OMEGA0_ALMANAC{};
double d_E_ALMANAC{};
double d_DELTA_I{};
double d_TOA{};
double d_OMEGA_DOT_ALMANAC{};
double d_OMEGA_ALMANAC{};
double d_M0_ALMANAC{};
int32_t almanac_WN{};
double d_toa2{};
// Satellite velocity
double d_satvel_X{}; //!< Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y{}; //!< Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z{}; //!< Earth-fixed velocity coordinate z of the satellite [m]
// public functions
/*!
* \brief Obtain a BDS SV Ephemeris class filled with current SV data
*/
Beidou_Dnav_Ephemeris get_ephemeris();
Beidou_Dnav_Ephemeris get_ephemeris() const;
/*!
* \brief Obtain a BDS ionospheric correction parameters class filled with current SV data
@ -302,11 +113,44 @@ public:
*/
bool have_new_almanac();
/*!
* \brief Sets satellite PRN number
*/
inline void set_satellite_PRN(uint32_t prn)
{
i_satellite_PRN = prn;
}
inline void set_signal_type(int32_t signal_type)
{
i_signal_type = signal_type;
}
inline bool get_flag_CRC_test() const
{
return flag_crc_test;
}
inline bool get_flag_new_SOW_available() const
{
return flag_new_SOW_available;
}
inline void set_flag_new_SOW_available(bool new_SOW_available)
{
flag_new_SOW_available = new_SOW_available;
}
inline double get_SOW() const
{
return d_SOW;
}
private:
uint64_t read_navigation_unsigned(std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
int64_t read_navigation_signed(std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
bool read_navigation_bool(std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
void print_beidou_word_bytes(uint32_t BEIDOU_word);
uint64_t read_navigation_unsigned(std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
int64_t read_navigation_signed(std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
bool read_navigation_bool(std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
void print_beidou_word_bytes(uint32_t BEIDOU_word) const;
/*
* Accounts for the beginning or end of week crossover
@ -315,6 +159,171 @@ private:
* \param[out] - corrected time, in seconds
*/
double check_t(double time);
// broadcast orbit 1
double d_SOW{}; // Time of BeiDou Week of the ephemeris set (taken from subframes SOW) [s]
double d_SOW_SF1{}; // Time of BeiDou Week from HOW word of Subframe 1 [s]
double d_SOW_SF2{}; // Time of BeiDou Week from HOW word of Subframe 2 [s]
double d_SOW_SF3{}; // Time of BeiDou Week from HOW word of Subframe 3 [s]
double d_SOW_SF4{}; // Time of BeiDou Week from HOW word of Subframe 4 [s]
double d_SOW_SF5{}; // Time of BeiDou Week from HOW word of Subframe 5 [s]
double d_AODE{};
double d_Crs{}; // Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
double d_Delta_n{}; // Mean Motion Difference From Computed Value [semi-circles/s]
double d_M_0{}; // Mean Anomaly at Reference Time [semi-circles]
// broadcast orbit 2
double d_Cuc{}; // Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_eccentricity{}; // Eccentricity [dimensionless]
double d_Cus{}; // Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad]
double d_sqrt_A{}; // Square Root of the Semi-Major Axis [sqrt(m)]
// broadcast orbit 3
double d_Toe_sf2{}; // Ephemeris data reference time of week in subframe 2, D1 Message
double d_Toe_sf3{}; // Ephemeris data reference time of week in subframe 3, D1 Message
double d_Toe{}; // Ephemeris data reference time of week in subframe 1, D2 Message
double d_Toc{}; // clock data reference time [s]
double d_Cic{}; // Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad]
double d_OMEGA0{}; // Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
double d_Cis{}; // Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad]
// broadcast orbit 4
double d_i_0{}; // Inclination Angle at Reference Time [semi-circles]
double d_Crc{}; // Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m]
double d_OMEGA{}; // Argument of Perigee [semi-cicles]
double d_OMEGA_DOT{}; // Rate of Right Ascension [semi-circles/s]
// broadcast orbit 5
double d_IDOT{}; // Rate of Inclination Angle [semi-circles/s]
int32_t i_BEIDOU_week{}; // BeiDou week number, aka WN [week]
// broadcast orbit 6
int32_t i_SV_accuracy{}; // User Range Accuracy (URA) index of the SV
int32_t i_SV_health{};
double d_TGD1{}; // Estimated Group Delay Differential in B1 [s]
double d_TGD2{}; // Estimated Group Delay Differential in B2 [s]
double d_AODC{}; // Age of Data, Clock
// broadcast orbit 7
// int32_t i_AODO{}; // Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
// bool b_fit_interval_flag{}; // indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
// double d_spare1{};
// double d_spare2{};
double d_A_f0{}; // Clock correction parameters. Coefficient 0 of code phase offset model [s]
double d_A_f1{}; // Clock correction parameters. Coefficient 1 of code phase offset model [s/s]
double d_A_f2{}; // Clock correction parameters. Coefficient 2 of code phase offset model [s/s^2]
// D2 NAV Message Decoding
uint64_t d_A_f1_msb_bits{}; // Clock correction parameters, D2 NAV MSB
uint64_t d_A_f1_lsb_bits{}; // Clock correction parameters, D2 NAV LSB
uint64_t d_Cuc_msb_bits{}; // Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_Cuc_lsb_bits{}; // Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_eccentricity_msb{}; // Eccentricity [dimensionless]
uint64_t d_eccentricity_lsb{}; // Eccentricity [dimensionless]
uint64_t d_Cic_msb_bits{}; // Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_Cic_lsb_bits{}; // Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
uint64_t d_eccentricity_msb_bits{}; // Eccentricity [dimensionless]
uint64_t d_eccentricity_lsb_bits{};
uint64_t d_i_0_msb_bits{}; // Inclination Angle at Reference Time [semi-circles]
uint64_t d_i_0_lsb_bits{}; // Inclination Angle at Reference Time [semi-circles]
uint64_t d_OMEGA_msb_bits{}; // Argument of Perigee [semi-cicles]
uint64_t d_OMEGA_lsb_bits{}; // Argument of Perigee [semi-cicles]
uint64_t d_OMEGA_DOT_msb_bits{}; // Rate of Right Ascension [semi-circles/s]
uint64_t d_OMEGA_DOT_lsb_bits{}; // Rate of Right Ascension [semi-circles/s]
// Almanac
// double d_Toa{}; // Almanac reference time [s]
// int32_t i_WN_A{}; // Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced
std::map<int32_t, int32_t> almanacHealth; // Map that stores the health information stored in the almanac
std::map<int32_t, std::string> satelliteBlock; // Map that stores to which block the PRN belongs
// clock terms
double d_satClkCorr{}; // GPS clock error
double d_dtr{}; // relativistic clock correction term
// satellite positions
double d_satpos_X{}; // Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double d_satpos_Y{}; // Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double d_satpos_Z{}; // Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
// satellite identification info
int32_t i_signal_type{}; // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q)
uint32_t i_satellite_PRN{};
// Ionospheric parameters
double d_alpha0{}; // Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
double d_alpha1{}; // Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
double d_alpha2{}; // Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2]
double d_alpha3{}; // Coefficient 3 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^3]
double d_beta0{}; // Coefficient 0 of a cubic equation representing the period of the model [s]
double d_beta1{}; // Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle]
double d_beta2{}; // Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
double d_beta3{}; // Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]
// UTC parameters
double d_A1UTC{}; // 1st order term of a model that relates GPS and UTC time [s/s]
double d_A0UTC{}; // Constant of a model that relates GPS and UTC time [s]
double d_DeltaT_LS{}; // delta time due to leap seconds [s]. Number of leap seconds since 6-Jan-1980 as transmitted by the GPS almanac.
int32_t i_WN_LSF{}; // Week number at the end of which the leap second becomes effective [weeks]
int32_t i_DN{}; // Day number (DN) at the end of which the leap second becomes effective [days]
double d_DeltaT_LSF{}; // Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
double d_A1GPS{};
double d_A0GPS{};
double d_A1GAL{};
double d_A0GAL{};
double d_A1GLO{};
double d_A0GLO{};
double d_SQRT_A_ALMANAC{};
double d_A1_ALMANAC{};
double d_A0_ALMANAC{};
double d_OMEGA0_ALMANAC{};
double d_E_ALMANAC{};
double d_DELTA_I{};
double d_TOA{};
double d_OMEGA_DOT_ALMANAC{};
double d_OMEGA_ALMANAC{};
double d_M0_ALMANAC{};
int32_t almanac_WN{};
double d_toa2{};
// Satellite velocity
double d_satvel_X{}; // Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y{}; // Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z{}; // Earth-fixed velocity coordinate z of the satellite [m]
// System flags for data processing
bool flag_eph_valid{};
bool flag_utc_model_valid{};
bool flag_iono_valid{};
bool flag_d1_sf1{};
bool flag_d1_sf2{};
bool flag_d1_sf3{};
bool flag_d1_sf4{};
bool flag_d1_sf5{};
bool flag_new_SOW_available{};
bool flag_crc_test{};
double d_previous_aode{};
// bool flag_d1_sf5_p7{}; // D1 NAV Message, Subframe 5, Page 09 decoded indicator
// bool flag_d1_sf5_p8{}; // D1 NAV Message, Subframe 5, Page 09 decoded indicator
bool flag_d1_sf5_p9{}; // D1 NAV Message, Subframe 5, Page 09 decoded indicator
bool flag_d1_sf5_p10{}; // D1 NAV Message, Subframe 5, Page 10 decoded indicator
bool flag_sf1_p1{}; // D2 NAV Message, Subframe 1, Page 1 decoded indicator
bool flag_sf1_p2{}; // D2 NAV Message, Subframe 1, Page 2 decoded indicator
bool flag_sf1_p3{}; // D2 NAV Message, Subframe 1, Page 3 decoded indicator
bool flag_sf1_p4{}; // D2 NAV Message, Subframe 1, Page 4 decoded indicator
bool flag_sf1_p5{}; // D2 NAV Message, Subframe 1, Page 5 decoded indicator
bool flag_sf1_p6{}; // D2 NAV Message, Subframe 1, Page 6 decoded indicator
bool flag_sf1_p7{}; // D2 NAV Message, Subframe 1, Page 7 decoded indicator
bool flag_sf1_p8{}; // D2 NAV Message, Subframe 1, Page 8 decoded indicator
bool flag_sf1_p9{}; // D2 NAV Message, Subframe 1, Page 9 decoded indicator
bool flag_sf1_p10{}; // D2 NAV Message, Subframe 1, Page 10 decoded indicator
};
#endif

View File

@ -35,8 +35,6 @@ class Beidou_Dnav_Utc_Model
public:
Beidou_Dnav_Utc_Model() = default;
bool valid{};
// BeiDou UTC parameters
double d_A0_UTC{}; //!< BDT clock bias relative to UTC [s]
double d_A1_UTC{}; //!< BDT clock rate relative to UTC [s/s]
@ -57,6 +55,8 @@ public:
double d_A0_GLO{}; //!< BDT clock bias relative to GLO time [s]
double d_A1_GLO{}; //!< BDT clock rate relative to GLO time [s/s]
bool valid{};
template <class Archive>
/*
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.
@ -67,7 +67,6 @@ public:
if (version)
{
};
archive& make_nvp("valid", valid);
archive& make_nvp("d_A1", d_A1_UTC);
archive& make_nvp("d_A0", d_A0_UTC);
archive& make_nvp("d_DeltaT_LS", d_DeltaT_LS);
@ -80,6 +79,7 @@ public:
archive& make_nvp("d_A0_GPS", d_A1_GAL);
archive& make_nvp("d_A0_GPS", d_A0_GLO);
archive& make_nvp("d_A0_GPS", d_A1_GLO);
archive& make_nvp("valid", valid);
}
};

View File

@ -30,6 +30,11 @@
class Galileo_Almanac
{
public:
/*!
* Default constructor
*/
Galileo_Almanac() = default;
uint32_t i_satellite_PRN{}; //!< SV PRN NUMBER
int32_t i_Toa{};
int32_t i_WNa{};
@ -47,11 +52,6 @@ public:
int32_t E1B_HS{};
int32_t E5a_HS{};
/*!
* Default constructor
*/
Galileo_Almanac() = default;
template <class Archive>
void serialize(Archive& ar, const unsigned int version)

View File

@ -33,6 +33,8 @@ class Galileo_Almanac_Helper
public:
Galileo_Almanac_Helper() = default; //!< Default constructor
Galileo_Almanac get_almanac(int i);
// Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number
int32_t IOD_a_7{};
int32_t WN_a_7{};
@ -87,8 +89,6 @@ public:
int32_t E5b_HS_10{};
int32_t E1B_HS_10{};
int32_t E5a_HS_10{};
Galileo_Almanac get_almanac(int i);
};
#endif

View File

@ -35,9 +35,14 @@ class Galileo_Ephemeris
{
public:
Galileo_Ephemeris() = default;
void satellitePosition(double transmitTime); //!< Computes the ECEF SV coordinates and ECEF velocity
double Galileo_System_Time(double WN, double TOW); //!< Galileo System Time (GST), ICD paragraph 5.1.2
double sv_clock_drift(double transmitTime); //!< Satellite Time Correction Algorithm, ICD 5.1.4
double sv_clock_relativistic_term(double transmitTime); //!< Satellite Time Correction Algorithm, ICD 5.1.4
/* Galileo ephemeris are 16 parameters and here are reported following the ICD order, paragraph 5.1.1.
The number in the name after underscore (_1, _2, _3 and so on) refers to the page were we can find that parameter */
bool flag_all_ephemeris{};
int32_t IOD_ephemeris{};
int32_t IOD_nav_1{};
int32_t SV_ID_PRN_4{};
@ -73,13 +78,12 @@ public:
// SV status
int32_t SISA_3{};
int32_t E5a_HS{}; //!< E5a Signal Health Status
int32_t E5b_HS_5{}; //!< E5b Signal Health Status
int32_t E1B_HS_5{}; //!< E1B Signal Health Status
bool E5a_DVS{}; //!< E5a Data Validity Status
bool E5b_DVS_5{}; //!< E5b Data Validity Status
bool E1B_DVS_5{}; //!< E1B Data Validity Status
int32_t E5a_HS{}; //!< E5a Signal Health Status
int32_t E5b_HS_5{}; //!< E5b Signal Health Status
int32_t E1B_HS_5{}; //!< E1B Signal Health Status
bool E5a_DVS{}; //!< E5a Data Validity Status
bool E5b_DVS_5{}; //!< E5b Data Validity Status
bool E1B_DVS_5{}; //!< E1B Data Validity Status
double BGD_E1E5a_5{}; //!< E1-E5a Broadcast Group Delay [s]
double BGD_E1E5b_5{}; //!< E1-E5b Broadcast Group Delay [s]
@ -95,10 +99,7 @@ public:
uint32_t i_satellite_PRN{}; //!< SV PRN NUMBER
void satellitePosition(double transmitTime); //!< Computes the ECEF SV coordinates and ECEF velocity
double Galileo_System_Time(double WN, double TOW); //!< Galileo System Time (GST), ICD paragraph 5.1.2
double sv_clock_drift(double transmitTime); //!< Satellite Time Correction Algorithm, ICD 5.1.4
double sv_clock_relativistic_term(double transmitTime); //!< Satellite Time Correction Algorithm, ICD 5.1.4
bool flag_all_ephemeris{};
template <class Archive>
@ -140,7 +141,6 @@ public:
archive& BOOST_SERIALIZATION_NVP(Galileo_satClkDrift);
archive& BOOST_SERIALIZATION_NVP(Galileo_dtr);
archive& BOOST_SERIALIZATION_NVP(flag_all_ephemeris);
archive& BOOST_SERIALIZATION_NVP(IOD_ephemeris);
archive& BOOST_SERIALIZATION_NVP(IOD_nav_1);
@ -154,6 +154,8 @@ public:
archive& BOOST_SERIALIZATION_NVP(BGD_E1E5a_5);
archive& BOOST_SERIALIZATION_NVP(BGD_E1E5b_5);
archive& BOOST_SERIALIZATION_NVP(flag_all_ephemeris);
}
};

View File

@ -53,7 +53,7 @@ void Galileo_Fnav_Message::split_page(const std::string& page_string)
}
bool Galileo_Fnav_Message::_CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, uint32_t checksum)
bool Galileo_Fnav_Message::_CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, uint32_t checksum) const
{
CRC_Galileo_FNAV_type CRC_Galileo;
@ -283,7 +283,7 @@ void Galileo_Fnav_Message::decode_page(const std::string& data)
}
uint64_t Galileo_Fnav_Message::read_navigation_unsigned(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
uint64_t Galileo_Fnav_Message::read_navigation_unsigned(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
uint64_t value = 0ULL;
int num_of_slices = parameter.size();
@ -302,7 +302,7 @@ uint64_t Galileo_Fnav_Message::read_navigation_unsigned(std::bitset<GALILEO_FNAV
}
int64_t Galileo_Fnav_Message::read_navigation_signed(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
int64_t Galileo_Fnav_Message::read_navigation_signed(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
int64_t value = 0LL;
int num_of_slices = parameter.size();
@ -399,7 +399,7 @@ bool Galileo_Fnav_Message::have_new_almanac() // Check if we have a new almanac
}
Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris()
Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris() const
{
Galileo_Ephemeris ephemeris;
ephemeris.flag_all_ephemeris = flag_all_ephemeris;
@ -440,7 +440,7 @@ Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris()
}
Galileo_Iono Galileo_Fnav_Message::get_iono()
Galileo_Iono Galileo_Fnav_Message::get_iono() const
{
Galileo_Iono iono;
// Ionospheric correction
@ -462,7 +462,7 @@ Galileo_Iono Galileo_Fnav_Message::get_iono()
}
Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model()
Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model() const
{
Galileo_Utc_Model utc_model;
// Word type 6: GST-UTC conversion parameters
@ -482,7 +482,7 @@ Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model()
}
Galileo_Almanac_Helper Galileo_Fnav_Message::get_almanac()
Galileo_Almanac_Helper Galileo_Fnav_Message::get_almanac() const
{
Galileo_Almanac_Helper almanac;
// FNAV equivalent of INAV Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number

View File

@ -53,28 +53,100 @@ public:
bool have_new_iono_and_GST();
bool have_new_utc_model();
bool have_new_almanac();
Galileo_Ephemeris get_ephemeris();
Galileo_Iono get_iono();
Galileo_Utc_Model get_utc_model();
Galileo_Almanac_Helper get_almanac();
Galileo_Ephemeris get_ephemeris() const;
Galileo_Iono get_iono() const;
Galileo_Utc_Model get_utc_model() const;
Galileo_Almanac_Helper get_almanac() const;
bool flag_CRC_test{};
bool flag_all_ephemeris{}; //!< Flag indicating that all words containing ephemeris have been received
bool flag_ephemeris_1{}; //!< Flag indicating that ephemeris 1/3 (word 2) have been received
bool flag_ephemeris_2{}; //!< Flag indicating that ephemeris 2/3 (word 3) have been received
bool flag_ephemeris_3{}; //!< Flag indicating that ephemeris 3/3 (word 4) have been received
inline int32_t get_TOW1() const
{
return FNAV_TOW_1;
}
bool flag_iono_and_GST{}; //!< Flag indicating that ionospheric and GST parameters (word 1) have been received
bool flag_TOW_1{};
bool flag_TOW_2{};
bool flag_TOW_3{};
bool flag_TOW_4{};
bool flag_TOW_set{}; //!< it is true when page 1,2,3 or 4 arrives
bool flag_utc_model{}; //!< Flag indicating that utc model parameters (word 4) have been received
inline int32_t get_TOW2() const
{
return FNAV_TOW_2;
}
bool flag_all_almanac{}; //!< Flag indicating that all almanac have been received
bool flag_almanac_1{}; //!< Flag indicating that almanac 1/2 (word 5) have been received
bool flag_almanac_2{}; //!< Flag indicating that almanac 2/2 (word 6) have been received
inline int32_t get_TOW3() const
{
return FNAV_TOW_3;
}
inline int32_t get_TOW4() const
{
return FNAV_TOW_4;
}
inline bool get_flag_CRC_test() const
{
return flag_CRC_test;
}
inline bool get_flag_TOW_set() const
{
return flag_TOW_set;
}
inline void set_flag_TOW_set(bool flag_tow)
{
flag_TOW_set = flag_tow;
}
inline bool is_TOW_set() const
{
return flag_TOW_set;
}
inline bool is_TOW1_set() const
{
return flag_TOW_1;
}
inline void set_TOW1_flag(bool flag_tow1)
{
flag_TOW_1 = flag_tow1;
}
inline bool is_TOW2_set() const
{
return flag_TOW_2;
}
inline void set_TOW2_flag(bool flag_tow2)
{
flag_TOW_2 = flag_tow2;
}
inline bool is_TOW3_set() const
{
return flag_TOW_3;
}
inline void set_TOW3_flag(bool flag_tow3)
{
flag_TOW_3 = flag_tow3;
}
inline bool is_TOW4_set() const
{
return flag_TOW_4;
}
inline void set_TOW4_flag(bool flag_tow4)
{
flag_TOW_4 = flag_tow4;
}
private:
bool _CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, uint32_t checksum) const;
void decode_page(const std::string& data);
uint64_t read_navigation_unsigned(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
int64_t read_navigation_signed(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
std::string omega0_1{};
// std::string omega0_2{};
// bool omega_flag{};
int32_t IOD_ephemeris{};
@ -186,15 +258,23 @@ public:
double FNAV_af1_3_6{};
int32_t FNAV_E5ahs_3_6{};
private:
bool _CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, uint32_t checksum);
void decode_page(const std::string& data);
uint64_t read_navigation_unsigned(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
int64_t read_navigation_signed(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
bool flag_CRC_test{};
bool flag_all_ephemeris{}; // Flag indicating that all words containing ephemeris have been received
bool flag_ephemeris_1{}; // Flag indicating that ephemeris 1/3 (word 2) have been received
bool flag_ephemeris_2{}; // Flag indicating that ephemeris 2/3 (word 3) have been received
bool flag_ephemeris_3{}; // Flag indicating that ephemeris 3/3 (word 4) have been received
std::string omega0_1{};
// std::string omega0_2{};
// bool omega_flag{};
bool flag_iono_and_GST{}; // Flag indicating that ionospheric and GST parameters (word 1) have been received
bool flag_TOW_1{};
bool flag_TOW_2{};
bool flag_TOW_3{};
bool flag_TOW_4{};
bool flag_TOW_set{}; // it is true when page 1,2,3 or 4 arrives
bool flag_utc_model{}; // Flag indicating that utc model parameters (word 4) have been received
bool flag_all_almanac{}; // Flag indicating that all almanac have been received
bool flag_almanac_1{}; // Flag indicating that almanac 1/2 (word 5) have been received
bool flag_almanac_2{}; // Flag indicating that almanac 2/2 (word 6) have been received
};
#endif // GNSS_SDR_GALILEO_FNAV_MESSAGE_H

View File

@ -42,6 +42,10 @@ public:
double ai1_5{}; //!< Effective Ionisation Level 2st order parameter [sfu/degree]
double ai2_5{}; //!< Effective Ionisation Level 3st order parameter [sfu/degree]
// from page 5 (UTC) to have a timestamp
int32_t TOW_5{}; //!< UTC data reference Time of Week [s]
int32_t WN_5{}; //!< UTC data reference Week number [week]
// Ionospheric disturbance flag
bool Region1_flag_5{}; //!< Ionospheric Disturbance Flag for region 1
bool Region2_flag_5{}; //!< Ionospheric Disturbance Flag for region 2
@ -49,10 +53,6 @@ public:
bool Region4_flag_5{}; //!< Ionospheric Disturbance Flag for region 4
bool Region5_flag_5{}; //!< Ionospheric Disturbance Flag for region 5
// from page 5 (UTC) to have a timestamp
int32_t TOW_5{}; //!< UTC data reference Time of Week [s]
int32_t WN_5{}; //!< UTC data reference Week number [week]
template <class Archive>
/*!
@ -68,13 +68,13 @@ public:
archive& make_nvp("ai0_5", ai0_5);
archive& make_nvp("ai1_5", ai1_5);
archive& make_nvp("ai2_5", ai2_5);
archive& make_nvp("TOW_5", TOW_5);
archive& make_nvp("WN_5", WN_5);
archive& make_nvp("Region1_flag_5", Region1_flag_5);
archive& make_nvp("Region2_flag_5", Region2_flag_5);
archive& make_nvp("Region3_flag_5", Region3_flag_5);
archive& make_nvp("Region4_flag_5", Region4_flag_5);
archive& make_nvp("Region5_flag_5", Region5_flag_5);
archive& make_nvp("TOW_5", TOW_5);
archive& make_nvp("WN_5", WN_5);
}
};

View File

@ -31,7 +31,7 @@
using CRC_Galileo_INAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false, false>;
bool Galileo_Navigation_Message::CRC_test(std::bitset<GALILEO_DATA_FRAME_BITS> bits, uint32_t checksum)
bool Galileo_Navigation_Message::CRC_test(std::bitset<GALILEO_DATA_FRAME_BITS> bits, uint32_t checksum) const
{
CRC_Galileo_INAV_type CRC_Galileo;
@ -59,7 +59,7 @@ bool Galileo_Navigation_Message::CRC_test(std::bitset<GALILEO_DATA_FRAME_BITS> b
}
uint64_t Galileo_Navigation_Message::read_navigation_unsigned(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter)
uint64_t Galileo_Navigation_Message::read_navigation_unsigned(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const
{
uint64_t value = 0ULL;
int32_t num_of_slices = parameter.size();
@ -78,7 +78,7 @@ uint64_t Galileo_Navigation_Message::read_navigation_unsigned(std::bitset<GALILE
}
uint64_t Galileo_Navigation_Message::read_page_type_unsigned(std::bitset<GALILEO_PAGE_TYPE_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter)
uint64_t Galileo_Navigation_Message::read_page_type_unsigned(std::bitset<GALILEO_PAGE_TYPE_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const
{
uint64_t value = 0ULL;
int32_t num_of_slices = parameter.size();
@ -97,7 +97,7 @@ uint64_t Galileo_Navigation_Message::read_page_type_unsigned(std::bitset<GALILEO
}
int64_t Galileo_Navigation_Message::read_navigation_signed(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter)
int64_t Galileo_Navigation_Message::read_navigation_signed(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const
{
int64_t value = 0LL;
int32_t num_of_slices = parameter.size();
@ -128,7 +128,7 @@ int64_t Galileo_Navigation_Message::read_navigation_signed(std::bitset<GALILEO_D
}
bool Galileo_Navigation_Message::read_navigation_bool(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter)
bool Galileo_Navigation_Message::read_navigation_bool(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const
{
bool value;
if (static_cast<int>(static_cast<int>(bits[GALILEO_DATA_JK_BITS - parameter[0].first])) == 1)
@ -268,7 +268,7 @@ bool Galileo_Navigation_Message::have_new_almanac() // Check if we have a new a
}
Galileo_Ephemeris Galileo_Navigation_Message::get_ephemeris()
Galileo_Ephemeris Galileo_Navigation_Message::get_ephemeris() const
{
Galileo_Ephemeris ephemeris;
ephemeris.flag_all_ephemeris = flag_all_ephemeris;
@ -317,7 +317,7 @@ Galileo_Ephemeris Galileo_Navigation_Message::get_ephemeris()
}
Galileo_Iono Galileo_Navigation_Message::get_iono()
Galileo_Iono Galileo_Navigation_Message::get_iono() const
{
Galileo_Iono iono;
// Ionospheric correction
@ -325,6 +325,11 @@ Galileo_Iono Galileo_Navigation_Message::get_iono()
iono.ai1_5 = ai1_5; // Effective Ionisation Level 2st order parameter [sfu/degree]
iono.ai2_5 = ai2_5; // Effective Ionisation Level 3st order parameter [sfu/degree]
// GST
// This is the ONLY page containing the Week Number (WN)
iono.TOW_5 = TOW_5;
iono.WN_5 = WN_5;
// Ionospheric disturbance flag
iono.Region1_flag_5 = Region1_flag_5; // Ionospheric Disturbance Flag for region 1
iono.Region2_flag_5 = Region2_flag_5; // Ionospheric Disturbance Flag for region 2
@ -332,15 +337,11 @@ Galileo_Iono Galileo_Navigation_Message::get_iono()
iono.Region4_flag_5 = Region4_flag_5; // Ionospheric Disturbance Flag for region 4
iono.Region5_flag_5 = Region5_flag_5; // Ionospheric Disturbance Flag for region 5
// GST
// This is the ONLY page containing the Week Number (WN)
iono.TOW_5 = TOW_5;
iono.WN_5 = WN_5;
return iono;
}
Galileo_Utc_Model Galileo_Navigation_Message::get_utc_model()
Galileo_Utc_Model Galileo_Navigation_Message::get_utc_model() const
{
Galileo_Utc_Model utc_model;
// Word type 6: GST-UTC conversion parameters
@ -362,7 +363,7 @@ Galileo_Utc_Model Galileo_Navigation_Message::get_utc_model()
}
Galileo_Almanac_Helper Galileo_Navigation_Message::get_almanac()
Galileo_Almanac_Helper Galileo_Navigation_Message::get_almanac() const
{
Galileo_Almanac_Helper almanac;
// Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number

View File

@ -44,91 +44,206 @@ class Galileo_Navigation_Message
public:
Galileo_Navigation_Message() = default;
int32_t Page_type_time_stamp{};
/*
* \brief Takes in input a page (Odd or Even) of 120 bit, split it according ICD 4.3.2.3 and join Data_k with Data_j
*/
void split_page(std::string page_string, int32_t flag_even_word);
/*
* \brief Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5
*
* Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5
*/
int32_t page_jk_decoder(const char* data_jk);
/*
* \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed
*/
bool have_new_ephemeris();
/*
* \brief Returns true if new Iono model has arrived. The flag is set to false when the function is executed
*/
bool have_new_iono_and_GST();
/*
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
*/
bool have_new_utc_model();
/*
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
*/
bool have_new_almanac();
/*
* \brief Returns a Galileo_Ephemeris object filled with the latest navigation data received
*/
Galileo_Ephemeris get_ephemeris() const;
/*
* \brief Returns a Galileo_Iono object filled with the latest navigation data received
*/
Galileo_Iono get_iono() const;
/*
* \brief Returns a Galileo_Utc_Model object filled with the latest navigation data received
*/
Galileo_Utc_Model get_utc_model() const;
/*
* \brief Returns a Galileo_Almanac_Helper object filled with the latest navigation data received
*/
Galileo_Almanac_Helper get_almanac() const;
inline bool get_flag_CRC_test() const
{
return flag_CRC_test;
}
inline bool get_flag_TOW_set() const
{
return flag_TOW_set;
}
inline void set_flag_TOW_set(bool flag_tow)
{
flag_TOW_set = flag_tow;
}
inline int32_t get_Galileo_week() const
{
return WN_0;
}
inline bool is_TOW_set() const
{
return flag_TOW_set;
}
inline int32_t get_TOW5() const
{
return TOW_5;
}
inline int32_t get_TOW6() const
{
return TOW_6;
}
inline bool is_TOW5_set() const
{
return flag_TOW_5;
}
inline void set_TOW5_flag(bool flag_tow5)
{
flag_TOW_5 = flag_tow5;
}
inline bool is_TOW6_set() const
{
return flag_TOW_6;
}
inline void set_TOW6_flag(bool flag_tow6)
{
flag_TOW_6 = flag_tow6;
}
inline bool get_flag_GGTO() const
{
return (flag_GGTO_1 == true and flag_GGTO_2 == true and flag_GGTO_3 == true and flag_GGTO_4 == true);
}
inline double get_A0G() const
{
return A_0G_10;
}
inline double get_A1G() const
{
return A_1G_10;
}
inline double get_t0G() const
{
return t_0G_10;
}
inline double get_WN0G() const
{
return WN_0G_10;
}
private:
bool CRC_test(std::bitset<GALILEO_DATA_FRAME_BITS> bits, uint32_t checksum) const;
bool read_navigation_bool(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const;
uint64_t read_navigation_unsigned(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const;
uint64_t read_page_type_unsigned(std::bitset<GALILEO_PAGE_TYPE_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const;
int64_t read_navigation_signed(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter) const;
std::string page_Even{};
bool flag_CRC_test{};
bool flag_all_ephemeris{}; //!< Flag indicating that all words containing ephemeris have been received
bool flag_ephemeris_1{}; //!< Flag indicating that ephemeris 1/4 (word 1) have been received
bool flag_ephemeris_2{}; //!< Flag indicating that ephemeris 2/4 (word 2) have been received
bool flag_ephemeris_3{}; //!< Flag indicating that ephemeris 3/4 (word 3) have been received
bool flag_ephemeris_4{}; //!< Flag indicating that ephemeris 4/4 (word 4) have been received
bool flag_iono_and_GST{}; //!< Flag indicating that ionospheric and GST parameters (word 5) have been received
bool flag_TOW_5{};
bool flag_TOW_6{};
bool flag_TOW_set{}; //!< it is true when page 5 or page 6 arrives
bool flag_utc_model{}; //!< Flag indicating that utc model parameters (word 6) have been received
bool flag_all_almanac{}; //!< Flag indicating that all almanac have been received
bool flag_almanac_1{}; //!< Flag indicating that almanac 1/4 (word 7) have been received
bool flag_almanac_2{}; //!< Flag indicating that almanac 2/4 (word 8) have been received
bool flag_almanac_3{}; //!< Flag indicating that almanac 3/4 (word 9) have been received
bool flag_almanac_4{}; //!< Flag indicating that almanac 4/4 (word 10) have been received
int32_t Page_type_time_stamp{};
int32_t IOD_ephemeris{};
bool flag_GGTO{};
bool flag_GGTO_1{};
bool flag_GGTO_2{};
bool flag_GGTO_3{};
bool flag_GGTO_4{};
// Word type 1: Ephemeris (1/4)
int32_t IOD_nav_1{}; //!< IOD_nav page 1
int32_t t0e_1{}; //!< Ephemeris reference time [s]
double M0_1{}; //!< Mean anomaly at reference time [semi-circles]
double e_1{}; //!< Eccentricity
double A_1{}; //!< Square root of the semi-major axis [meters^1/2]
int32_t IOD_nav_1{}; // IOD_nav page 1
int32_t t0e_1{}; // Ephemeris reference time [s]
double M0_1{}; // Mean anomaly at reference time [semi-circles]
double e_1{}; // Eccentricity
double A_1{}; // Square root of the semi-major axis [meters^1/2]
// Word type 2: Ephemeris (2/4)
int32_t IOD_nav_2{}; //!< IOD_nav page 2
double OMEGA_0_2{}; //!< Longitude of ascending node of orbital plane at weekly epoch [semi-circles]
double i_0_2{}; //!< Inclination angle at reference time [semi-circles]
double omega_2{}; //!< Argument of perigee [semi-circles]
double iDot_2{}; //!< Rate of inclination angle [semi-circles/sec]
int32_t IOD_nav_2{}; // IOD_nav page 2
double OMEGA_0_2{}; // Longitude of ascending node of orbital plane at weekly epoch [semi-circles]
double i_0_2{}; // Inclination angle at reference time [semi-circles]
double omega_2{}; // Argument of perigee [semi-circles]
double iDot_2{}; // Rate of inclination angle [semi-circles/sec]
// Word type 3: Ephemeris (3/4) and SISA
int32_t IOD_nav_3{}; //
double OMEGA_dot_3{}; //!< Rate of right ascension [semi-circles/sec]
double delta_n_3{}; //!< Mean motion difference from computed value [semi-circles/sec]
double C_uc_3{}; //!< Amplitude of the cosine harmonic correction term to the argument of latitude [radians]
double C_us_3{}; //!< Amplitude of the sine harmonic correction term to the argument of latitude [radians]
double C_rc_3{}; //!< Amplitude of the cosine harmonic correction term to the orbit radius [meters]
double C_rs_3{}; //!< Amplitude of the sine harmonic correction term to the orbit radius [meters]
double OMEGA_dot_3{}; // Rate of right ascension [semi-circles/sec]
double delta_n_3{}; // Mean motion difference from computed value [semi-circles/sec]
double C_uc_3{}; // Amplitude of the cosine harmonic correction term to the argument of latitude [radians]
double C_us_3{}; // Amplitude of the sine harmonic correction term to the argument of latitude [radians]
double C_rc_3{}; // Amplitude of the cosine harmonic correction term to the orbit radius [meters]
double C_rs_3{}; // Amplitude of the sine harmonic correction term to the orbit radius [meters]
int32_t SISA_3{};
// Word type 4: Ephemeris (4/4) and Clock correction parameters*/
int32_t IOD_nav_4{}; //
int32_t SV_ID_PRN_4{}; //
double C_ic_4{}; //!< Amplitude of the cosine harmonic correction term to the angle of inclination [radians]
double C_is_4{}; //!< Amplitude of the sine harmonic correction term to the angle of inclination [radians]
double C_ic_4{}; // Amplitude of the cosine harmonic correction term to the angle of inclination [radians]
double C_is_4{}; // Amplitude of the sine harmonic correction term to the angle of inclination [radians]
// Clock correction parameters
int32_t t0c_4{}; //!< Clock correction data reference Time of Week [sec]
double af0_4{}; //!< SV clock bias correction coefficient [s]
double af1_4{}; //!< SV clock drift correction coefficient [s/s]
double af2_4{}; //!< clock drift rate correction coefficient [s/s^2]
int32_t t0c_4{}; // Clock correction data reference Time of Week [sec]
double af0_4{}; // SV clock bias correction coefficient [s]
double af1_4{}; // SV clock drift correction coefficient [s/s]
double af2_4{}; // clock drift rate correction coefficient [s/s^2]
double spare_4{};
// Word type 5: Ionospheric correction, BGD, signal health and data validity status and GST*/
// Ionospheric correction
double ai0_5{}; //!< Effective Ionisation Level 1st order parameter [sfu]
double ai1_5{}; //!< Effective Ionisation Level 2st order parameter [sfu/degree]
double ai2_5{}; //!< Effective Ionisation Level 3st order parameter [sfu/degree]
double ai0_5{}; // Effective Ionisation Level 1st order parameter [sfu]
double ai1_5{}; // Effective Ionisation Level 2st order parameter [sfu/degree]
double ai2_5{}; // Effective Ionisation Level 3st order parameter [sfu/degree]
// Ionospheric disturbance flag
bool Region1_flag_5{}; //!< Ionospheric Disturbance Flag for region 1
bool Region2_flag_5{}; //!< Ionospheric Disturbance Flag for region 2
bool Region3_flag_5{}; //!< Ionospheric Disturbance Flag for region 3
bool Region4_flag_5{}; //!< Ionospheric Disturbance Flag for region 4
bool Region5_flag_5{}; //!< Ionospheric Disturbance Flag for region 5
double BGD_E1E5a_5{}; //!< E1-E5a Broadcast Group Delay [s]
double BGD_E1E5b_5{}; //!< E1-E5b Broadcast Group Delay [s]
bool Region1_flag_5{}; // Ionospheric Disturbance Flag for region 1
bool Region2_flag_5{}; // Ionospheric Disturbance Flag for region 2
bool Region3_flag_5{}; // Ionospheric Disturbance Flag for region 3
bool Region4_flag_5{}; // Ionospheric Disturbance Flag for region 4
bool Region5_flag_5{}; // Ionospheric Disturbance Flag for region 5
double BGD_E1E5a_5{}; // E1-E5a Broadcast Group Delay [s]
double BGD_E1E5b_5{}; // E1-E5b Broadcast Group Delay [s]
int32_t E5b_HS_5{}; //!< E5b Signal Health Status
int32_t E1B_HS_5{}; //!< E1B Signal Health Status
bool E5b_DVS_5{}; //!< E5b Data Validity Status
bool E1B_DVS_5{}; //!< E1B Data Validity Status
int32_t E5b_HS_5{}; // E5b Signal Health Status
int32_t E1B_HS_5{}; // E1B Signal Health Status
bool E5b_DVS_5{}; // E5b Data Validity Status
bool E1B_DVS_5{}; // E1B Data Validity Status
// GST
int32_t WN_5{};
@ -199,10 +314,10 @@ public:
int32_t E1B_HS_10{};
// GST-GPS conversion
double A_0G_10{}; //!< Constant term of the offset Delta t systems
double A_1G_10{}; //!< Rate of change of the offset Delta t systems
int32_t t_0G_10{}; //!< Reference time for Galileo/GPS Time Offset (GGTO) data
int32_t WN_0G_10{}; //!< Week Number of Galileo/GPS Time Offset (GGTO) reference
double A_0G_10{}; // Constant term of the offset Delta t systems
double A_1G_10{}; // Rate of change of the offset Delta t systems
int32_t t_0G_10{}; // Reference time for Galileo/GPS Time Offset (GGTO) data
int32_t WN_0G_10{}; // Week Number of Galileo/GPS Time Offset (GGTO) reference
// Word type 0: I/NAV Spare Word
int32_t Time_0{};
@ -210,76 +325,30 @@ public:
int32_t TOW_0{};
double Galileo_satClkDrift{};
double Galileo_dtr{}; //!< Relativistic clock correction term
// satellite positions
double galileo_satpos_X{}; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double galileo_satpos_Y{}; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double galileo_satpos_Z{}; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
bool flag_CRC_test{};
bool flag_all_ephemeris{}; // Flag indicating that all words containing ephemeris have been received
bool flag_ephemeris_1{}; // Flag indicating that ephemeris 1/4 (word 1) have been received
bool flag_ephemeris_2{}; // Flag indicating that ephemeris 2/4 (word 2) have been received
bool flag_ephemeris_3{}; // Flag indicating that ephemeris 3/4 (word 3) have been received
bool flag_ephemeris_4{}; // Flag indicating that ephemeris 4/4 (word 4) have been received
// Satellite velocity
double galileo_satvel_X{}; //!< Earth-fixed velocity coordinate x of the satellite [m]
double galileo_satvel_Y{}; //!< Earth-fixed velocity coordinate y of the satellite [m]
double galileo_satvel_Z{}; //!< Earth-fixed velocity coordinate z of the satellite [m]
bool flag_iono_and_GST{}; // Flag indicating that ionospheric and GST parameters (word 5) have been received
bool flag_TOW_5{};
bool flag_TOW_6{};
bool flag_TOW_set{}; // it is true when page 5 or page 6 arrives
bool flag_utc_model{}; // Flag indicating that utc model parameters (word 6) have been received
/*
* \brief Takes in input a page (Odd or Even) of 120 bit, split it according ICD 4.3.2.3 and join Data_k with Data_j
*/
void split_page(std::string page_string, int32_t flag_even_word);
bool flag_all_almanac{}; // Flag indicating that all almanac have been received
bool flag_almanac_1{}; // Flag indicating that almanac 1/4 (word 7) have been received
bool flag_almanac_2{}; // Flag indicating that almanac 2/4 (word 8) have been received
bool flag_almanac_3{}; // Flag indicating that almanac 3/4 (word 9) have been received
bool flag_almanac_4{}; // Flag indicating that almanac 4/4 (word 10) have been received
/*
* \brief Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5
*
* Takes in input Data_jk (128 bit) and split it in ephemeris parameters according ICD 4.3.5
*/
int32_t page_jk_decoder(const char* data_jk);
/*
* \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed
*/
bool have_new_ephemeris();
/*
* \brief Returns true if new Iono model has arrived. The flag is set to false when the function is executed
*/
bool have_new_iono_and_GST();
/*
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
*/
bool have_new_utc_model();
/*
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
*/
bool have_new_almanac();
/*
* \brief Returns a Galileo_Ephemeris object filled with the latest navigation data received
*/
Galileo_Ephemeris get_ephemeris();
/*
* \brief Returns a Galileo_Iono object filled with the latest navigation data received
*/
Galileo_Iono get_iono();
/*
* \brief Returns a Galileo_Utc_Model object filled with the latest navigation data received
*/
Galileo_Utc_Model get_utc_model();
/*
* \brief Returns a Galileo_Almanac_Helper object filled with the latest navigation data received
*/
Galileo_Almanac_Helper get_almanac();
private:
bool CRC_test(std::bitset<GALILEO_DATA_FRAME_BITS> bits, uint32_t checksum);
bool read_navigation_bool(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter);
uint64_t read_navigation_unsigned(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter);
uint64_t read_page_type_unsigned(std::bitset<GALILEO_PAGE_TYPE_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter);
int64_t read_navigation_signed(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& parameter);
bool flag_GGTO_1{};
bool flag_GGTO_2{};
bool flag_GGTO_3{};
bool flag_GGTO_4{};
};
#endif // GNSS_SDR_GALILEO_NAVIGATION_MESSAGE_H

View File

@ -38,6 +38,9 @@ public:
*/
Galileo_Utc_Model() = default;
// double TOW_6;
double GST_to_UTC_time(double t_e, int32_t WN); //!< GST-UTC Conversion Algorithm and Parameters
// Word type 6: GST-UTC conversion parameters
double A0_6{};
double A1_6{};
@ -47,7 +50,6 @@ public:
int32_t WN_LSF_6{};
int32_t DN_6{};
int32_t Delta_tLSF_6{};
bool flag_utc_model{};
// GPS to Galileo GST conversion parameters
double A_0G_10{};
@ -55,8 +57,7 @@ public:
int32_t t_0G_10{};
int32_t WN_0G_10{};
// double TOW_6;
double GST_to_UTC_time(double t_e, int32_t WN); //!< GST-UTC Conversion Algorithm and Parameters
bool flag_utc_model{};
template <class Archive>

View File

@ -40,7 +40,7 @@ Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message()
}
bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_BITS> bits)
bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_BITS> bits) const
{
uint32_t sum_bits = 0;
int32_t sum_hamming = 0;
@ -146,7 +146,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
}
bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value;
@ -162,7 +162,7 @@ bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset<GLONASS_G
}
uint64_t Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
uint64_t Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
uint64_t value = 0ULL;
int32_t num_of_slices = parameter.size();
@ -181,7 +181,7 @@ uint64_t Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std::bitset<G
}
int64_t Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
int64_t Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
int64_t value = 0LL;
int64_t sign = 0LL;
@ -628,19 +628,7 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
}
Glonass_Gnav_Ephemeris Glonass_Gnav_Navigation_Message::get_ephemeris()
{
return gnav_ephemeris;
}
Glonass_Gnav_Utc_Model Glonass_Gnav_Navigation_Message::get_utc_model()
{
return gnav_utc_model;
}
Glonass_Gnav_Almanac Glonass_Gnav_Navigation_Message::get_almanac(uint32_t satellite_slot_number)
Glonass_Gnav_Almanac Glonass_Gnav_Navigation_Message::get_almanac(uint32_t satellite_slot_number) const
{
return gnav_almanac[satellite_slot_number - 1];
}

View File

@ -48,61 +48,11 @@ public:
*/
Glonass_Gnav_Navigation_Message();
bool flag_CRC_test{};
uint32_t d_frame_ID{};
uint32_t d_string_ID{};
bool flag_update_slot_number{};
int32_t i_channel_ID{};
uint32_t i_satellite_PRN{};
Glonass_Gnav_Ephemeris gnav_ephemeris{}; //!< Ephemeris information decoded
Glonass_Gnav_Utc_Model gnav_utc_model{}; //!< UTC model information
Glonass_Gnav_Almanac gnav_almanac[GLONASS_CA_NBR_SATS]{}; //!< Almanac information for all 24 satellites
// Ephemeris Flags and control variables
bool flag_all_ephemeris{}; //!< Flag indicating that all strings containing ephemeris have been received
bool flag_ephemeris_str_1{}; //!< Flag indicating that ephemeris 1/4 (string 1) have been received
bool flag_ephemeris_str_2{}; //!< Flag indicating that ephemeris 2/4 (string 2) have been received
bool flag_ephemeris_str_3{}; //!< Flag indicating that ephemeris 3/4 (string 3) have been received
bool flag_ephemeris_str_4{}; //!< Flag indicating that ephemeris 4/4 (string 4) have been received
// Almanac Flags
bool flag_all_almanac{}; //!< Flag indicating that all almanac have been received
bool flag_almanac_str_6{}; //!< Flag indicating that almanac of string 6 have been received
bool flag_almanac_str_7{}; //!< Flag indicating that almanac of string 7 have been received
bool flag_almanac_str_8{}; //!< Flag indicating that almanac of string 8 have been received
bool flag_almanac_str_9{}; //!< Flag indicating that almanac of string 9 have been received
bool flag_almanac_str_10{}; //!< Flag indicating that almanac of string 10 have been received
bool flag_almanac_str_11{}; //!< Flag indicating that almanac of string 11 have been received
bool flag_almanac_str_12{}; //!< Flag indicating that almanac of string 12 have been received
bool flag_almanac_str_13{}; //!< Flag indicating that almanac of string 13 have been received
bool flag_almanac_str_14{}; //!< Flag indicating that almanac of string 14 have been received
bool flag_almanac_str_15{}; //!< Flag indicating that almanac of string 15 have been received
uint32_t i_alm_satellite_slot_number{}; //!< SV Orbit Slot Number
// UTC and System Clocks Flags
bool flag_utc_model_valid{}; //!< If set, it indicates that the UTC model parameters are filled
bool flag_utc_model_str_5{}; //!< Clock info send in string 5 of navigation data
bool flag_utc_model_str_15{}; //!< Clock info send in string 15 of frame 5 of navigation data
bool flag_TOW_set{}; //!< Flag indicating when the TOW has been set
bool flag_TOW_new{}; //!< Flag indicating when a new TOW has been computed
double d_satClkCorr{}; //!< Satellite clock error
double d_dtr{}; //!< Relativistic clock correction term
double d_satClkDrift{}; //!< Satellite clock drift
double d_previous_tb{}; //!< Previous iode for the Glonass_Gnav_Ephemeris object. Used to determine when new data arrives
double d_previous_Na[GLONASS_CA_NBR_SATS]{}; //!< Previous time for almanac of the Glonass_Gnav_Almanac object
std::map<int, std::string> satelliteBlock; // Map that stores to which block the PRN belongs
/*!
* \brief Compute CRC for GLONASS GNAV strings
* \param bits Bits of the string message where to compute CRC
*/
bool CRC_test(std::bitset<GLONASS_GNAV_STRING_BITS> bits);
bool CRC_test(std::bitset<GLONASS_GNAV_STRING_BITS> bits) const;
/*!
* \brief Computes the frame number being decoded given the satellite slot number
@ -114,19 +64,25 @@ public:
/*!
* \brief Obtain a GLONASS GNAV SV Ephemeris class filled with current SV data
*/
Glonass_Gnav_Ephemeris get_ephemeris();
Glonass_Gnav_Ephemeris get_ephemeris() const
{
return gnav_ephemeris;
}
/*!
* \brief Obtain a GLONASS GNAV UTC model parameters class filled with current SV data
*/
Glonass_Gnav_Utc_Model get_utc_model();
inline Glonass_Gnav_Utc_Model get_utc_model() const
{
return gnav_utc_model;
}
/*!
* \brief Returns a Glonass_Gnav_Almanac object filled with the latest navigation data received
* \param satellite_slot_number Slot number identifier for the satellite
* \returns Returns the Glonass_Gnav_Almanac object for the input slot number
*/
Glonass_Gnav_Almanac get_almanac(uint32_t satellite_slot_number);
Glonass_Gnav_Almanac get_almanac(uint32_t satellite_slot_number) const;
/*!
* \brief Returns true if a new Glonass_Gnav_Ephemeris object has arrived.
@ -150,10 +106,111 @@ public:
*/
int32_t string_decoder(const std::string& frame_string);
inline bool get_flag_CRC_test() const
{
return flag_CRC_test;
}
inline void set_rf_link(int32_t rf_link)
{
gnav_ephemeris.i_satellite_freq_channel = rf_link;
}
inline uint32_t get_alm_satellite_slot_number() const
{
return i_alm_satellite_slot_number;
}
inline bool get_flag_update_slot_number() const
{
return flag_update_slot_number;
}
inline void set_flag_update_slot_number(bool flag_slot)
{
flag_update_slot_number = flag_slot;
}
inline bool get_flag_TOW_new() const
{
return flag_TOW_new;
}
inline void set_flag_TOW_new(bool tow_new)
{
flag_TOW_new = tow_new;
}
inline bool is_flag_TOW_set() const
{
return flag_TOW_set;
}
inline void set_flag_ephemeris_str_1(bool ephemeris_str_1)
{
flag_ephemeris_str_1 = ephemeris_str_1;
}
inline void set_flag_ephemeris_str_2(bool ephemeris_str_2)
{
flag_ephemeris_str_2 = ephemeris_str_2;
}
inline void set_flag_ephemeris_str_3(bool ephemeris_str_3)
{
flag_ephemeris_str_3 = ephemeris_str_3;
}
inline void set_flag_ephemeris_str_4(bool ephemeris_str_4)
{
flag_ephemeris_str_4 = ephemeris_str_4;
}
private:
uint64_t read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
int64_t read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
bool read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
uint64_t read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
int64_t read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
bool read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
Glonass_Gnav_Ephemeris gnav_ephemeris{}; // Ephemeris information decoded
Glonass_Gnav_Utc_Model gnav_utc_model{}; // UTC model information
Glonass_Gnav_Almanac gnav_almanac[GLONASS_CA_NBR_SATS]{}; // Almanac information for all 24 satellites
std::map<int, std::string> satelliteBlock; // Map that stores to which block the PRN belongs
double d_previous_tb{}; // Previous iode for the Glonass_Gnav_Ephemeris object. Used to determine when new data arrives
double d_previous_Na[GLONASS_CA_NBR_SATS]{}; // Previous time for almanac of the Glonass_Gnav_Almanac object
uint32_t d_frame_ID{};
uint32_t d_string_ID{};
uint32_t i_alm_satellite_slot_number{}; // SV Orbit Slot Number
bool flag_CRC_test{};
bool flag_update_slot_number{};
// Ephemeris Flags and control variables
bool flag_all_ephemeris{}; // Flag indicating that all strings containing ephemeris have been received
bool flag_ephemeris_str_1{}; // Flag indicating that ephemeris 1/4 (string 1) have been received
bool flag_ephemeris_str_2{}; // Flag indicating that ephemeris 2/4 (string 2) have been received
bool flag_ephemeris_str_3{}; // Flag indicating that ephemeris 3/4 (string 3) have been received
bool flag_ephemeris_str_4{}; // Flag indicating that ephemeris 4/4 (string 4) have been received
// Almanac Flags
bool flag_almanac_str_6{}; // Flag indicating that almanac of string 6 have been received
bool flag_almanac_str_7{}; // Flag indicating that almanac of string 7 have been received
bool flag_almanac_str_8{}; // Flag indicating that almanac of string 8 have been received
bool flag_almanac_str_9{}; // Flag indicating that almanac of string 9 have been received
bool flag_almanac_str_10{}; // Flag indicating that almanac of string 10 have been received
bool flag_almanac_str_11{}; // Flag indicating that almanac of string 11 have been received
bool flag_almanac_str_12{}; // Flag indicating that almanac of string 12 have been received
bool flag_almanac_str_13{}; // Flag indicating that almanac of string 13 have been received
bool flag_almanac_str_14{}; // Flag indicating that almanac of string 14 have been received
bool flag_almanac_str_15{}; // Flag indicating that almanac of string 15 have been received
// UTC and System Clocks Flags
bool flag_utc_model_str_5{}; // Clock info send in string 5 of navigation data
bool flag_TOW_set{}; // Flag indicating when the TOW has been set
bool flag_TOW_new{}; // Flag indicating when a new TOW has been computed
};
#endif

View File

@ -37,9 +37,18 @@
class Gnss_Satellite
{
public:
Gnss_Satellite() = default; //!< Default Constructor.
Gnss_Satellite(const std::string& system_, uint32_t PRN_); //!< Concrete GNSS satellite Constructor.
~Gnss_Satellite() = default; //!< Default Destructor.
Gnss_Satellite() = default; //!< Default Constructor.
Gnss_Satellite(const std::string& system_, uint32_t PRN_); //!< Concrete GNSS satellite Constructor.
~Gnss_Satellite() = default; //!< Default Destructor.
Gnss_Satellite(const Gnss_Satellite& other) noexcept; //!< Copy constructor
Gnss_Satellite& operator=(const Gnss_Satellite&); //!< Copy assignment operator
Gnss_Satellite(Gnss_Satellite&& other) noexcept; //!< Move constructor
Gnss_Satellite& operator=(Gnss_Satellite&& other) noexcept; //!< Move assignment operator
friend bool operator==(const Gnss_Satellite& /*sat1*/, const Gnss_Satellite& /*sat2*/); //!< operator== for comparison
friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Satellite& /*sat*/); //!< operator<< for pretty printing
void update_PRN(uint32_t PRN); //!< Updates the PRN Number when information is decoded, only applies to GLONASS GNAV messages
uint32_t get_PRN() const; //!< Gets satellite's PRN
int32_t get_rf_link() const; //!< Gets the satellite's rf link
@ -48,19 +57,7 @@ public:
std::string get_block() const; //!< Gets the satellite block. If GPS, returns {"IIA", "IIR", "IIR-M", "IIF"}
std::string what_block(const std::string& system_, uint32_t PRN_); //!< Gets the block of a given satellite
friend bool operator==(const Gnss_Satellite& /*sat1*/, const Gnss_Satellite& /*sat2*/); //!< operator== for comparison
friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Satellite& /*sat*/); //!< operator<< for pretty printing
Gnss_Satellite(const Gnss_Satellite& other) noexcept; //!< Copy constructor
Gnss_Satellite& operator=(const Gnss_Satellite&); //!< Copy assignment operator
Gnss_Satellite(Gnss_Satellite&& other) noexcept; //!< Move constructor
Gnss_Satellite& operator=(Gnss_Satellite&& other) noexcept; //!< Move assignment operator
private:
uint32_t PRN{};
int32_t rf_link{};
std::string system{};
std::string block{};
const std::set<std::string> system_set = {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"};
const std::map<std::string, std::string> satelliteSystem = {{"GPS", "G"}, {"Glonass", "R"}, {"SBAS", "S"}, {"Galileo", "E"}, {"Beidou", "C"}};
void set_system(const std::string& system); // Sets the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"}.
@ -68,5 +65,9 @@ private:
void set_block(const std::string& system_, uint32_t PRN_);
void reset();
void set_rf_link(int32_t rf_link_);
std::string system{};
std::string block{};
uint32_t PRN{};
int32_t rf_link{};
};
#endif

View File

@ -38,6 +38,24 @@ public:
*/
Gps_CNAV_Ephemeris() = default;
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200K)
*/
double satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_drift(double transmitTime);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
uint32_t i_satellite_PRN{}; // SV PRN NUMBER
// Message Types 10 and 11 Parameters (1 of 2)
@ -84,21 +102,6 @@ public:
int32_t d_TOW{}; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
/*! \brief If true, enhanced level of integrity assurance.
*
* If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.
* That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound
* value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less
* than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of
* integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73
* times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an
* accompanying alert, is less than 1E-8 per hour.
*/
bool b_integrity_status_flag{};
bool b_l2c_phasing_flag{};
bool b_alert_flag{}; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
bool b_antispoofing_flag{}; //!< If true, the AntiSpoofing mode is ON in that SV
// clock terms derived from ephemeris data
double d_satClkDrift{}; //!< GPS clock error
double d_dtr{}; //!< relativistic clock correction term
@ -113,23 +116,20 @@ public:
double d_satvel_Y{}; //!< Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z{}; //!< Earth-fixed velocity coordinate z of the satellite [m]
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200K)
/*! \brief If true, enhanced level of integrity assurance.
*
* If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.
* That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound
* value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less
* than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of
* integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73
* times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an
* accompanying alert, is less than 1E-8 per hour.
*/
double satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_drift(double transmitTime);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
bool b_integrity_status_flag{};
bool b_l2c_phasing_flag{};
bool b_alert_flag{}; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
bool b_antispoofing_flag{}; //!< If true, the AntiSpoofing mode is ON in that SV
template <class Archive>

View File

@ -35,7 +35,6 @@ class Gps_CNAV_Iono
public:
Gps_CNAV_Iono() = default; //!< Default constructor
bool valid{}; //!< Valid flag
// Ionospheric parameters
double d_alpha0{}; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
double d_alpha1{}; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
@ -45,6 +44,7 @@ public:
double d_beta1{}; //!< Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle]
double d_beta2{}; //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
double d_beta3{}; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]
bool valid{}; //!< Valid flag
template <class Archive>

View File

@ -36,7 +36,7 @@ Gps_CNAV_Navigation_Message::Gps_CNAV_Navigation_Message()
}
bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value;
@ -52,7 +52,7 @@ bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset<GPS_CNAV_DATA
}
uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
uint64_t value = 0ULL;
int32_t num_of_slices = parameter.size();
@ -71,7 +71,7 @@ uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_C
}
int64_t Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
int64_t Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
int64_t value = 0LL;
int32_t num_of_slices = parameter.size();
@ -306,7 +306,7 @@ bool Gps_CNAV_Navigation_Message::have_new_ephemeris() // Check if we have a ne
}
Gps_CNAV_Ephemeris Gps_CNAV_Navigation_Message::get_ephemeris()
Gps_CNAV_Ephemeris Gps_CNAV_Navigation_Message::get_ephemeris() const
{
return ephemeris_record;
}
@ -323,7 +323,7 @@ bool Gps_CNAV_Navigation_Message::have_new_iono() // Check if we have a new ion
}
Gps_CNAV_Iono Gps_CNAV_Navigation_Message::get_iono()
Gps_CNAV_Iono Gps_CNAV_Navigation_Message::get_iono() const
{
return iono_record;
}

View File

@ -47,36 +47,12 @@ public:
*/
Gps_CNAV_Navigation_Message();
int32_t d_TOW{};
bool b_flag_ephemeris_1{};
bool b_flag_ephemeris_2{};
bool b_flag_iono_valid{}; //!< If set, it indicates that the ionospheric parameters are filled and are not yet read by the get_iono
bool b_flag_utc_valid{}; //!< If set, it indicates that the utc parameters are filled and are not yet read by the get_utc_model
std::map<int32_t, std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs https://www.navcen.uscg.gov/?Do=constellationStatus
// satellite positions
double d_satpos_X{}; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double d_satpos_Y{}; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double d_satpos_Z{}; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
// satellite identification info
int32_t i_channel_ID{};
uint32_t i_satellite_PRN{};
// Satellite velocity
double d_satvel_X{}; //!< Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y{}; //!< Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z{}; //!< Earth-fixed velocity coordinate z of the satellite [m]
// public functions
void decode_page(std::bitset<GPS_CNAV_DATA_PAGE_BITS> data_bits);
/*!
* \brief Obtain a GPS SV Ephemeris class filled with current SV data
*/
Gps_CNAV_Ephemeris get_ephemeris();
Gps_CNAV_Ephemeris get_ephemeris() const;
/*!
* \brief Check if we have a new iono record stored in the GPS ephemeris class
@ -86,7 +62,7 @@ public:
/*!
* \brief Obtain a GPS ionospheric correction parameters class filled with current SV data
*/
Gps_CNAV_Iono get_iono();
Gps_CNAV_Iono get_iono() const;
/*!
* \brief Obtain a GPS UTC model parameters class filled with current SV data
@ -104,13 +80,22 @@ public:
bool have_new_ephemeris();
private:
uint64_t read_navigation_unsigned(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
int64_t read_navigation_signed(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
bool read_navigation_bool(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
uint64_t read_navigation_unsigned(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
int64_t read_navigation_signed(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
bool read_navigation_bool(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
Gps_CNAV_Ephemeris ephemeris_record{};
Gps_CNAV_Iono iono_record{};
Gps_CNAV_Utc_Model utc_model_record{};
std::map<int32_t, std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs https://www.navcen.uscg.gov/?Do=constellationStatus
int32_t d_TOW{};
bool b_flag_ephemeris_1{};
bool b_flag_ephemeris_2{};
bool b_flag_iono_valid{}; //!< If set, it indicates that the ionospheric parameters are filled and are not yet read by the get_iono
bool b_flag_utc_valid{}; //!< If set, it indicates that the utc parameters are filled and are not yet read by the get_utc_model
};
#endif

View File

@ -37,7 +37,12 @@ public:
*/
Gps_CNAV_Utc_Model() = default;
bool valid{};
/*!
* \brief Computes the Coordinated Universal Time (UTC) and
* returns it in [s] (IS-GPS-200K, 20.3.3.5.2.4 + 30.3.3.6.2)
*/
double utc_time(double gpstime_corrected, int32_t i_GPS_week);
// UTC parameters
double d_A2{}; //!< 2nd order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s/s]
double d_A1{}; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s/s]
@ -48,12 +53,7 @@ public:
int32_t i_WN_LSF{}; //!< Week number at the end of which the leap second becomes effective [weeks]
int32_t i_DN{}; //!< Day number (DN) at the end of which the leap second becomes effective [days]
int32_t d_DeltaT_LSF{}; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
/*!
* \brief Computes the Coordinated Universal Time (UTC) and
* returns it in [s] (IS-GPS-200K, 20.3.3.5.2.4 + 30.3.3.6.2)
*/
double utc_time(double gpstime_corrected, int32_t i_GPS_week);
bool valid{};
template <class Archive>
/*
@ -65,7 +65,6 @@ public:
if (version)
{
};
archive& make_nvp("valid", valid);
archive& make_nvp("d_A1", d_A1);
archive& make_nvp("d_A0", d_A0);
archive& make_nvp("d_t_OT", d_t_OT);
@ -74,6 +73,7 @@ public:
archive& make_nvp("i_WN_LSF", i_WN_LSF);
archive& make_nvp("i_DN", i_DN);
archive& make_nvp("d_DeltaT_LSF", d_DeltaT_LSF);
archive& make_nvp("valid", valid);
}
};

View File

@ -41,6 +41,25 @@ public:
*/
Gps_Ephemeris();
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200K)
* and compute the clock bias term including relativistic effect (return value)
*/
double satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_drift(double transmitTime);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
uint32_t i_satellite_PRN{}; // SV PRN NUMBER
int32_t d_TOW{}; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
double d_Crs{}; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
@ -111,25 +130,6 @@ public:
std::map<int, std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs https://www.navcen.uscg.gov/?Do=constellationStatus
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Table 20-IV (IS-GPS-200K)
* and compute the clock bias term including relativistic effect (return value)
*/
double satellitePosition(double transmitTime);
/*!
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_drift(double transmitTime);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200K, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
template <class Archive>
/*!

View File

@ -1,13 +1,14 @@
/*!
m * \file gps_navigation_message.cc
* \file gps_navigation_message.cc
* \brief Implementation of a GPS NAV Data message decoder as described in IS-GPS-200K
*
* See https://www.gps.gov/technical/icwg/IS-GPS-200K.pdf Appendix II
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
* See https://www.gps.gov/technical/icwg/IS-GPS-200K.pdf Appendix II
*
* Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
*
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
@ -16,7 +17,7 @@ m * \file gps_navigation_message.cc
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
* -----------------------------------------------------------------------------
*/
#include "gps_navigation_message.h"
@ -42,7 +43,7 @@ Gps_Navigation_Message::Gps_Navigation_Message()
}
void Gps_Navigation_Message::print_gps_word_bytes(uint32_t GPS_word)
void Gps_Navigation_Message::print_gps_word_bytes(uint32_t GPS_word) const
{
std::cout << " Word =";
std::cout << std::bitset<32>(GPS_word);
@ -50,7 +51,7 @@ void Gps_Navigation_Message::print_gps_word_bytes(uint32_t GPS_word)
}
bool Gps_Navigation_Message::read_navigation_bool(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
bool Gps_Navigation_Message::read_navigation_bool(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
bool value;
@ -66,7 +67,7 @@ bool Gps_Navigation_Message::read_navigation_bool(std::bitset<GPS_SUBFRAME_BITS>
}
uint64_t Gps_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
uint64_t Gps_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
uint64_t value = 0ULL;
int32_t num_of_slices = parameter.size();
@ -85,7 +86,7 @@ uint64_t Gps_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_SUBFRA
}
int64_t Gps_Navigation_Message::read_navigation_signed(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter)
int64_t Gps_Navigation_Message::read_navigation_signed(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{
int64_t value = 0LL;
int32_t num_of_slices = parameter.size();
@ -428,7 +429,7 @@ double Gps_Navigation_Message::utc_time(const double gpstime_corrected) const
}
Gps_Ephemeris Gps_Navigation_Message::get_ephemeris()
Gps_Ephemeris Gps_Navigation_Message::get_ephemeris() const
{
Gps_Ephemeris ephemeris;
ephemeris.i_satellite_PRN = i_satellite_PRN;

View File

@ -3,9 +3,10 @@
* \brief Interface of a GPS NAV Data message decoder
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2019 (see AUTHORS file for a list of contributors)
* -----------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
@ -14,7 +15,7 @@
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
* -----------------------------------------------------------------------------
*/
@ -47,129 +48,10 @@ public:
*/
Gps_Navigation_Message();
bool b_valid_ephemeris_set_flag{}; // flag indicating that this ephemeris set have passed the validation check
// broadcast orbit 1
int32_t d_TOW{}; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
int32_t d_TOW_SF1{}; //!< Time of GPS Week from HOW word of Subframe 1 [s]
int32_t d_TOW_SF2{}; //!< Time of GPS Week from HOW word of Subframe 2 [s]
int32_t d_TOW_SF3{}; //!< Time of GPS Week from HOW word of Subframe 3 [s]
int32_t d_TOW_SF4{}; //!< Time of GPS Week from HOW word of Subframe 4 [s]
int32_t d_TOW_SF5{}; //!< Time of GPS Week from HOW word of Subframe 5 [s]
int32_t d_IODE_SF2{};
int32_t d_IODE_SF3{};
double d_Crs{}; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
double d_Delta_n{}; //!< Mean Motion Difference From Computed Value [semi-circles/s]
double d_M_0{}; //!< Mean Anomaly at Reference Time [semi-circles]
// broadcast orbit 2
double d_Cuc{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_e_eccentricity{}; //!< Eccentricity [dimensionless]
double d_Cus{}; //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad]
double d_sqrt_A{}; //!< Square Root of the Semi-Major Axis [sqrt(m)]
// broadcast orbit 3
int32_t d_Toe{}; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200K) [s]
int32_t d_Toc{}; //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200K) [s]
double d_Cic{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad]
double d_OMEGA0{}; //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
double d_Cis{}; //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad]
// broadcast orbit 4
double d_i_0{}; //!< Inclination Angle at Reference Time [semi-circles]
double d_Crc{}; //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m]
double d_OMEGA{}; //!< Argument of Perigee [semi-cicles]
double d_OMEGA_DOT{}; //!< Rate of Right Ascension [semi-circles/s]
// broadcast orbit 5
double d_IDOT{}; //!< Rate of Inclination Angle [semi-circles/s]
int32_t i_code_on_L2{}; //!< If 1, P code ON in L2; if 2, C/A code ON in L2;
int32_t i_GPS_week{}; //!< GPS week number, aka WN [week]
bool b_L2_P_data_flag{}; //!< When true, indicates that the NAV data stream was commanded OFF on the P-code of the L2 channel
// broadcast orbit 6
int32_t i_SV_accuracy{}; //!< User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200K)
int32_t i_SV_health{};
double d_TGD{}; //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
int32_t d_IODC{}; //!< Issue of Data, Clock
// broadcast orbit 7
int32_t i_AODO{}; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
bool b_fit_interval_flag{}; //!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
double d_spare1{};
double d_spare2{};
double d_A_f0{}; //!< Coefficient 0 of code phase offset model [s]
double d_A_f1{}; //!< Coefficient 1 of code phase offset model [s/s]
double d_A_f2{}; //!< Coefficient 2 of code phase offset model [s/s^2]
// Almanac
int32_t i_Toa{}; //!< Almanac reference time [s]
int32_t i_WN_A{}; //!< Modulo 256 of the GPS week number to which the almanac reference time (i_Toa) is referenced
std::map<int32_t, int32_t> almanacHealth; //!< Map that stores the health information stored in the almanac
std::map<int32_t, std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs https://www.navcen.uscg.gov/?Do=constellationStatus
// Flags
/*! \brief If true, enhanced level of integrity assurance.
*
* If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.
* That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound
* value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less
* than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of
* integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73
* times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an
* accompanying alert, is less than 1E-8 per hour.
*/
bool b_integrity_status_flag{};
bool b_alert_flag{}; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
bool b_antispoofing_flag{}; //!< If true, the AntiSpoofing mode is ON in that SV
// clock terms
// double d_master_clock{}; // GPS transmission time
double d_satClkCorr{}; // GPS clock error
double d_dtr{}; // relativistic clock correction term
double d_satClkDrift{};
// satellite positions
double d_satpos_X{}; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double d_satpos_Y{}; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double d_satpos_Z{}; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
// satellite identification info
int32_t i_channel_ID{};
uint32_t i_satellite_PRN{};
// time synchro
double d_subframe_timestamp_ms{}; // [ms]
// Ionospheric parameters
bool flag_iono_valid{}; //!< If set, it indicates that the ionospheric parameters are filled (page 18 has arrived and decoded)
double d_alpha0{}; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
double d_alpha1{}; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
double d_alpha2{}; //!< Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2]
double d_alpha3{}; //!< Coefficient 3 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^3]
double d_beta0{}; //!< Coefficient 0 of a cubic equation representing the period of the model [s]
double d_beta1{}; //!< Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle]
double d_beta2{}; //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
double d_beta3{}; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]
// UTC parameters
bool flag_utc_model_valid{}; //!< If set, it indicates that the UTC model parameters are filled
double d_A0{}; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s]
double d_A1{}; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s/s]
double d_A2{}; //!< 2nd order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s/s]
int32_t d_t_OT{}; //!< Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200K) [s]
int32_t i_WN_T{}; //!< UTC reference week number [weeks]
int32_t d_DeltaT_LS{}; //!< delta time due to leap seconds [s]. Number of leap seconds since 6-Jan-1980 as transmitted by the GPS almanac.
int32_t i_WN_LSF{}; //!< Week number at the end of which the leap second becomes effective [weeks]
int32_t i_DN{}; //!< Day number (DN) at the end of which the leap second becomes effective [days]
int32_t d_DeltaT_LSF{}; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
// Satellite velocity
double d_satvel_X{}; //!< Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y{}; //!< Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z{}; //!< Earth-fixed velocity coordinate z of the satellite [m]
// public functions
/*!
* \brief Obtain a GPS SV Ephemeris class filled with current SV data
*/
Gps_Ephemeris get_ephemeris();
Gps_Ephemeris get_ephemeris() const;
/*!
* \brief Obtain a GPS ionospheric correction parameters class filled with current SV data
@ -181,7 +63,6 @@ public:
*/
Gps_Utc_Model get_utc_model();
/*!
* \brief Decodes the GPS NAV message
*/
@ -193,13 +74,184 @@ public:
*/
double utc_time(const double gpstime_corrected) const;
/*!
* \brief Gets Time of Week, in seconds
*/
inline int32_t get_TOW() const
{
return d_TOW;
}
/*!
* \brief Sets Time of Week, in seconds
*/
inline int32_t get_GPS_week() const
{
return i_GPS_week;
}
/*!
* \brief Sets satellite PRN number
*/
inline void set_satellite_PRN(uint32_t prn)
{
i_satellite_PRN = prn;
}
/*!
* \brief Gets satellite PRN number
*/
inline uint32_t get_satellite_PRN() const
{
return i_satellite_PRN;
}
/*!
* \brief Sets channel ID
*/
inline void set_channel(int32_t channel_id)
{
i_channel_ID = channel_id;
}
/*!
* \brief Gets flag_iono_valid
*/
inline bool get_flag_iono_valid() const
{
return flag_iono_valid;
}
/*!
* \brief Gets flag_utc_model_valid
*/
inline bool get_flag_utc_model_valid() const
{
return flag_utc_model_valid;
}
bool satellite_validation();
private:
uint64_t read_navigation_unsigned(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
int64_t read_navigation_signed(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
bool read_navigation_bool(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter);
void print_gps_word_bytes(uint32_t GPS_word);
uint64_t read_navigation_unsigned(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
int64_t read_navigation_signed(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
bool read_navigation_bool(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& parameter) const;
void print_gps_word_bytes(uint32_t GPS_word) const;
std::map<int32_t, int32_t> almanacHealth; //!< Map that stores the health information stored in the almanac
std::map<int32_t, std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs https://www.navcen.uscg.gov/?Do=constellationStatus
// broadcast orbit 1
int32_t d_TOW{}; // Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
int32_t d_TOW_SF1{}; // Time of GPS Week from HOW word of Subframe 1 [s]
int32_t d_TOW_SF2{}; // Time of GPS Week from HOW word of Subframe 2 [s]
int32_t d_TOW_SF3{}; // Time of GPS Week from HOW word of Subframe 3 [s]
int32_t d_TOW_SF4{}; // Time of GPS Week from HOW word of Subframe 4 [s]
int32_t d_TOW_SF5{}; // Time of GPS Week from HOW word of Subframe 5 [s]
int32_t d_IODE_SF2{};
int32_t d_IODE_SF3{};
double d_Crs{}; // Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
double d_Delta_n{}; // Mean Motion Difference From Computed Value [semi-circles/s]
double d_M_0{}; // Mean Anomaly at Reference Time [semi-circles]
// broadcast orbit 2
double d_Cuc{}; // Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_e_eccentricity{}; // Eccentricity [dimensionless]
double d_Cus{}; // Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad]
double d_sqrt_A{}; // Square Root of the Semi-Major Axis [sqrt(m)]
// broadcast orbit 3
int32_t d_Toe{}; // Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200K) [s]
int32_t d_Toc{}; // clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200K) [s]
double d_Cic{}; // Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad]
double d_OMEGA0{}; // Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
double d_Cis{}; // Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad]
// broadcast orbit 4
double d_i_0{}; // Inclination Angle at Reference Time [semi-circles]
double d_Crc{}; // Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m]
double d_OMEGA{}; // Argument of Perigee [semi-cicles]
double d_OMEGA_DOT{}; // Rate of Right Ascension [semi-circles/s]
// broadcast orbit 5
double d_IDOT{}; // Rate of Inclination Angle [semi-circles/s]
int32_t i_code_on_L2{}; // If 1, P code ON in L2; if 2, C/A code ON in L2;
int32_t i_GPS_week{}; // GPS week number, aka WN [week]
bool b_L2_P_data_flag{}; // When true, indicates that the NAV data stream was commanded OFF on the P-code of the L2 channel
// broadcast orbit 6
int32_t i_SV_accuracy{}; // User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200K)
int32_t i_SV_health{};
double d_TGD{}; // Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
int32_t d_IODC{}; // Issue of Data, Clock
// broadcast orbit 7
int32_t i_AODO{}; // Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
bool b_fit_interval_flag{}; // indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
double d_spare1{};
double d_spare2{};
double d_A_f0{}; // Coefficient 0 of code phase offset model [s]
double d_A_f1{}; // Coefficient 1 of code phase offset model [s/s]
double d_A_f2{}; // Coefficient 2 of code phase offset model [s/s^2]
// Almanac
int32_t i_Toa{}; // Almanac reference time [s]
int32_t i_WN_A{}; // Modulo 256 of the GPS week number to which the almanac reference time (i_Toa) is referenced
// clock terms
// double d_master_clock{}; // GPS transmission time
double d_dtr{}; // relativistic clock correction term
double d_satClkDrift{};
// satellite positions
double d_satpos_X{}; // Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double d_satpos_Y{}; // Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double d_satpos_Z{}; // Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
// Satellite velocity
double d_satvel_X{}; // Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y{}; // Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z{}; // Earth-fixed velocity coordinate z of the satellite [m]
// satellite identification info
int32_t i_channel_ID{};
uint32_t i_satellite_PRN{};
// Ionospheric parameters
double d_alpha0{}; // Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s]
double d_alpha1{}; // Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle]
double d_alpha2{}; // Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2]
double d_alpha3{}; // Coefficient 3 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^3]
double d_beta0{}; // Coefficient 0 of a cubic equation representing the period of the model [s]
double d_beta1{}; // Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle]
double d_beta2{}; // Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2]
double d_beta3{}; // Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3]
// UTC parameters
double d_A0{}; // Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s]
double d_A1{}; // 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s/s]
int32_t d_t_OT{}; // Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200K) [s]
int32_t i_WN_T{}; // UTC reference week number [weeks]
int32_t d_DeltaT_LS{}; // delta time due to leap seconds [s]. Number of leap seconds since 6-Jan-1980 as transmitted by the GPS almanac.
int32_t i_WN_LSF{}; // Week number at the end of which the leap second becomes effective [weeks]
int32_t i_DN{}; // Day number (DN) at the end of which the leap second becomes effective [days]
int32_t d_DeltaT_LSF{}; // Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
// Flags
bool b_valid_ephemeris_set_flag{}; // flag indicating that this ephemeris set have passed the validation check
bool flag_iono_valid{}; // If set, it indicates that the ionospheric parameters are filled (page 18 has arrived and decoded)
bool flag_utc_model_valid{}; // If set, it indicates that the UTC model parameters are filled
/* If true, enhanced level of integrity assurance.
*
* If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.
* That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound
* value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less
* than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of
* integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73
* times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an
* accompanying alert, is less than 1E-8 per hour.
*/
bool b_integrity_status_flag{};
bool b_alert_flag{}; // If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
bool b_antispoofing_flag{}; // If true, the AntiSpoofing mode is ON in that SV
};
#endif

View File

@ -37,7 +37,6 @@ public:
*/
Gps_Utc_Model() = default;
bool valid{};
// UTC parameters
double d_A0{}; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s]
double d_A1{}; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200K) [s/s]
@ -49,6 +48,8 @@ public:
int32_t i_DN{}; //!< Day number (DN) at the end of which the leap second becomes effective [days]
int32_t d_DeltaT_LSF{}; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
bool valid{};
template <class Archive>
/*
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.
@ -59,7 +60,6 @@ public:
if (version)
{
};
archive& make_nvp("valid", valid);
archive& make_nvp("d_A1", d_A1);
archive& make_nvp("d_A0", d_A0);
archive& make_nvp("d_t_OT", d_t_OT);
@ -68,6 +68,7 @@ public:
archive& make_nvp("i_WN_LSF", i_WN_LSF);
archive& make_nvp("i_DN", i_DN);
archive& make_nvp("d_DeltaT_LSF", d_DeltaT_LSF);
archive& make_nvp("valid", valid);
}
/*!

View File

@ -105,7 +105,7 @@ public:
{
// DECODE COMPLETE WORD (even + odd) and TEST CRC
INAV_decoder.split_page(page_String, flag_even_word_arrived);
if (INAV_decoder.flag_CRC_test == true)
if (INAV_decoder.get_flag_CRC_test() == true)
{
std::cout << "Galileo E1 INAV PAGE CRC correct \n";
// std::cout << "Galileo E1 CRC correct on channel " << d_channel << " from satellite " << d_satellite << std::endl;
@ -164,7 +164,7 @@ public:
// DECODE COMPLETE WORD (even + odd) and TEST CRC
FNAV_decoder.split_page(page_String);
if (FNAV_decoder.flag_CRC_test == true)
if (FNAV_decoder.get_flag_CRC_test() == true)
{
std::cout << "Galileo E5a FNAV PAGE CRC correct \n";
return true;

View File

@ -89,11 +89,11 @@ TEST(GlonassGnavNavigationMessageTest, String1Decoder)
gnav_nav_message.string_decoder(str1);
// Perform assertions of decoded fields
ASSERT_TRUE(gnav_ephemeris.d_P_1 - gnav_nav_message.gnav_ephemeris.d_P_1 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_t_k - gnav_nav_message.gnav_ephemeris.d_t_k < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_VXn - gnav_nav_message.gnav_ephemeris.d_VXn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_AXn - gnav_nav_message.gnav_ephemeris.d_AXn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Xn - gnav_nav_message.gnav_ephemeris.d_Xn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P_1 - gnav_nav_message.get_ephemeris().d_P_1 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_t_k - gnav_nav_message.get_ephemeris().d_t_k < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_VXn - gnav_nav_message.get_ephemeris().d_VXn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_AXn - gnav_nav_message.get_ephemeris().d_AXn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Xn - gnav_nav_message.get_ephemeris().d_Xn < FLT_EPSILON);
}
@ -120,17 +120,16 @@ TEST(GlonassGnavNavigationMessageTest, String2Decoder)
gnav_ephemeris.d_Yn = -11456.7348632812;
// Call target test method
gnav_nav_message.flag_ephemeris_str_1 = true;
gnav_nav_message.gnav_ephemeris.d_P_1 = 15;
gnav_nav_message.set_flag_ephemeris_str_1(true);
gnav_nav_message.string_decoder(str2);
// Perform assertions of decoded fields
ASSERT_TRUE(gnav_ephemeris.d_B_n - gnav_nav_message.gnav_ephemeris.d_B_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P_2 - gnav_nav_message.gnav_ephemeris.d_P_2 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_t_b - gnav_nav_message.gnav_ephemeris.d_t_b < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_VYn - gnav_nav_message.gnav_ephemeris.d_VYn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_AYn - gnav_nav_message.gnav_ephemeris.d_AYn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Yn - gnav_nav_message.gnav_ephemeris.d_Yn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_B_n - gnav_nav_message.get_ephemeris().d_B_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P_2 - gnav_nav_message.get_ephemeris().d_P_2 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_t_b - gnav_nav_message.get_ephemeris().d_t_b < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_VYn - gnav_nav_message.get_ephemeris().d_VYn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_AYn - gnav_nav_message.get_ephemeris().d_AYn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Yn - gnav_nav_message.get_ephemeris().d_Yn < FLT_EPSILON);
}
@ -158,17 +157,17 @@ TEST(GlonassGnavNavigationMessageTest, String3Decoder)
gnav_ephemeris.d_Zn = 19929.2377929688;
// Call target test method
gnav_nav_message.flag_ephemeris_str_2 = true;
gnav_nav_message.set_flag_ephemeris_str_2(true);
gnav_nav_message.string_decoder(str3);
// Perform assertions of decoded fields
ASSERT_TRUE(gnav_ephemeris.d_P_3 - gnav_nav_message.gnav_ephemeris.d_P_3 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_gamma_n - gnav_nav_message.gnav_ephemeris.d_gamma_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P - gnav_nav_message.gnav_ephemeris.d_P < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_l3rd_n - gnav_nav_message.gnav_ephemeris.d_l3rd_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_VZn - gnav_nav_message.gnav_ephemeris.d_VZn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_AZn - gnav_nav_message.gnav_ephemeris.d_AZn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Zn - gnav_nav_message.gnav_ephemeris.d_Zn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P_3 - gnav_nav_message.get_ephemeris().d_P_3 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_gamma_n - gnav_nav_message.get_ephemeris().d_gamma_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P - gnav_nav_message.get_ephemeris().d_P < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_l3rd_n - gnav_nav_message.get_ephemeris().d_l3rd_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_VZn - gnav_nav_message.get_ephemeris().d_VZn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_AZn - gnav_nav_message.get_ephemeris().d_AZn < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Zn - gnav_nav_message.get_ephemeris().d_Zn < FLT_EPSILON);
}
@ -197,18 +196,18 @@ TEST(GlonassGnavNavigationMessageTest, String4Decoder)
gnav_ephemeris.d_M = 1;
// Call target test method
gnav_nav_message.flag_ephemeris_str_3 = true;
gnav_nav_message.set_flag_ephemeris_str_3(true);
gnav_nav_message.string_decoder(str4);
// Perform assertions of decoded fields
ASSERT_TRUE(gnav_ephemeris.d_tau_n - gnav_nav_message.gnav_ephemeris.d_tau_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Delta_tau_n - gnav_nav_message.gnav_ephemeris.d_Delta_tau_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_E_n - gnav_nav_message.gnav_ephemeris.d_E_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P_4 - gnav_nav_message.gnav_ephemeris.d_P_4 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_F_T - gnav_nav_message.gnav_ephemeris.d_F_T < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_N_T - gnav_nav_message.gnav_ephemeris.d_N_T < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_n - gnav_nav_message.gnav_ephemeris.d_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_M - gnav_nav_message.gnav_ephemeris.d_M < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_tau_n - gnav_nav_message.get_ephemeris().d_tau_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_Delta_tau_n - gnav_nav_message.get_ephemeris().d_Delta_tau_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_E_n - gnav_nav_message.get_ephemeris().d_E_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_P_4 - gnav_nav_message.get_ephemeris().d_P_4 < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_F_T - gnav_nav_message.get_ephemeris().d_F_T < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_N_T - gnav_nav_message.get_ephemeris().d_N_T < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_n - gnav_nav_message.get_ephemeris().d_n < FLT_EPSILON);
ASSERT_TRUE(gnav_ephemeris.d_M - gnav_nav_message.get_ephemeris().d_M < FLT_EPSILON);
}
@ -233,14 +232,14 @@ TEST(GlonassGnavNavigationMessageTest, String5Decoder)
gnav_utc_model.d_tau_gps = 9.313225746154785e-08;
// Call target test method
gnav_nav_message.flag_ephemeris_str_4 = true;
gnav_nav_message.set_flag_ephemeris_str_4(true);
gnav_nav_message.string_decoder(str5);
// Perform assertions of decoded fields
ASSERT_TRUE(gnav_utc_model.d_N_A - gnav_nav_message.gnav_utc_model.d_N_A < FLT_EPSILON);
ASSERT_TRUE(gnav_utc_model.d_tau_c - gnav_nav_message.gnav_utc_model.d_tau_c < FLT_EPSILON);
ASSERT_TRUE(gnav_utc_model.d_N_4 - gnav_nav_message.gnav_utc_model.d_N_4 < FLT_EPSILON);
ASSERT_TRUE(gnav_utc_model.d_tau_gps - gnav_nav_message.gnav_utc_model.d_tau_gps < FLT_EPSILON);
ASSERT_TRUE(gnav_utc_model.d_N_A - gnav_nav_message.get_utc_model().d_N_A < FLT_EPSILON);
ASSERT_TRUE(gnav_utc_model.d_tau_c - gnav_nav_message.get_utc_model().d_tau_c < FLT_EPSILON);
ASSERT_TRUE(gnav_utc_model.d_N_4 - gnav_nav_message.get_utc_model().d_N_4 < FLT_EPSILON);
ASSERT_TRUE(gnav_utc_model.d_tau_gps - gnav_nav_message.get_utc_model().d_tau_gps < FLT_EPSILON);
}
std::string str6("0011010100110100001100111100011100001101011000000110101111001000000101100011111011001");