1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-18 11:09:56 +00:00

Completed the migration to Gnss_Synchro as a common object to exchange signal synchronization data between gnss-sdr GNURadio blocks

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@155 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas 2012-02-03 16:13:52 +00:00
parent 16a1797c6d
commit 0046d9169c
8 changed files with 56 additions and 102 deletions

View File

@ -286,7 +286,7 @@ PVT.averaging_depth=2
PVT.flag_averaging=true
;#dump: Enable or disable the PVT internal binary data file logging [true] or [false]
PVT.dump=true
PVT.dump=false
;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump.
PVT.dump_filename=./PVT

View File

@ -40,7 +40,7 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
#include "control_message_factory.h"
#include "gnss_synchro.h"
using google::LogMessage;
@ -53,7 +53,7 @@ gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump
gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging) :
gr_block ("gps_l1_ca_pvt_cc", gr_make_io_signature (nchannels, nchannels, sizeof(gnss_pseudorange)),
gr_block ("gps_l1_ca_pvt_cc", gr_make_io_signature (nchannels, nchannels, sizeof(Gnss_Synchro)),
gr_make_io_signature(1, 1, sizeof(gr_complex)))
{
d_queue = queue;
@ -95,9 +95,9 @@ gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc()
bool pseudoranges_pairCompare_min( std::pair<int,gnss_pseudorange> a, std::pair<int,gnss_pseudorange> b)
bool pseudoranges_pairCompare_min( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.pseudorange_m) < (b.second.pseudorange_m);
return (a.second.Pseudorange_m) < (b.second.Pseudorange_m);
}
@ -107,17 +107,17 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{
d_sample_counter++;
std::map<int,gnss_pseudorange> gnss_pseudoranges_map;
std::map<int,float> pseudoranges;
std::map<int,gnss_pseudorange>::iterator gnss_pseudoranges_iter;
std::map<int,Gnss_Synchro> gnss_pseudoranges_map;
std::map<int,double> pseudoranges;
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
gnss_pseudorange **in = (gnss_pseudorange **) &input_items[0]; //Get the input pointer
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
for (unsigned int i=0; i<d_nchannels; i++)
{
if (in[i][0].valid == true)
if (in[i][0].Flag_valid_pseudorange == true)
{
gnss_pseudoranges_map.insert(std::pair<int,gnss_pseudorange>(in[i][0].SV_ID, in[i][0])); //record the valid pseudorange in a map
gnss_pseudoranges_map.insert(std::pair<int,Gnss_Synchro>(in[i][0].PRN, in[i][0])); //record the valid pseudorange in a map
}
}
@ -125,7 +125,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
{
float pr = (float)gnss_pseudoranges_iter->second.pseudorange_m;
double pr = gnss_pseudoranges_iter->second.Pseudorange_m;
pseudoranges[gnss_pseudoranges_iter->first] = pr;
// std::cout << "Pseudoranges(SV ID,pseudorange [m]) =(" << gnss_pseudoranges_iter->first << ","
// << gnss_pseudoranges_iter->second.pseudorange_m << ")" <<std::endl;
@ -147,7 +147,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
nav_data_map[nav_msg.i_channel_ID] = nav_msg;
// **** update pseudoranges clock ****
if (nav_msg.i_satellite_PRN == gnss_pseudoranges_iter->second.SV_ID)
if (nav_msg.i_satellite_PRN == gnss_pseudoranges_iter->second.PRN)
{
d_ephemeris_clock_s = d_last_nav_msg.d_TOW;
d_ephemeris_timestamp_ms = d_last_nav_msg.d_subframe1_timestamp_ms;
@ -162,7 +162,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
// compute on the fly PVT solution
//std::cout<<"diff_clock_ephemeris="<<(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0<<"\r\n";
if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,
d_ephemeris_clock_s + (gnss_pseudoranges_iter->second.timestamp_ms - d_ephemeris_timestamp_ms)/1000.0,
d_ephemeris_clock_s + (gnss_pseudoranges_iter->second.Pseudorange_timestamp_ms - d_ephemeris_timestamp_ms)/1000.0,
d_flag_averaging) == true)
{
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);

View File

@ -35,6 +35,8 @@
#include <glog/logging.h>
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
using google::LogMessage;
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file)
@ -217,9 +219,9 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
}
bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_map,double GPS_current_time,bool flag_averaging)
bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,double GPS_current_time,bool flag_averaging)
{
std::map<int,gnss_pseudorange>::iterator gnss_pseudoranges_iter;
std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
arma::mat W = arma::eye(d_nchannels,d_nchannels); //channels weights matrix
arma::vec obs = arma::zeros(d_nchannels); // pseudoranges observation vector
@ -258,7 +260,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
satpos(2,i) = d_ephemeris[i].d_satpos_Z;
LOG_AT_LEVEL(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
obs(i) = gnss_pseudoranges_iter->second.pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
obs(i) = gnss_pseudoranges_iter->second.Pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
valid_obs++;
}
else

View File

@ -45,6 +45,8 @@
#include "GPS_L1_CA.h"
#include "armadillo"
#include "gnss_synchro.h"
/*!
* \brief This class implements a simple PVT Least Squares solution
*/
@ -84,7 +86,7 @@ public:
gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~gps_l1_ca_ls_pvt();
bool get_PVT(std::map<int,gnss_pseudorange> pseudoranges,double GPS_current_time,bool flag_averaging);
bool get_PVT(std::map<int,Gnss_Synchro> pseudoranges,double GPS_current_time,bool flag_averaging);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical

View File

@ -966,7 +966,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Navigation_Message
void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav_msg, std::map<int,float> pseudoranges)
void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav_msg, std::map<int,double> pseudoranges)
{
std::string line;
@ -1017,7 +1017,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav
line += std::string(2, ' ');
//Number of satellites observed in current epoch
int numSatellitesObserved = 0;
std::map<int,float>::iterator pseudoranges_iter;
std::map<int,double>::iterator pseudoranges_iter;
for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++)
@ -1048,7 +1048,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav
lineObs.clear();
line.clear();
line += std::string(2, ' ');
lineObs += Rinex_Printer::rightJustify(asString((double)pseudoranges_iter->second, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second, 3), 14);
//Loss of lock indicator (LLI)
int lli = 0; // Include in the observation!!
@ -1097,7 +1097,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav
//Number of satellites observed in current epoch
int numSatellitesObserved = 0;
std::map<int,float>::iterator pseudoranges_iter;
std::map<int,double>::iterator pseudoranges_iter;
for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++)
@ -1124,7 +1124,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav
if ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0');
lineObs += boost::lexical_cast<std::string>((int)pseudoranges_iter->first);
//lineObs += std::string(2, ' ');
lineObs += Rinex_Printer::rightJustify(asString((double)pseudoranges_iter->second, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second, 3), 14);
//Loss of lock indicator (LLI)
int lli = 0; // Include in the observation!!

View File

@ -105,7 +105,7 @@ public:
/*!
* \brief Writes observables into the RINEX file
*/
void log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav_msg, std::map<int,float> pseudoranges);
void log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav_msg, std::map<int,double> pseudoranges);
std::map<std::string,std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass
std::map<std::string,std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH

View File

@ -58,7 +58,7 @@ gps_l1_ca_make_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, b
gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging) :
gr_block ("gps_l1_ca_observables_cc", gr_make_io_signature (nchannels, nchannels, sizeof(Gnss_Synchro)),
gr_make_io_signature(nchannels, nchannels, sizeof(gnss_pseudorange)))
gr_make_io_signature(nchannels, nchannels, sizeof(Gnss_Synchro)))
{
// initialize internal vars
d_queue = queue;
@ -87,9 +87,6 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
}
}
gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
{
d_dump_file.close();
@ -97,28 +94,18 @@ gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
}
bool pairCompare_gnss_synchro( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.Preamble_delay_ms) < (b.second.Preamble_delay_ms);
}
bool pairCompare_double( std::pair<int,double> a, std::pair<int,double> b)
{
return (a.second) < (b.second);
}
void clearQueue( std::deque<double> &q )
{
std::deque<double> empty;
@ -126,16 +113,13 @@ void clearQueue( std::deque<double> &q )
}
int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
gnss_pseudorange **out = (gnss_pseudorange **) &output_items[0]; //Get the output pointer
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //Get the output pointer
gnss_pseudorange current_gnss_pseudorange;
Gnss_Synchro current_gnss_synchro[d_nchannels];
std::map<int,Gnss_Synchro> gps_words;
std::map<int,Gnss_Synchro>::iterator gps_words_iter;
@ -164,20 +148,23 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
*/
for (unsigned int i=0; i<d_nchannels ; i++)
{
if (in[i][0].Flag_valid_word) //if this channel have valid word
//Copy the telemetry decoder data to local copy
current_gnss_synchro[i]=in[i][0];
if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word
{
gps_words.insert(std::pair<int,Gnss_Synchro>(in[i][0].Channel_ID, in[i][0])); //record the word structure in a map for pseudoranges
gps_words.insert(std::pair<int,Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); //record the word structure in a map for pseudoranges
// RECORD PRN start timestamps history
if (d_history_prn_delay_ms[i].size()<MAX_TOA_DELAY_MS)
{
d_history_prn_delay_ms[i].push_front(in[i][0].Prn_delay_ms);
d_history_prn_delay_ms[i].push_front(current_gnss_synchro[i].Prn_delay_ms);
flag_history_ok = false; // at least one channel need more samples
}
else
{
//clearQueue(d_history_prn_delay_ms[i]); //clear the queue as the preamble arrives
d_history_prn_delay_ms[i].pop_back();
d_history_prn_delay_ms[i].push_front(in[i][0].Prn_delay_ms);
d_history_prn_delay_ms[i].push_front(current_gnss_synchro[i].Prn_delay_ms);
}
}
}
@ -187,11 +174,9 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
*/
for (unsigned int i=0; i<d_nchannels ; i++)
{
current_gnss_pseudorange.valid = false;
current_gnss_pseudorange.SV_ID = 0;
current_gnss_pseudorange.pseudorange_m = 0;
current_gnss_pseudorange.timestamp_ms = 0;
*out[i] = current_gnss_pseudorange;
current_gnss_synchro[i].Flag_valid_pseudorange = false;
current_gnss_synchro[i].Pseudorange_m = 0.0;
current_gnss_synchro[i].Pseudorange_timestamp_ms = 0.0;
}
/*
* 2. Compute RAW pseudorranges: Use only the valid channels (channels that are tracking a satellite)
@ -259,18 +244,15 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
traveltime_ms = current_prn_delay_ms - actual_min_prn_delay_ms + GPS_STARTOFFSET_ms; //[ms]
//std::cout<<"delta_time_ms="<<current_prn_delay_ms-actual_min_prn_delay_ms<<"\r\n";
pseudorange_m = traveltime_ms*GPS_C_m_ms; // [m]
// update the pseudorange object
current_gnss_pseudorange.pseudorange_m = pseudorange_m;
current_gnss_pseudorange.timestamp_ms = pseudoranges_timestamp_ms;
current_gnss_pseudorange.SV_ID = gps_words_iter->second.PRN;
current_gnss_pseudorange.valid = true;
// #### write the pseudorrange block output for this satellite ###
*out[gps_words_iter->second.Channel_ID] = current_gnss_pseudorange;
current_gnss_synchro[gps_words_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
current_gnss_synchro[gps_words_iter->second.Channel_ID].Pseudorange_timestamp_ms = pseudoranges_timestamp_ms;
current_gnss_synchro[gps_words_iter->second.Channel_ID].Flag_valid_pseudorange = true;
}
}
}
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
@ -279,15 +261,15 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
tmp_double = in[i][0].Preamble_delay_ms;
tmp_double = current_gnss_synchro[i].Preamble_delay_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = in[i][0].Prn_delay_ms;
tmp_double = current_gnss_synchro[i].Prn_delay_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = out[i][0].pseudorange_m;
tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = out[i][0].timestamp_ms;
tmp_double = current_gnss_synchro[i].Pseudorange_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = out[i][0].SV_ID;
tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
}
@ -299,8 +281,14 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
consume_each(1); //one by one
if ((d_sample_counter % d_output_rate_ms) == 0)
{
for (unsigned int i=0; i<d_nchannels ; i++)
{
*out[i] = current_gnss_synchro[i];
}
return 1; //Output the observables
}
else

View File

@ -76,11 +76,6 @@ const int GPS_SUBFRAME_BITS=300; //!< Number of bits per subframe
const int GPS_WORD_BITS=30; //!< Number of bits per word in the NAV message [bits]
/*!
* \brief Navigation message bits slice structure: A portion of bits is indicated by
* the start position inside the subframe and the length in number of bits */
@ -96,39 +91,6 @@ typedef struct bits_slice
} bits_slice;
/*!
* \brief Demodulator gnss_synchro structure, used to feed the pseudorange block */
typedef struct gnss_synchro
{
double preamble_delay_ms;
double prn_delay_ms;
double preamble_code_phase_ms;
double preamble_code_phase_correction_ms;
int satellite_PRN;
int channel_ID;
bool valid_word;
bool flag_preamble;
} gnss_synchro;
/*!
* \brief Observables structure, used to feed the PVT block */
typedef struct gnss_pseudorange
{
double pseudorange_m;
double timestamp_ms;
unsigned int SV_ID;
bool valid;
} gnss_pseudorange;
/* Constants for scaling the ephemeris found in the data message
the format is the following: TWO_N5 -> 2^-5, TWO_P4 -> 2^4, PI_TWO_N43 -> Pi*2^-43, etc etc
Additionally some of the PI*2^N terms are used in the tracking stuff