MOD: remove static variables

src/algorithms/PVT/libs/rtklib_solver.cc

@@ -2091,11 +2091,11 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         // User clock offset [s]
                                         //tmp_double = rx_position_and_time[3];
-                                        tmp_double = vtl_data.get_user_clock_offset_s();
+                                        tmp_double = vtl_engine.get_user_clock_offset_s();
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         
                                         // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double)
-                                        std::vector<double> p_vec_m = vtl_data.get_position_ecef_m();
+                                        std::vector<double> p_vec_m = vtl_engine.get_position_ecef_m();
                                         tmp_double = p_vec_m[0];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         tmp_double = p_vec_m[1];
@@ -2103,7 +2103,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                         tmp_double = p_vec_m[2];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         
-                                        std::vector<double>  v_vec_m = vtl_data.get_velocity_ecef_m_s();
+                                        std::vector<double>  v_vec_m = vtl_engine.get_velocity_ecef_m_s();
                                         tmp_double = v_vec_m[0];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         tmp_double = v_vec_m[1];
@@ -2111,7 +2111,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                         tmp_double = v_vec_m[2];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
 
-                                        std::vector<double>  a_vec_m = vtl_data.get_accel_ecef_m_s2();
+                                        std::vector<double>  a_vec_m = vtl_engine.get_accel_ecef_m_s2();
                                         tmp_double = a_vec_m[0];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         tmp_double = a_vec_m[1];
@@ -2121,7 +2121,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
 
                                         // position/velocity/acceleration variance/ (units^2)  (9 x double)
 
-                                        std::vector<double>  p_var_vec_m = vtl_data.get_position_var_ecef_m();
+                                        std::vector<double>  p_var_vec_m = vtl_engine.get_position_var_ecef_m();
                                         tmp_double = p_var_vec_m[0];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         tmp_double = p_var_vec_m[1];
@@ -2130,7 +2130,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
 
 
-                                        std::vector<double>  v_var_vec_m = vtl_data.get_velocity_var_ecef_m_s();
+                                        std::vector<double>  v_var_vec_m = vtl_engine.get_velocity_var_ecef_m_s();
                                         tmp_double = v_var_vec_m[0];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         tmp_double = v_var_vec_m[1];
@@ -2138,7 +2138,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
                                         tmp_double = v_var_vec_m[2];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
 
-                                        vector<double>  a_var_vec_m = vtl_data.get_accel_var_ecef_m_s2();
+                                        vector<double>  a_var_vec_m = vtl_engine.get_accel_var_ecef_m_s2();
                                         tmp_double = a_var_vec_m[0];
                                         d_vtl_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
                                         tmp_double = a_var_vec_m[1];

src/algorithms/PVT/libs/vtl_data.cc

@@ -23,8 +23,6 @@ Vtl_Data::Vtl_Data()
 {
     epoch_tow_s = 0;
     sample_counter = 0;
-    kf_state = arma::mat(11,1);
-    kf_P = arma::mat(11,11);
 }
 
 void Vtl_Data::init_storage(int n_sats)
@@ -69,88 +67,4 @@ void Vtl_Data::debug_print()
     // carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
 }
 
-std::vector<double> Vtl_Data::get_position_ecef_m()
-{
-    std::vector<double> temp = {42,42,42};
-    temp[0] = kf_state[0];
-    temp[1] = kf_state[1];
-    temp[2] = kf_state[2];
-
-    return temp;
-}
-
-std::vector<double> Vtl_Data::get_velocity_ecef_m_s()
-{
-    std::vector<double> temp = {42,42,42};
-    temp[0] = kf_state[3];
-    temp[1] = kf_state[4];
-    temp[2] = kf_state[5];
-
-    return temp;
-}
-
-std::vector<double> Vtl_Data::get_accel_ecef_m_s2()
-{
-    std::vector<double> temp = {42,42,42};
-    temp[0] = kf_state[6];
-    temp[1] = kf_state[7];
-    temp[2] = kf_state[8];
-
-    return temp;
-}
-std::vector<double> Vtl_Data::get_position_var_ecef_m()
-{
-    std::vector<double> temp = {42,42,42};
-    temp[0] = kf_P(0,0);
-    temp[1] = kf_P(1,1);
-    temp[2] = kf_P(2,2);
-
-    return temp;
-}
-
-std::vector<double> Vtl_Data::get_velocity_var_ecef_m_s()
-{
-    std::vector<double> temp = {42,42,42};
-    temp[0] = kf_P(3,3);
-    temp[1] = kf_P(4,4);
-    temp[2] = kf_P(5,5);
-
-    return temp;
-}
-
-std::vector<double> Vtl_Data::get_accel_var_ecef_m_s2()
-{
-    std::vector<double> temp = {42,42,42};
-    temp[0] = kf_P(6,6);
-    temp[1] = kf_P(7,7);
-    temp[2] = kf_P(8,8);
-
-    return temp;
-}
-
-double Vtl_Data::get_latitude()
-{
-
-    return -1.0;
-}
-
-double Vtl_Data::get_longitude()
-{
-    
-
-    return -1.0;
-}
-
-double Vtl_Data::get_height()
-{
-    return -1.0;
-}
-
-double Vtl_Data::get_user_clock_offset_s()
-{
-    double temp=0;
-    temp=kf_state[9];
-
-    return temp;
-}
 

src/algorithms/PVT/libs/vtl_data.h

@@ -62,16 +62,6 @@ public:
     double epoch_tow_s;       // current observation RX time [s]
     uint64_t sample_counter;  // current sample counter associated with RX time [samples from start]
     void debug_print();
-    std::vector<double> get_position_ecef_m();   // get_position_ecef_m
-    std::vector<double> get_velocity_ecef_m_s(); // get_velocity_ecef_m_s
-    std::vector<double> get_accel_ecef_m_s2(); // get_accel_ecef_m_s2
-    std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m
-    std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s
-    std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2
-    double get_latitude(); // get_latitude
-    double get_longitude(); // get_longitude
-    double get_height(); // get_height
-    double get_user_clock_offset_s(); // get_user_clock_offset_s;
 };
 
 

src/algorithms/PVT/libs/vtl_engine.cc

@@ -22,25 +22,31 @@ using namespace std;
 
 Vtl_Engine::Vtl_Engine()
 {
+    counter=0;
+    refSampleCounter=0;
+    n_of_states=11;
+    kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0;  //TODO: use a real value.; 
+    kf_x = arma::zeros(n_of_states, 1);
 }
 
 Vtl_Engine::~Vtl_Engine()
 {
 }
 
-bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
+bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
 {
     //TODO: Implement main VTL loop here
     using arma::as_scalar;
     
-    static uint64_t refSampleCounter = new_data.sample_counter;
+    if (refSampleCounter=0)
+    {
+        refSampleCounter=new_data.sample_counter;
+    } 
     double delta_t_vtl = (new_data.sample_counter - refSampleCounter) / 5000000.0; 
     refSampleCounter = new_data.sample_counter;
     // ################## Kalman filter initialization ######################################
     //State variables
-    int n_of_states=11;
-    static arma::mat kf_P_x = arma::eye(n_of_states, n_of_states) * 1.0;  //TODO: use a real value.; 
-    static arma::mat kf_x = arma::zeros(n_of_states, 1);
+
     arma::mat kf_dx = arma::zeros(n_of_states, 1);
     // covariances (static)
       
@@ -58,8 +64,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
     kf_S = arma::zeros(2 * new_data.sat_number, 2 * new_data.sat_number);  // kf_P_y innovation covariance matrix
     kf_K = arma::zeros(n_of_states, 2 * new_data.sat_number); ;
     // ################## Kalman Tracking ######################################
-    static uint32_t counter = 0;  //counter
-    counter = counter + 1;        //uint64_t
+    counter++;        //uint64_t
     //new_data.kf_state.print("new_data kf initial");
     uint32_t closure_point=3;
     
@@ -339,3 +344,88 @@ bool Vtl_Engine::kf_measurements(arma::mat &kf_yerr, int sat_number, arma::mat r
     }
     return -1;
 }
+
+std::vector<double> Vtl_Engine::get_position_ecef_m()
+{
+    std::vector<double> temp = {42,42,42};
+    temp[0] = kf_x[0];
+    temp[1] = kf_x[1];
+    temp[2] = kf_x[2];
+
+    return temp;
+}
+
+std::vector<double> Vtl_Engine::get_velocity_ecef_m_s()
+{
+    std::vector<double> temp = {42,42,42};
+    temp[0] = kf_x[3];
+    temp[1] = kf_x[4];
+    temp[2] = kf_x[5];
+
+    return temp;
+}
+
+std::vector<double> Vtl_Engine::get_accel_ecef_m_s2()
+{
+    std::vector<double> temp = {42,42,42};
+    temp[0] = kf_x[6];
+    temp[1] = kf_x[7];
+    temp[2] = kf_x[8];
+
+    return temp;
+}
+std::vector<double> Vtl_Engine::get_position_var_ecef_m()
+{
+    std::vector<double> temp = {42,42,42};
+    temp[0] = kf_P_x(0,0);
+    temp[1] = kf_P_x(1,1);
+    temp[2] = kf_P_x(2,2);
+
+    return temp;
+}
+
+std::vector<double> Vtl_Engine::get_velocity_var_ecef_m_s()
+{
+    std::vector<double> temp = {42,42,42};
+    temp[0] = kf_P_x(3,3);
+    temp[1] = kf_P_x(4,4);
+    temp[2] = kf_P_x(5,5);
+
+    return temp;
+}
+
+std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2()
+{
+    std::vector<double> temp = {42,42,42};
+    temp[0] = kf_P_x(6,6);
+    temp[1] = kf_P_x(7,7);
+    temp[2] = kf_P_x(8,8);
+
+    return temp;
+}
+
+double Vtl_Engine::get_latitude()
+{
+
+    return -1.0;
+}
+
+double Vtl_Engine::get_longitude()
+{
+    
+
+    return -1.0;
+}
+
+double Vtl_Engine::get_height()
+{
+    return -1.0;
+}
+
+double Vtl_Engine::get_user_clock_offset_s()
+{
+    double temp=0;
+    temp = kf_x[9];
+
+    return temp;
+}

src/algorithms/PVT/libs/vtl_engine.h

@@ -42,11 +42,21 @@ public:
     void configure(Vtl_Conf config_);  //set config parameters
 
     //TODO: output functions here (output for tracking KF updates, VTL computed user PVT, etc...)
-    bool vtl_loop(Vtl_Data& new_data);
+    bool vtl_loop(Vtl_Data new_data);
     void reset();        // reset all internal states
     void debug_print();  // print debug information
 
     std::vector<TrackingCmd> trk_cmd_outs;  // vector holding the Tracking command states updates to be sent to tracking KFs
+    std::vector<double> get_position_ecef_m();   // get_position_ecef_m
+    std::vector<double> get_velocity_ecef_m_s(); // get_velocity_ecef_m_s
+    std::vector<double> get_accel_ecef_m_s2(); // get_accel_ecef_m_s2
+    std::vector<double> get_position_var_ecef_m(); // get_position_var_ecef_m
+    std::vector<double> get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s
+    std::vector<double> get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2
+    double get_latitude(); // get_latitude
+    double get_longitude(); // get_longitude
+    double get_height(); // get_height
+    double get_user_clock_offset_s(); // get_user_clock_offset_s;    
 
 private:
     Vtl_Conf config;
@@ -67,6 +77,7 @@ private:
     arma::mat kf_P_x_ini;  // initial state error covariance matrix
     // arma::mat kf_P_x;      // state error covariance matrix
     arma::mat kf_P_x_pre;  // Predicted state error covariance matrix
+    arma::mat kf_P_x;
     arma::mat kf_S;      // innovation covariance matrix
 
     arma::mat kf_F;  // state transition matrix
@@ -84,6 +95,11 @@ private:
     // Gaussian estimator
     arma::mat kf_R_est;  // measurement error covariance
 
+
+    uint32_t counter;
+    int n_of_states;
+    uint64_t refSampleCounter;
+
     bool kf_H_fill(arma::mat &kf_H, int sat_number, arma::colvec ax, arma::colvec ay, arma::colvec az, double kf_dt); // Observation Matrix constructor
     bool kf_F_fill(arma::mat &kf_F,double kf_dt); // System Matrix constructor
     bool obsv_calc(arma::mat &rho_pri,arma::mat &rhoDot_pri,arma::colvec &ax, arma::colvec &ay, arma::colvec &az,int sat_number,arma::mat sat_p,arma::mat sat_v,arma::mat kf_x); // Observables calculation