mirror of
				https://github.com/gnss-sdr/gnss-sdr
				synced 2025-11-04 09:13:05 +00:00 
			
		
		
		
	Make clang-tidy happy
This commit is contained in:
		@@ -1,5 +1,5 @@
 | 
				
			|||||||
/*!
 | 
					/*!
 | 
				
			||||||
 * \file Pvt_Kf.cc
 | 
					 * \file pvt_kf.cc
 | 
				
			||||||
 * \brief Kalman Filter for Position and Velocity
 | 
					 * \brief Kalman Filter for Position and Velocity
 | 
				
			||||||
 * \author Javier Arribas, 2023. jarribas(at)cttc.es
 | 
					 * \author Javier Arribas, 2023. jarribas(at)cttc.es
 | 
				
			||||||
 *
 | 
					 *
 | 
				
			||||||
@@ -14,11 +14,13 @@
 | 
				
			|||||||
 *
 | 
					 *
 | 
				
			||||||
 * -----------------------------------------------------------------------------
 | 
					 * -----------------------------------------------------------------------------
 | 
				
			||||||
 */
 | 
					 */
 | 
				
			||||||
 | 
					
 | 
				
			||||||
#include "pvt_kf.h"
 | 
					#include "pvt_kf.h"
 | 
				
			||||||
#include <glog/logging.h>
 | 
					#include <glog/logging.h>
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
 | 
					void Pvt_Kf::init_kf(const arma::vec& p,
 | 
				
			||||||
 | 
					    const arma::vec& v,
 | 
				
			||||||
    double solver_interval_s,
 | 
					    double solver_interval_s,
 | 
				
			||||||
    double measures_ecef_pos_sd_m,
 | 
					    double measures_ecef_pos_sd_m,
 | 
				
			||||||
    double measures_ecef_vel_sd_ms,
 | 
					    double measures_ecef_vel_sd_ms,
 | 
				
			||||||
@@ -28,7 +30,6 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
 | 
				
			|||||||
    // Kalman Filter class variables
 | 
					    // Kalman Filter class variables
 | 
				
			||||||
    const double Ti = solver_interval_s;
 | 
					    const double Ti = solver_interval_s;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
    std::cout << "Ti=" << Ti << "\n";
 | 
					 | 
				
			||||||
    d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
 | 
					    d_F = {{1.0, 0.0, 0.0, Ti, 0.0, 0.0},
 | 
				
			||||||
        {0.0, 1.0, 0.0, 0.0, Ti, 0.0},
 | 
					        {0.0, 1.0, 0.0, 0.0, Ti, 0.0},
 | 
				
			||||||
        {0.0, 0.0, 1.0, 0.0, 0.0, Ti},
 | 
					        {0.0, 0.0, 1.0, 0.0, 0.0, Ti},
 | 
				
			||||||
@@ -38,7 +39,7 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
 | 
				
			|||||||
 | 
					
 | 
				
			||||||
    d_H = arma::eye(6, 6);
 | 
					    d_H = arma::eye(6, 6);
 | 
				
			||||||
 | 
					
 | 
				
			||||||
    //   measurement matrix static covariances
 | 
					    // measurement matrix static covariances
 | 
				
			||||||
    d_R = {{pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
 | 
					    d_R = {{pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
 | 
				
			||||||
        {0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
 | 
					        {0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
 | 
				
			||||||
        {0.0, 0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0},
 | 
					        {0.0, 0.0, pow(measures_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0},
 | 
				
			||||||
@@ -46,9 +47,7 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
 | 
				
			|||||||
        {0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0},
 | 
					        {0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0), 0.0},
 | 
				
			||||||
        {0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}};
 | 
					        {0.0, 0.0, 0.0, 0.0, 0.0, pow(measures_ecef_vel_sd_ms, 2.0)}};
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					 | 
				
			||||||
    // system covariance matrix (static)
 | 
					    // system covariance matrix (static)
 | 
				
			||||||
 | 
					 | 
				
			||||||
    d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
 | 
					    d_Q = {{pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0, 0.0},
 | 
				
			||||||
        {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
 | 
					        {0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0, 0.0},
 | 
				
			||||||
        {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0},
 | 
					        {0.0, 0.0, pow(system_ecef_pos_sd_m, 2.0), 0.0, 0.0, 0.0},
 | 
				
			||||||
@@ -71,6 +70,7 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
 | 
				
			|||||||
 | 
					
 | 
				
			||||||
    initialized = true;
 | 
					    initialized = true;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					    DLOG(INFO) << "Ti: " << Ti;
 | 
				
			||||||
    DLOG(INFO) << "F: " << d_F;
 | 
					    DLOG(INFO) << "F: " << d_F;
 | 
				
			||||||
    DLOG(INFO) << "H: " << d_H;
 | 
					    DLOG(INFO) << "H: " << d_H;
 | 
				
			||||||
    DLOG(INFO) << "R: " << d_R;
 | 
					    DLOG(INFO) << "R: " << d_R;
 | 
				
			||||||
@@ -79,9 +79,10 @@ void Pvt_Kf::init_kf(arma::vec p, arma::vec v,
 | 
				
			|||||||
    DLOG(INFO) << "x: " << d_x_old_old;
 | 
					    DLOG(INFO) << "x: " << d_x_old_old;
 | 
				
			||||||
}
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
void Pvt_Kf::run_Kf(arma::vec p, arma::vec v)
 | 
					
 | 
				
			||||||
 | 
					void Pvt_Kf::run_Kf(const arma::vec& p, const arma::vec& v);
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
    //  Kalman loop
 | 
					    // Kalman loop
 | 
				
			||||||
    // Prediction
 | 
					    // Prediction
 | 
				
			||||||
    d_x_new_old = d_F * d_x_old_old;
 | 
					    d_x_new_old = d_F * d_x_old_old;
 | 
				
			||||||
    d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
 | 
					    d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q;
 | 
				
			||||||
@@ -98,10 +99,6 @@ void Pvt_Kf::run_Kf(arma::vec p, arma::vec v)
 | 
				
			|||||||
    d_P_old_old = d_P_new_new;
 | 
					    d_P_old_old = d_P_new_new;
 | 
				
			||||||
}
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
Pvt_Kf::Pvt_Kf()
 | 
					 | 
				
			||||||
{
 | 
					 | 
				
			||||||
    initialized = false;
 | 
					 | 
				
			||||||
}
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
void Pvt_Kf::get_pvt(arma::vec& p, arma::vec& v)
 | 
					void Pvt_Kf::get_pvt(arma::vec& p, arma::vec& v)
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
 
 | 
				
			|||||||
@@ -1,5 +1,5 @@
 | 
				
			|||||||
/*!
 | 
					/*!
 | 
				
			||||||
 * \file Pvt_Kf.h
 | 
					 * \file pvt_kf.h
 | 
				
			||||||
 * \brief Kalman Filter for Position and Velocity
 | 
					 * \brief Kalman Filter for Position and Velocity
 | 
				
			||||||
 * \author Javier Arribas, 2023. jarribas(at)cttc.es
 | 
					 * \author Javier Arribas, 2023. jarribas(at)cttc.es
 | 
				
			||||||
 *
 | 
					 *
 | 
				
			||||||
@@ -33,18 +33,18 @@
 | 
				
			|||||||
class Pvt_Kf
 | 
					class Pvt_Kf
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
public:
 | 
					public:
 | 
				
			||||||
    Pvt_Kf();
 | 
					    Pvt_Kf() = default;
 | 
				
			||||||
    virtual ~Pvt_Kf() = default;
 | 
					    virtual ~Pvt_Kf() = default;
 | 
				
			||||||
    void init_kf(arma::vec p, arma::vec v,
 | 
					    void init_kf(const arma::vec& p,
 | 
				
			||||||
 | 
					        const arma::vec& v,
 | 
				
			||||||
        double solver_interval_s,
 | 
					        double solver_interval_s,
 | 
				
			||||||
        double measures_ecef_pos_sd_m,
 | 
					        double measures_ecef_pos_sd_m,
 | 
				
			||||||
        double measures_ecef_vel_sd_ms,
 | 
					        double measures_ecef_vel_sd_ms,
 | 
				
			||||||
        double system_ecef_pos_sd_m,
 | 
					        double system_ecef_pos_sd_m,
 | 
				
			||||||
        double system_ecef_vel_sd_ms);
 | 
					        double system_ecef_vel_sd_ms);
 | 
				
			||||||
 | 
					    void run_Kf(const arma::vec& p, const arma::vec& v);
 | 
				
			||||||
    void run_Kf(arma::vec p, arma::vec v);
 | 
					    void get_pvt(arma::vec& p, arma::vec& v);
 | 
				
			||||||
    bool initialized;
 | 
					    bool initialized{false};
 | 
				
			||||||
    void get_pvt(arma::vec &p, arma::vec &v);
 | 
					 | 
				
			||||||
 | 
					
 | 
				
			||||||
private:
 | 
					private:
 | 
				
			||||||
    // Kalman Filter class variables
 | 
					    // Kalman Filter class variables
 | 
				
			||||||
 
 | 
				
			|||||||
@@ -51,11 +51,10 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
 | 
				
			|||||||
    bool flag_dump_to_mat,
 | 
					    bool flag_dump_to_mat,
 | 
				
			||||||
    Pvt_Conf conf) : d_dump_filename(dump_filename),
 | 
					    Pvt_Conf conf) : d_dump_filename(dump_filename),
 | 
				
			||||||
                     d_rtk(rtk),
 | 
					                     d_rtk(rtk),
 | 
				
			||||||
 | 
					                     d_conf(std::move(conf)),
 | 
				
			||||||
                     d_type_of_rx(type_of_rx),
 | 
					                     d_type_of_rx(type_of_rx),
 | 
				
			||||||
                     d_flag_dump_enabled(flag_dump_to_file),
 | 
					                     d_flag_dump_enabled(flag_dump_to_file),
 | 
				
			||||||
                     d_flag_dump_mat_enabled(flag_dump_to_mat),
 | 
					                     d_flag_dump_mat_enabled(flag_dump_to_mat)
 | 
				
			||||||
                     d_conf(conf)
 | 
					 | 
				
			||||||
 | 
					 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
    this->set_averaging_flag(false);
 | 
					    this->set_averaging_flag(false);
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 
 | 
				
			|||||||
@@ -151,11 +151,11 @@ private:
 | 
				
			|||||||
    rtk_t d_rtk{};
 | 
					    rtk_t d_rtk{};
 | 
				
			||||||
    nav_t d_nav_data{};
 | 
					    nav_t d_nav_data{};
 | 
				
			||||||
    Monitor_Pvt d_monitor_pvt{};
 | 
					    Monitor_Pvt d_monitor_pvt{};
 | 
				
			||||||
 | 
					    Pvt_Conf d_conf;
 | 
				
			||||||
 | 
					    Pvt_Kf d_pvt_kf;
 | 
				
			||||||
    uint32_t d_type_of_rx;
 | 
					    uint32_t d_type_of_rx;
 | 
				
			||||||
    bool d_flag_dump_enabled;
 | 
					    bool d_flag_dump_enabled;
 | 
				
			||||||
    bool d_flag_dump_mat_enabled;
 | 
					    bool d_flag_dump_mat_enabled;
 | 
				
			||||||
    Pvt_Conf d_conf;
 | 
					 | 
				
			||||||
    Pvt_Kf d_pvt_kf;
 | 
					 | 
				
			||||||
};
 | 
					};
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 
 | 
				
			|||||||
@@ -1,6 +1,7 @@
 | 
				
			|||||||
/*!
 | 
					/*!
 | 
				
			||||||
 * \file file_timestamp_signal_source.h
 | 
					 * \file file_timestamp_signal_source.cc
 | 
				
			||||||
 * \brief This class reads samples stored in a file and generate stream tags with its timestamp information stored in separated file
 | 
					 * \brief This class reads samples stored in a file and generate stream tags
 | 
				
			||||||
 | 
					 * with its timestamp information stored in separated file
 | 
				
			||||||
 * \author Javier Arribas, jarribas(at)cttc.es
 | 
					 * \author Javier Arribas, jarribas(at)cttc.es
 | 
				
			||||||
 *
 | 
					 *
 | 
				
			||||||
 * -----------------------------------------------------------------------------
 | 
					 * -----------------------------------------------------------------------------
 | 
				
			||||||
@@ -13,6 +14,7 @@
 | 
				
			|||||||
 *
 | 
					 *
 | 
				
			||||||
 * -----------------------------------------------------------------------------
 | 
					 * -----------------------------------------------------------------------------
 | 
				
			||||||
 */
 | 
					 */
 | 
				
			||||||
 | 
					
 | 
				
			||||||
#include "file_timestamp_signal_source.h"
 | 
					#include "file_timestamp_signal_source.h"
 | 
				
			||||||
#include "gnss_sdr_flags.h"
 | 
					#include "gnss_sdr_flags.h"
 | 
				
			||||||
#include "gnss_sdr_string_literals.h"
 | 
					#include "gnss_sdr_string_literals.h"
 | 
				
			||||||
@@ -22,7 +24,9 @@
 | 
				
			|||||||
using namespace std::string_literals;
 | 
					using namespace std::string_literals;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
FileTimestampSignalSource::FileTimestampSignalSource(const ConfigurationInterface* configuration,
 | 
					FileTimestampSignalSource::FileTimestampSignalSource(const ConfigurationInterface* configuration,
 | 
				
			||||||
    const std::string& role, unsigned int in_streams, unsigned int out_streams,
 | 
					    const std::string& role,
 | 
				
			||||||
 | 
					    unsigned int in_streams,
 | 
				
			||||||
 | 
					    unsigned int out_streams,
 | 
				
			||||||
    Concurrent_Queue<pmt::pmt_t>* queue)
 | 
					    Concurrent_Queue<pmt::pmt_t>* queue)
 | 
				
			||||||
    : FileSourceBase(configuration, role, "File_Timestamp_Signal_Source"s, queue, "byte"s),
 | 
					    : FileSourceBase(configuration, role, "File_Timestamp_Signal_Source"s, queue, "byte"s),
 | 
				
			||||||
      timestamp_file_(configuration->property(role + ".timestamp_filename"s, "../data/example_capture_timestamp.dat"s)),
 | 
					      timestamp_file_(configuration->property(role + ".timestamp_filename"s, "../data/example_capture_timestamp.dat"s)),
 | 
				
			||||||
@@ -53,23 +57,23 @@ void FileTimestampSignalSource::create_file_source_hook()
 | 
				
			|||||||
    int source_items_to_samples = 1;
 | 
					    int source_items_to_samples = 1;
 | 
				
			||||||
    bool is_complex = false;
 | 
					    bool is_complex = false;
 | 
				
			||||||
 | 
					
 | 
				
			||||||
    if (item_type().compare("ibyte") == 0)
 | 
					    if (item_type() == "ibyte")
 | 
				
			||||||
        {
 | 
					        {
 | 
				
			||||||
            source_items_to_samples = 1;
 | 
					            source_items_to_samples = 1;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
    else if (item_type().compare("byte") == 0)
 | 
					    else if (item_type() == "byte")
 | 
				
			||||||
        {
 | 
					        {
 | 
				
			||||||
            source_items_to_samples = 1;
 | 
					            source_items_to_samples = 1;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
    else if (item_type().compare("short") == 0)
 | 
					    else if (item_type() == "short")
 | 
				
			||||||
        {
 | 
					        {
 | 
				
			||||||
            source_items_to_samples = 1;
 | 
					            source_items_to_samples = 1;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
    else if (item_type().compare("ishort") == 0)
 | 
					    else if (item_type() == "ishort")
 | 
				
			||||||
        {
 | 
					        {
 | 
				
			||||||
            source_items_to_samples = 1;
 | 
					            source_items_to_samples = 1;
 | 
				
			||||||
        }
 | 
					        }
 | 
				
			||||||
    else if (item_type().compare("gr_complex") == 0)
 | 
					    else if (item_type() == "gr_complex")
 | 
				
			||||||
        {
 | 
					        {
 | 
				
			||||||
            source_items_to_samples = 1;
 | 
					            source_items_to_samples = 1;
 | 
				
			||||||
            is_complex = true;
 | 
					            is_complex = true;
 | 
				
			||||||
@@ -96,12 +100,14 @@ void FileTimestampSignalSource::create_file_source_hook()
 | 
				
			|||||||
    DLOG(INFO) << "timestamp_block_(" << timestamp_block_->unique_id() << ")";
 | 
					    DLOG(INFO) << "timestamp_block_(" << timestamp_block_->unique_id() << ")";
 | 
				
			||||||
}
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
void FileTimestampSignalSource::pre_connect_hook(gr::top_block_sptr top_block)
 | 
					void FileTimestampSignalSource::pre_connect_hook(gr::top_block_sptr top_block)
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
    top_block->connect(file_source(), 0, timestamp_block_, 0);
 | 
					    top_block->connect(file_source(), 0, timestamp_block_, 0);
 | 
				
			||||||
    DLOG(INFO) << "connected file_source to timestamp_block_";
 | 
					    DLOG(INFO) << "connected file_source to timestamp_block_";
 | 
				
			||||||
}
 | 
					}
 | 
				
			||||||
 | 
					
 | 
				
			||||||
 | 
					
 | 
				
			||||||
void FileTimestampSignalSource::pre_disconnect_hook(gr::top_block_sptr top_block)
 | 
					void FileTimestampSignalSource::pre_disconnect_hook(gr::top_block_sptr top_block)
 | 
				
			||||||
{
 | 
					{
 | 
				
			||||||
    top_block->disconnect(file_source(), 0, timestamp_block_, 0);
 | 
					    top_block->disconnect(file_source(), 0, timestamp_block_, 0);
 | 
				
			||||||
 
 | 
				
			|||||||
@@ -1,6 +1,7 @@
 | 
				
			|||||||
/*!
 | 
					/*!
 | 
				
			||||||
 * \file file_timestamp_signal_source.h
 | 
					 * \file file_timestamp_signal_source.h
 | 
				
			||||||
 * \brief This class reads samples stored in a file and generate stream tags with its timestamp information stored in separated file
 | 
					 * \brief This class reads samples stored in a file and generate stream tags
 | 
				
			||||||
 | 
					 * with its timestamp information stored in separated file
 | 
				
			||||||
 * \author Javier Arribas, jarribas(at)cttc.es
 | 
					 * \author Javier Arribas, jarribas(at)cttc.es
 | 
				
			||||||
 *
 | 
					 *
 | 
				
			||||||
 * -----------------------------------------------------------------------------
 | 
					 * -----------------------------------------------------------------------------
 | 
				
			||||||
 
 | 
				
			|||||||
		Reference in New Issue
	
	Block a user