mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-15 20:50:33 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
372ce62597
@ -283,9 +283,14 @@ void pcps_acquisition::init()
|
||||
{
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
||||
d_magnitude_grid[doppler_index] = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
|
||||
for (unsigned k = 0; k < d_fft_size; k++)
|
||||
{
|
||||
d_magnitude_grid[doppler_index][k] = 0.0;
|
||||
}
|
||||
int doppler = -static_cast<int>(acq_parameters.doppler_max) + d_doppler_step * doppler_index;
|
||||
update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_old_freq + doppler);
|
||||
}
|
||||
|
||||
d_worker_active = false;
|
||||
|
||||
if (acq_parameters.dump)
|
||||
@ -782,6 +787,14 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count)
|
||||
}
|
||||
d_num_noncoherent_integrations_counter = 0;
|
||||
d_positive_acq = 0;
|
||||
// Reset grid
|
||||
for (unsigned int i = 0; i < d_num_doppler_bins; i++)
|
||||
{
|
||||
for (unsigned k = 0; k < d_fft_size; k++)
|
||||
{
|
||||
d_magnitude_grid[i][k] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -238,7 +238,6 @@ bool gps_l1_ca_telemetry_decoder_cc::decode_subframe()
|
||||
{
|
||||
//std::cout << "word invalid sat " << this->d_satellite << std::endl;
|
||||
CRC_ok = false;
|
||||
//break;
|
||||
}
|
||||
//add word to subframe
|
||||
// insert the word in the correct position of the subframe
|
||||
|
||||
@ -32,7 +32,7 @@
|
||||
#define GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_CC_H
|
||||
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "gps_l1_ca_subframe_fsm.h"
|
||||
#include "gps_navigation_message.h"
|
||||
#include "gnss_satellite.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include <gnuradio/block.h>
|
||||
@ -84,17 +84,11 @@ private:
|
||||
boost::circular_buffer<Gnss_Synchro> d_symbol_history;
|
||||
float d_subframe_symbols[GPS_SUBFRAME_MS]; //symbols per subframe
|
||||
int d_current_subframe_symbol;
|
||||
//double d_symbol_accumulator;
|
||||
//short int d_symbol_accumulator_counter;
|
||||
|
||||
//bits and frame
|
||||
//unsigned short int d_frame_bit_index;
|
||||
//unsigned int d_GPS_frame_4bytes;
|
||||
unsigned int d_prev_GPS_frame_4bytes;
|
||||
//bool d_flag_parity;
|
||||
bool d_flag_preamble;
|
||||
bool d_flag_new_tow_available;
|
||||
//int d_word_number;
|
||||
|
||||
// navigation message vars
|
||||
Gps_Navigation_Message d_nav;
|
||||
|
||||
@ -19,7 +19,6 @@
|
||||
add_subdirectory(libswiftcnav)
|
||||
|
||||
set(TELEMETRY_DECODER_LIB_SOURCES
|
||||
gps_l1_ca_subframe_fsm.cc
|
||||
viterbi_decoder.cc
|
||||
)
|
||||
|
||||
|
||||
@ -1,289 +0,0 @@
|
||||
/*!
|
||||
* \file gps_l1_ca_subframe_fsm.cc
|
||||
* \brief Implementation of a GPS NAV message word-to-subframe decoder state machine
|
||||
* \author Javier Arribas, 2011. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "gps_l1_ca_subframe_fsm.h"
|
||||
#include "gnss_satellite.h"
|
||||
#include <boost/statechart/simple_state.hpp>
|
||||
#include <boost/statechart/state.hpp>
|
||||
#include <boost/statechart/transition.hpp>
|
||||
#include <boost/statechart/custom_reaction.hpp>
|
||||
#include <boost/mpl/list.hpp>
|
||||
#include <string>
|
||||
|
||||
|
||||
//************ GPS WORD TO SUBFRAME DECODER STATE MACHINE **********
|
||||
struct Ev_gps_word_valid : sc::event<Ev_gps_word_valid>
|
||||
{
|
||||
};
|
||||
|
||||
|
||||
struct Ev_gps_word_invalid : sc::event<Ev_gps_word_invalid>
|
||||
{
|
||||
};
|
||||
|
||||
|
||||
struct Ev_gps_word_preamble : sc::event<Ev_gps_word_preamble>
|
||||
{
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S0 : public sc::state<gps_subframe_fsm_S0, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
// sc::transition(event,next_status)
|
||||
typedef sc::transition<Ev_gps_word_preamble, gps_subframe_fsm_S1> reactions;
|
||||
gps_subframe_fsm_S0(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S0 "<<std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S1 : public sc::state<gps_subframe_fsm_S1, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S2> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S1(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S1 "<<std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S2 : public sc::state<gps_subframe_fsm_S2, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S3> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S2(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S2 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(0);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S3 : public sc::state<gps_subframe_fsm_S3, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S4> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S3(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S3 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(1);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S4 : public sc::state<gps_subframe_fsm_S4, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S5> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S4(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S4 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(2);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S5 : public sc::state<gps_subframe_fsm_S5, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S6> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S5(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S5 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(3);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S6 : public sc::state<gps_subframe_fsm_S6, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S7> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S6(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S6 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(4);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S7 : public sc::state<gps_subframe_fsm_S7, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S8> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S7(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S7 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(5);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S8 : public sc::state<gps_subframe_fsm_S8, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S9> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S8(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S8 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(6);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S9 : public sc::state<gps_subframe_fsm_S9, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S10> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S9(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S9 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(7);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S10 : public sc::state<gps_subframe_fsm_S10, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
|
||||
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S11> >
|
||||
reactions;
|
||||
|
||||
gps_subframe_fsm_S10(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Enter S10 "<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(8);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct gps_subframe_fsm_S11 : public sc::state<gps_subframe_fsm_S11, GpsL1CaSubframeFsm>
|
||||
{
|
||||
public:
|
||||
typedef sc::transition<Ev_gps_word_preamble, gps_subframe_fsm_S1> reactions;
|
||||
|
||||
gps_subframe_fsm_S11(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout<<"Completed GPS Subframe!"<<std::endl;
|
||||
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(9);
|
||||
context<GpsL1CaSubframeFsm>().gps_subframe_to_nav_msg(); //decode the subframe
|
||||
// DECODE SUBFRAME
|
||||
//std::cout<<"Enter S11"<<std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
GpsL1CaSubframeFsm::GpsL1CaSubframeFsm()
|
||||
{
|
||||
d_nav.reset();
|
||||
i_channel_ID = 0;
|
||||
i_satellite_PRN = 0;
|
||||
d_subframe_ID = 0;
|
||||
d_flag_new_subframe = false;
|
||||
initiate(); //start the FSM
|
||||
}
|
||||
|
||||
|
||||
void GpsL1CaSubframeFsm::gps_word_to_subframe(int position)
|
||||
{
|
||||
// insert the word in the correct position of the subframe
|
||||
std::memcpy(&d_subframe[position * GPS_WORD_LENGTH], &d_GPS_frame_4bytes, sizeof(char) * GPS_WORD_LENGTH);
|
||||
}
|
||||
|
||||
|
||||
void GpsL1CaSubframeFsm::clear_flag_new_subframe()
|
||||
{
|
||||
d_flag_new_subframe = false;
|
||||
}
|
||||
|
||||
|
||||
void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
|
||||
{
|
||||
//int subframe_ID;
|
||||
// NEW GPS SUBFRAME HAS ARRIVED!
|
||||
d_subframe_ID = d_nav.subframe_decoder(this->d_subframe); //decode the subframe
|
||||
std::cout << "New GPS NAV message received in channel " << i_channel_ID << ": "
|
||||
<< "subframe "
|
||||
<< d_subframe_ID << " from satellite "
|
||||
<< Gnss_Satellite(std::string("GPS"), i_satellite_PRN) << std::endl;
|
||||
d_nav.i_satellite_PRN = i_satellite_PRN;
|
||||
d_nav.i_channel_ID = i_channel_ID;
|
||||
|
||||
d_flag_new_subframe = true;
|
||||
}
|
||||
|
||||
|
||||
void GpsL1CaSubframeFsm::Event_gps_word_valid()
|
||||
{
|
||||
this->process_event(Ev_gps_word_valid());
|
||||
}
|
||||
|
||||
|
||||
void GpsL1CaSubframeFsm::Event_gps_word_invalid()
|
||||
{
|
||||
this->process_event(Ev_gps_word_invalid());
|
||||
}
|
||||
|
||||
|
||||
void GpsL1CaSubframeFsm::Event_gps_word_preamble()
|
||||
{
|
||||
this->process_event(Ev_gps_word_preamble());
|
||||
}
|
||||
@ -1,100 +0,0 @@
|
||||
/*!
|
||||
* \file gps_l1_ca_subframe_fsm.h
|
||||
* \brief Interface of a GPS NAV message word-to-subframe decoder state machine
|
||||
* \author Javier Arribas, 2011. jarribas(at)cttc.es
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_GPS_L1_CA_SUBFRAME_FSM_H_
|
||||
#define GNSS_SDR_GPS_L1_CA_SUBFRAME_FSM_H_
|
||||
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "gps_navigation_message.h"
|
||||
#include "gps_ephemeris.h"
|
||||
#include "gps_iono.h"
|
||||
#include "gps_almanac.h"
|
||||
#include "gps_utc_model.h"
|
||||
#include <boost/statechart/state_machine.hpp>
|
||||
|
||||
namespace sc = boost::statechart;
|
||||
namespace mpl = boost::mpl;
|
||||
|
||||
struct gps_subframe_fsm_S0;
|
||||
struct gps_subframe_fsm_S1;
|
||||
struct gps_subframe_fsm_S2;
|
||||
struct gps_subframe_fsm_S3;
|
||||
struct gps_subframe_fsm_S4;
|
||||
struct gps_subframe_fsm_S5;
|
||||
struct gps_subframe_fsm_S6;
|
||||
struct gps_subframe_fsm_S7;
|
||||
struct gps_subframe_fsm_S8;
|
||||
struct gps_subframe_fsm_S9;
|
||||
struct gps_subframe_fsm_S10;
|
||||
struct gps_subframe_fsm_S11;
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class implements a Finite State Machine that handles the decoding
|
||||
* of the GPS L1 C/A NAV message
|
||||
*/
|
||||
class GpsL1CaSubframeFsm : public sc::state_machine<GpsL1CaSubframeFsm, gps_subframe_fsm_S0>
|
||||
{
|
||||
public:
|
||||
GpsL1CaSubframeFsm(); //!< The constructor starts the Finite State Machine
|
||||
void clear_flag_new_subframe();
|
||||
// channel and satellite info
|
||||
int i_channel_ID; //!< Channel id
|
||||
unsigned int i_satellite_PRN; //!< Satellite PRN number
|
||||
|
||||
Gps_Navigation_Message d_nav; //!< GPS L1 C/A navigation message object
|
||||
|
||||
// GPS SV and System parameters
|
||||
Gps_Ephemeris ephemeris; //!< Object that handles GPS ephemeris parameters
|
||||
Gps_Almanac almanac; //!< Object that handles GPS almanac data
|
||||
Gps_Utc_Model utc_model; //!< Object that handles UTM model parameters
|
||||
Gps_Iono iono; //!< Object that handles ionospheric parameters
|
||||
|
||||
char d_subframe[GPS_SUBFRAME_LENGTH];
|
||||
int d_subframe_ID;
|
||||
bool d_flag_new_subframe;
|
||||
char d_GPS_frame_4bytes[GPS_WORD_LENGTH];
|
||||
//double d_preamble_time_ms;
|
||||
|
||||
void gps_word_to_subframe(int position); //!< inserts the word in the correct position of the subframe
|
||||
|
||||
/*!
|
||||
* \brief This function decodes a NAv message subframe and pushes the information to the right queues
|
||||
*/
|
||||
void gps_subframe_to_nav_msg();
|
||||
|
||||
//FSM EVENTS
|
||||
void Event_gps_word_valid(); //!< FSM event: the received word is valid
|
||||
void Event_gps_word_invalid(); //!< FSM event: the received word is not valid
|
||||
void Event_gps_word_preamble(); //!< FSM event: word preamble detected
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -399,7 +399,8 @@ void dll_pll_veml_tracking::start_tracking()
|
||||
double T_prn_mod_seconds = T_chip_mod_seconds * static_cast<double>(d_code_length_chips);
|
||||
double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters.fs_in;
|
||||
|
||||
d_current_prn_length_samples = std::round(T_prn_mod_samples);
|
||||
//d_current_prn_length_samples = std::round(T_prn_mod_samples);
|
||||
d_current_prn_length_samples = std::floor(T_prn_mod_samples);
|
||||
|
||||
double T_prn_true_seconds = static_cast<double>(d_code_length_chips) / d_code_chip_rate;
|
||||
double T_prn_true_samples = T_prn_true_seconds * trk_parameters.fs_in;
|
||||
@ -753,7 +754,8 @@ void dll_pll_veml_tracking::update_tracking_vars()
|
||||
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
|
||||
T_prn_samples = T_prn_seconds * trk_parameters.fs_in;
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * trk_parameters.fs_in;
|
||||
d_current_prn_length_samples = static_cast<int>(round(K_blk_samples)); // round to a discrete number of samples
|
||||
//d_current_prn_length_samples = static_cast<int>(round(K_blk_samples)); // round to a discrete number of samples
|
||||
d_current_prn_length_samples = static_cast<int>(std::floor(K_blk_samples)); // round to a discrete number of samples
|
||||
|
||||
//################### PLL COMMANDS #################################################
|
||||
// carrier phase step (NCO phase increment per sample) [rads/sample]
|
||||
@ -834,6 +836,7 @@ void dll_pll_veml_tracking::log_data(bool integrating)
|
||||
float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL;
|
||||
float tmp_float;
|
||||
double tmp_double;
|
||||
unsigned long int tmp_long_int;
|
||||
if (trk_parameters.track_pilot)
|
||||
{
|
||||
if (interchange_iq)
|
||||
@ -900,7 +903,8 @@ void dll_pll_veml_tracking::log_data(bool integrating)
|
||||
d_dump_file.write(reinterpret_cast<char *>(&prompt_I), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char *>(&prompt_Q), sizeof(float));
|
||||
// PRN start sample stamp
|
||||
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(unsigned long int));
|
||||
tmp_long_int = d_sample_counter + d_current_prn_length_samples;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_long_int), sizeof(unsigned long int));
|
||||
// accumulated carrier phase
|
||||
tmp_float = d_acc_carrier_phase_rad;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||
|
||||
@ -418,7 +418,7 @@ void GNSSFlowgraph::connect()
|
||||
}
|
||||
if (sat == 0)
|
||||
{
|
||||
//channels_.at(i)->set_signal(search_next_signal(gnss_signal, false));
|
||||
channels_.at(i)->set_signal(search_next_signal(gnss_signal, false));
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -896,6 +896,43 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
|
||||
case 1:
|
||||
LOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_[who]->get_signal().get_satellite();
|
||||
|
||||
// If the satellite is in the list of available ones, remove it.
|
||||
switch (mapStringValues_[channels_[who]->get_signal().get_signal_str()])
|
||||
{
|
||||
case evGPS_1C:
|
||||
available_GPS_1C_signals_.remove(channels_[who]->get_signal());
|
||||
break;
|
||||
|
||||
case evGPS_2S:
|
||||
available_GPS_2S_signals_.remove(channels_[who]->get_signal());
|
||||
break;
|
||||
|
||||
case evGPS_L5:
|
||||
available_GPS_L5_signals_.remove(channels_[who]->get_signal());
|
||||
break;
|
||||
|
||||
case evGAL_1B:
|
||||
available_GAL_1B_signals_.remove(channels_[who]->get_signal());
|
||||
break;
|
||||
|
||||
case evGAL_5X:
|
||||
available_GAL_5X_signals_.remove(channels_[who]->get_signal());
|
||||
break;
|
||||
|
||||
case evGLO_1G:
|
||||
available_GLO_1G_signals_.remove(channels_[who]->get_signal());
|
||||
break;
|
||||
|
||||
case evGLO_2G:
|
||||
available_GLO_2G_signals_.remove(channels_[who]->get_signal());
|
||||
break;
|
||||
|
||||
default:
|
||||
LOG(ERROR) << "This should not happen :-(";
|
||||
break;
|
||||
}
|
||||
|
||||
channels_state_[who] = 2;
|
||||
acq_channels_count_--;
|
||||
for (unsigned int i = 0; i < channels_count_; i++)
|
||||
|
||||
@ -65,6 +65,8 @@ DEFINE_int64(skip_samples, 0, "Skip an initial transitory in the processed signa
|
||||
|
||||
DEFINE_int32(plot_detail_level, 0, "Specify the desired plot detail (0,1,2): 0 - Minimum plots (default) 2 - Plot all tracking parameters");
|
||||
|
||||
DEFINE_double(skip_trk_transitory_s, 1.0, "Skip the initial tracking output signal to avoid transitory results [s]");
|
||||
|
||||
//Emulated acquisition configuration
|
||||
|
||||
//Tracking configuration
|
||||
|
||||
@ -34,6 +34,7 @@
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
#include <armadillo>
|
||||
#include <matio.h>
|
||||
#include <boost/filesystem.hpp>
|
||||
#include <gnuradio/top_block.h>
|
||||
#include <gnuradio/blocks/file_source.h>
|
||||
@ -136,20 +137,24 @@ public:
|
||||
arma::vec& meas_time_s,
|
||||
arma::vec& meas_value,
|
||||
double& mean_error,
|
||||
double& std_dev_error);
|
||||
double& std_dev_error,
|
||||
double& rmse);
|
||||
std::vector<double> check_results_acc_carrier_phase(arma::vec& true_time_s,
|
||||
arma::vec& true_value,
|
||||
arma::vec& meas_time_s,
|
||||
arma::vec& meas_value,
|
||||
double& mean_error,
|
||||
double& std_dev_error);
|
||||
double& std_dev_error,
|
||||
double& rmse);
|
||||
std::vector<double> check_results_codephase(arma::vec& true_time_s,
|
||||
arma::vec& true_value,
|
||||
arma::vec& meas_time_s,
|
||||
arma::vec& meas_value,
|
||||
double& mean_error,
|
||||
double& std_dev_error);
|
||||
double& std_dev_error,
|
||||
double& rmse);
|
||||
|
||||
bool save_mat_xy(std::vector<double>& x, std::vector<double>& y, std::string filename);
|
||||
GpsL1CADllPllTrackingTest()
|
||||
{
|
||||
factory = std::make_shared<GNSSBlockFactory>();
|
||||
@ -267,7 +272,8 @@ std::vector<double> GpsL1CADllPllTrackingTest::check_results_doppler(arma::vec&
|
||||
arma::vec& meas_time_s,
|
||||
arma::vec& meas_value,
|
||||
double& mean_error,
|
||||
double& std_dev_error)
|
||||
double& std_dev_error,
|
||||
double& rmse)
|
||||
{
|
||||
// 1. True value interpolation to match the measurement times
|
||||
arma::vec true_value_interp;
|
||||
@ -289,7 +295,7 @@ std::vector<double> GpsL1CADllPllTrackingTest::check_results_doppler(arma::vec&
|
||||
std::vector<double> err_std_vector(err.colptr(0), err.colptr(0) + err.n_rows);
|
||||
|
||||
arma::vec err2 = arma::square(err);
|
||||
double rmse = sqrt(arma::mean(err2));
|
||||
rmse = sqrt(arma::mean(err2));
|
||||
|
||||
// 3. Mean err and variance
|
||||
double error_mean = arma::mean(err);
|
||||
@ -317,7 +323,8 @@ std::vector<double> GpsL1CADllPllTrackingTest::check_results_acc_carrier_phase(a
|
||||
arma::vec& meas_time_s,
|
||||
arma::vec& meas_value,
|
||||
double& mean_error,
|
||||
double& std_dev_error)
|
||||
double& std_dev_error,
|
||||
double& rmse)
|
||||
{
|
||||
// 1. True value interpolation to match the measurement times
|
||||
arma::vec true_value_interp;
|
||||
@ -337,7 +344,7 @@ std::vector<double> GpsL1CADllPllTrackingTest::check_results_acc_carrier_phase(a
|
||||
//conversion between arma::vec and std:vector
|
||||
std::vector<double> err_std_vector(err.colptr(0), err.colptr(0) + err.n_rows);
|
||||
|
||||
double rmse = sqrt(arma::mean(err2));
|
||||
rmse = sqrt(arma::mean(err2));
|
||||
|
||||
// 3. Mean err and variance
|
||||
double error_mean = arma::mean(err);
|
||||
@ -364,7 +371,8 @@ std::vector<double> GpsL1CADllPllTrackingTest::check_results_codephase(arma::vec
|
||||
arma::vec& meas_time_s,
|
||||
arma::vec& meas_value,
|
||||
double& mean_error,
|
||||
double& std_dev_error)
|
||||
double& std_dev_error,
|
||||
double& rmse)
|
||||
{
|
||||
// 1. True value interpolation to match the measurement times
|
||||
arma::vec true_value_interp;
|
||||
@ -385,7 +393,7 @@ std::vector<double> GpsL1CADllPllTrackingTest::check_results_codephase(arma::vec
|
||||
std::vector<double> err_std_vector(err.colptr(0), err.colptr(0) + err.n_rows);
|
||||
|
||||
arma::vec err2 = arma::square(err);
|
||||
double rmse = sqrt(arma::mean(err2));
|
||||
rmse = sqrt(arma::mean(err2));
|
||||
|
||||
// 3. Mean err and variance
|
||||
double error_mean = arma::mean(err);
|
||||
@ -420,12 +428,15 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
//data containers for config param sweep
|
||||
std::vector<std::vector<double>> mean_doppler_error_sweep; //swep config param and cn0 sweep
|
||||
std::vector<std::vector<double>> std_dev_doppler_error_sweep; //swep config param and cn0 sweep
|
||||
std::vector<std::vector<double>> rmse_doppler_sweep; //swep config param and cn0 sweep
|
||||
|
||||
std::vector<std::vector<double>> mean_code_phase_error_sweep; //swep config param and cn0 sweep
|
||||
std::vector<std::vector<double>> std_dev_code_phase_error_sweep; //swep config param and cn0 sweep
|
||||
std::vector<std::vector<double>> rmse_code_phase_sweep; //swep config param and cn0 sweep
|
||||
|
||||
std::vector<std::vector<double>> mean_carrier_phase_error_sweep; //swep config param and cn0 sweep
|
||||
std::vector<std::vector<double>> std_dev_carrier_phase_error_sweep; //swep config param and cn0 sweep
|
||||
std::vector<std::vector<double>> rmse_carrier_phase_sweep; //swep config param and cn0 sweep
|
||||
|
||||
std::vector<std::vector<double>> trk_valid_timestamp_s_sweep;
|
||||
std::vector<std::vector<double>> generator_CN0_values_sweep_copy;
|
||||
@ -532,10 +543,13 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
|
||||
std::vector<double> mean_doppler_error;
|
||||
std::vector<double> std_dev_doppler_error;
|
||||
std::vector<double> rmse_doppler;
|
||||
std::vector<double> mean_code_phase_error;
|
||||
std::vector<double> std_dev_code_phase_error;
|
||||
std::vector<double> rmse_code_phase;
|
||||
std::vector<double> mean_carrier_phase_error;
|
||||
std::vector<double> std_dev_carrier_phase_error;
|
||||
std::vector<double> rmse_carrier_phase;
|
||||
std::vector<double> valid_CN0_values;
|
||||
|
||||
configure_receiver(PLL_wide_bw_values.at(config_idx),
|
||||
@ -707,7 +721,7 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
epoch_counter++;
|
||||
}
|
||||
// Align initial measurements and cut the tracking pull-in transitory
|
||||
double pull_in_offset_s = 1.0;
|
||||
double pull_in_offset_s = FLAGS_skip_trk_transitory_s;
|
||||
|
||||
arma::uvec initial_meas_point = arma::find(trk_timestamp_s >= (true_timestamp_s(0) + pull_in_offset_s), 1, "first");
|
||||
if (initial_meas_point.size() > 0 and tracking_last_msg != 3)
|
||||
@ -720,20 +734,24 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
|
||||
double mean_error;
|
||||
double std_dev_error;
|
||||
double rmse;
|
||||
|
||||
valid_CN0_values.push_back(generator_CN0_values.at(current_cn0_idx)); //save the current cn0 value (valid tracking)
|
||||
|
||||
doppler_error_hz = check_results_doppler(true_timestamp_s, true_Doppler_Hz, trk_timestamp_s, trk_Doppler_Hz, mean_error, std_dev_error);
|
||||
doppler_error_hz = check_results_doppler(true_timestamp_s, true_Doppler_Hz, trk_timestamp_s, trk_Doppler_Hz, mean_error, std_dev_error, rmse);
|
||||
mean_doppler_error.push_back(mean_error);
|
||||
std_dev_doppler_error.push_back(std_dev_error);
|
||||
rmse_doppler.push_back(rmse);
|
||||
|
||||
code_phase_error_chips = check_results_codephase(true_timestamp_s, true_prn_delay_chips, trk_timestamp_s, trk_prn_delay_chips, mean_error, std_dev_error);
|
||||
code_phase_error_chips = check_results_codephase(true_timestamp_s, true_prn_delay_chips, trk_timestamp_s, trk_prn_delay_chips, mean_error, std_dev_error, rmse);
|
||||
mean_code_phase_error.push_back(mean_error);
|
||||
std_dev_code_phase_error.push_back(std_dev_error);
|
||||
rmse_code_phase.push_back(rmse);
|
||||
|
||||
acc_carrier_phase_hz = check_results_acc_carrier_phase(true_timestamp_s, true_acc_carrier_phase_cycles, trk_timestamp_s, trk_acc_carrier_phase_cycles, mean_error, std_dev_error);
|
||||
acc_carrier_phase_hz = check_results_acc_carrier_phase(true_timestamp_s, true_acc_carrier_phase_cycles, trk_timestamp_s, trk_acc_carrier_phase_cycles, mean_error, std_dev_error, rmse);
|
||||
mean_carrier_phase_error.push_back(mean_error);
|
||||
std_dev_carrier_phase_error.push_back(std_dev_error);
|
||||
rmse_carrier_phase.push_back(rmse);
|
||||
|
||||
//save tracking measurement timestamps to std::vector
|
||||
std::vector<double> vector_trk_timestamp_s(trk_timestamp_s.colptr(0), trk_timestamp_s.colptr(0) + trk_timestamp_s.n_rows);
|
||||
@ -760,10 +778,16 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
{
|
||||
mean_doppler_error_sweep.push_back(mean_doppler_error);
|
||||
std_dev_doppler_error_sweep.push_back(std_dev_doppler_error);
|
||||
rmse_doppler_sweep.push_back(rmse_doppler);
|
||||
|
||||
mean_code_phase_error_sweep.push_back(mean_code_phase_error);
|
||||
std_dev_code_phase_error_sweep.push_back(std_dev_code_phase_error);
|
||||
rmse_code_phase_sweep.push_back(rmse_code_phase);
|
||||
|
||||
mean_carrier_phase_error_sweep.push_back(mean_carrier_phase_error);
|
||||
std_dev_carrier_phase_error_sweep.push_back(std_dev_carrier_phase_error);
|
||||
rmse_carrier_phase_sweep.push_back(rmse_carrier_phase);
|
||||
|
||||
//make a copy of the CN0 vector for each configuration parameter in order to filter the loss of lock events
|
||||
generator_CN0_values_sweep_copy.push_back(valid_CN0_values);
|
||||
}
|
||||
@ -894,6 +918,10 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
catch (const GnuplotException& ge)
|
||||
{
|
||||
}
|
||||
save_mat_xy(trk_valid_timestamp_s_sweep.at(current_cn0_idx),
|
||||
code_phase_error_sweep.at(current_cn0_idx),
|
||||
"Code_error_" + std::to_string(generator_CN0_values_sweep_copy.at(config_idx).at(current_cn0_idx)) +
|
||||
std::to_string(PLL_wide_bw_values.at(config_idx)) + "_" + std::to_string(DLL_wide_bw_values.at(config_idx)));
|
||||
}
|
||||
g5.set_legend();
|
||||
g5.set_legend();
|
||||
@ -926,6 +954,10 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
catch (const GnuplotException& ge)
|
||||
{
|
||||
}
|
||||
save_mat_xy(trk_valid_timestamp_s_sweep.at(current_cn0_idx),
|
||||
acc_carrier_phase_error_sweep.at(current_cn0_idx),
|
||||
"Carrier_phase_error_output" + std::to_string(generator_CN0_values_sweep_copy.at(config_idx).at(current_cn0_idx)) +
|
||||
std::to_string(PLL_wide_bw_values.at(config_idx)) + "_" + std::to_string(DLL_wide_bw_values.at(config_idx)));
|
||||
}
|
||||
g6.set_legend();
|
||||
g6.set_legend();
|
||||
@ -946,7 +978,7 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
for (unsigned int current_cn0_idx = 0; current_cn0_idx < generator_CN0_values_sweep_copy.at(config_idx).size(); current_cn0_idx++)
|
||||
{
|
||||
g4.reset_plot();
|
||||
g4.set_title(std::to_string(static_cast<int>(round(generator_CN0_values_sweep_copy.at(config_idx).at(current_cn0_idx)))) + "[dB-Hz], PLL/DLL BW: " + std::to_string(PLL_wide_bw_values.at(config_idx)) + "," + std::to_string(DLL_wide_bw_values.at(config_idx)) + " Hz (PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")");
|
||||
g4.set_title("Dopper error" + std::to_string(static_cast<int>(round(generator_CN0_values_sweep_copy.at(config_idx).at(current_cn0_idx)))) + "[dB-Hz], PLL/DLL BW: " + std::to_string(PLL_wide_bw_values.at(config_idx)) + "," + std::to_string(DLL_wide_bw_values.at(config_idx)) + " Hz (PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")");
|
||||
g4.set_grid();
|
||||
//g4.cmd("set key box opaque");
|
||||
g4.set_xlabel("Time [s]");
|
||||
@ -959,6 +991,11 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
catch (const GnuplotException& ge)
|
||||
{
|
||||
}
|
||||
|
||||
save_mat_xy(trk_valid_timestamp_s_sweep.at(current_cn0_idx),
|
||||
doppler_error_sweep.at(current_cn0_idx),
|
||||
"Doppler_error_output" + std::to_string(generator_CN0_values_sweep_copy.at(config_idx).at(current_cn0_idx)) +
|
||||
std::to_string(PLL_wide_bw_values.at(config_idx)) + "_" + std::to_string(DLL_wide_bw_values.at(config_idx)));
|
||||
}
|
||||
g4.unset_multiplot();
|
||||
g4.savetops("Doppler_error_output");
|
||||
@ -1002,14 +1039,21 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
for (unsigned int config_sweep_idx = 0; config_sweep_idx < mean_doppler_error_sweep.size(); config_sweep_idx++)
|
||||
{
|
||||
g7.plot_xy_err(generator_CN0_values_sweep_copy.at(config_sweep_idx),
|
||||
mean_doppler_error_sweep.at(config_sweep_idx),
|
||||
generator_CN0_values_sweep_copy.at(config_sweep_idx),
|
||||
std_dev_doppler_error_sweep.at(config_sweep_idx),
|
||||
"PLL/DLL BW: " + std::to_string(PLL_wide_bw_values.at(config_sweep_idx)) +
|
||||
+"," + std::to_string(DLL_wide_bw_values.at(config_sweep_idx)) + " Hz");
|
||||
|
||||
//matlab save
|
||||
save_mat_xy(generator_CN0_values_sweep_copy.at(config_sweep_idx),
|
||||
rmse_doppler_sweep.at(config_sweep_idx),
|
||||
"RMSE_Doppler_CN0_Sweep_PLL_DLL" + std::to_string(PLL_wide_bw_values.at(config_sweep_idx)) +
|
||||
+"_" + std::to_string(DLL_wide_bw_values.at(config_sweep_idx)));
|
||||
}
|
||||
g7.savetops("Doppler_error_metrics");
|
||||
g7.savetopdf("Doppler_error_metrics", 18);
|
||||
|
||||
|
||||
Gnuplot g8("linespoints");
|
||||
g8.set_title("Accumulated carrier phase error metrics (PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")");
|
||||
g8.set_grid();
|
||||
@ -1025,6 +1069,11 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
std_dev_carrier_phase_error_sweep.at(config_sweep_idx),
|
||||
"PLL/DLL BW: " + std::to_string(PLL_wide_bw_values.at(config_sweep_idx)) +
|
||||
+"," + std::to_string(DLL_wide_bw_values.at(config_sweep_idx)) + " Hz");
|
||||
//matlab save
|
||||
save_mat_xy(generator_CN0_values_sweep_copy.at(config_sweep_idx),
|
||||
rmse_carrier_phase_sweep.at(config_sweep_idx),
|
||||
"RMSE_Carrier_Phase_CN0_Sweep_PLL_DLL" + std::to_string(PLL_wide_bw_values.at(config_sweep_idx)) +
|
||||
+"_" + std::to_string(DLL_wide_bw_values.at(config_sweep_idx)));
|
||||
}
|
||||
g8.savetops("Carrier_error_metrics");
|
||||
g8.savetopdf("Carrier_error_metrics", 18);
|
||||
@ -1044,6 +1093,11 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
std_dev_code_phase_error_sweep.at(config_sweep_idx),
|
||||
"PLL/DLL BW: " + std::to_string(PLL_wide_bw_values.at(config_sweep_idx)) +
|
||||
+"," + std::to_string(DLL_wide_bw_values.at(config_sweep_idx)) + " Hz");
|
||||
//matlab save
|
||||
save_mat_xy(generator_CN0_values_sweep_copy.at(config_sweep_idx),
|
||||
rmse_code_phase_sweep.at(config_sweep_idx),
|
||||
"RMSE_Code_Phase_CN0_Sweep_PLL_DLL" + std::to_string(PLL_wide_bw_values.at(config_sweep_idx)) +
|
||||
+"_" + std::to_string(DLL_wide_bw_values.at(config_sweep_idx)));
|
||||
}
|
||||
g9.savetops("Code_error_metrics");
|
||||
g9.savetopdf("Code_error_metrics", 18);
|
||||
@ -1055,3 +1109,33 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool GpsL1CADllPllTrackingTest::save_mat_xy(std::vector<double>& x, std::vector<double>& y, std::string filename)
|
||||
{
|
||||
try
|
||||
{
|
||||
// WRITE MAT FILE
|
||||
mat_t* matfp;
|
||||
matvar_t* matvar;
|
||||
filename.erase(filename.length() - 4, 4);
|
||||
filename.append(".mat");
|
||||
matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
|
||||
if (reinterpret_cast<long*>(matfp) != NULL)
|
||||
{
|
||||
size_t dims[2] = {1, x.size()};
|
||||
matvar = Mat_VarCreate("x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, &x[0], 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, &y[0], 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
}
|
||||
Mat_Close(matfp);
|
||||
return true;
|
||||
}
|
||||
catch (const std::exception& ex)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user