mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-10-31 23:26:22 +00:00
Code Issues Releases Wiki Activity

Added a method in gps_navigation message that computes UTC time.

Browse Source 
Experimental creation of RINEX headers.

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@112 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez 2012-01-07 05:21:11 +00:00
parent 03d6999225
commit 8738498691
11 changed files with 542 additions and 404 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
View File

@ -40,7 +40,7 @@
#include <glog/logging.h> #include <glog/logging.h>
#include "gps_l1_ca_pvt_cc.h" #include "gps_l1_ca_pvt_cc.h"
#include "control_message_factory.h" #include "control_message_factory.h"
#include "rinex_2_1_printer.h"
using google::LogMessage; using google::LogMessage;
@ -78,11 +78,20 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
d_ephemeris_clock_s=0.0; d_ephemeris_clock_s=0.0;
d_sample_counter=0; d_sample_counter=0;
b_rinex_header_writen = false;
rp = new rinex_printer();
//rp->navFile.open(rp->createFilename("RINEX_FILE_TYPE_GPS_NAV"), std::ios::out | std::ios::app);
//rp->obsFile.open(rp->createFilename("RINEX_FILE_TYPE_OBS"), std::ios::out | std::ios::app);
//Rinex_Nav_File=rp.getNavFileStream();
//Rinex_Obs_File=rp.getObsFileStream();
} }
gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc() { gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc() {
d_kml_dump.close_file(); d_kml_dump.close_file();
delete d_ls_pvt; delete d_ls_pvt;
delete rp;
} }
bool pseudoranges_pairCompare_min( std::pair<int,gnss_pseudorange> a, std::pair<int,gnss_pseudorange> b) bool pseudoranges_pairCompare_min( std::pair<int,gnss_pseudorange> a, std::pair<int,gnss_pseudorange> b)
@ -146,12 +155,19 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{ {
//d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,pseudoranges); //d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,pseudoranges);
// compute on the fly PVT solution // compute on the fly PVT solution
//std::cout<<"diff_clock_ephemerids="<<(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0<<"\r\n"; //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, 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.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,
d_flag_averaging)==true) d_flag_averaging)==true)
{ {
d_kml_dump.print_position(d_ls_pvt,d_flag_averaging); d_kml_dump.print_position(d_ls_pvt,d_flag_averaging);
if (!b_rinex_header_writen) // & we have utc data in nav message!
{
// rinex_printer rinex_printer(d_last_nav_msg);
rp->Rinex2NavHeader(rp->navFile, d_last_nav_msg);
rp->Rinex2ObsHeader(rp->obsFile, d_last_nav_msg);
b_rinex_header_writen=true; // do not write header anymore
}
} }
} }

4
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h
View File

@ -63,6 +63,10 @@ private:
// class private vars // class private vars
gr_msg_queue_sptr d_queue; gr_msg_queue_sptr d_queue;
bool d_dump; bool d_dump;
bool b_rinex_header_writen;
//std::ofstream Rinex_Nav_File;
//std::ofstream Rinex_Obs_File;
rinex_printer *rp;
unsigned int d_nchannels; unsigned int d_nchannels;

20
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@ -33,7 +33,7 @@
#include "GPS_L1_CA.h" #include "GPS_L1_CA.h"
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
#include "boost/date_time/posix_time/posix_time.hpp"
using google::LogMessage; using google::LogMessage;
@ -211,6 +211,10 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
arma::vec obs=arma::zeros(d_nchannels); // pseudoranges observation vector arma::vec obs=arma::zeros(d_nchannels); // pseudoranges observation vector
arma::mat satpos=arma::zeros(3,d_nchannels); //satellite positions matrix arma::mat satpos=arma::zeros(3,d_nchannels); //satellite positions matrix
int GPS_week;
double GPS_corrected_time;
double utc;
int valid_obs=0; //valid observations counter int valid_obs=0; //valid observations counter
for (int i=0; i<d_nchannels; i++) for (int i=0; i<d_nchannels; i++)
{ {
@ -227,9 +231,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
// d_ephemeris[i].master_clock(GPS_current_time); ????? // d_ephemeris[i].master_clock(GPS_current_time); ?????
// compute the clock error including relativistic effects // compute the clock error including relativistic effects
d_ephemeris[i].sv_clock_correction(GPS_current_time); GPS_corrected_time = d_ephemeris[i].sv_clock_correction(GPS_current_time);
GPS_week = d_ephemeris[i].i_GPS_week;
utc =d_ephemeris[i].utc_time(GPS_corrected_time);
// compute the satellite current ECEF position // compute the satellite current ECEF position
d_ephemeris[i].satellitePosition(GPS_current_time); d_ephemeris[i].satellitePosition(GPS_corrected_time);
satpos(0,i)=d_ephemeris[i].d_satpos_X; satpos(0,i)=d_ephemeris[i].d_satpos_X;
satpos(1,i)=d_ephemeris[i].d_satpos_Y; satpos(1,i)=d_ephemeris[i].d_satpos_Y;
@ -256,7 +264,11 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
mypos=leastSquarePos(satpos,obs,W); mypos=leastSquarePos(satpos,obs,W);
LOG_AT_LEVEL(INFO) << "Position at TOW="<<GPS_current_time<<" in ECEF (X,Y,Z) = " << mypos << std::endl; LOG_AT_LEVEL(INFO) << "Position at TOW="<<GPS_current_time<<" in ECEF (X,Y,Z) = " << mypos << std::endl;
cart2geo(mypos(0), mypos(1), mypos(2), 4); cart2geo(mypos(0), mypos(1), mypos(2), 4);
std::cout << "Position at TOW="<<GPS_current_time<<" is Lat = " << d_latitude_d << " [deg] Long = "<< d_longitude_d <<" [deg] Height= "<<d_height_m<<" [m]" <<std::endl;
// Compute UTC time and print PVT solution
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800*(double)GPS_week);
boost::posix_time::ptime p_time(boost::gregorian::date(1999,8,22),t);
std::cout << "Position at "<<boost::posix_time::to_simple_string(p_time)<<" is Lat = " << d_latitude_d << " [deg] Long = "<< d_longitude_d <<" [deg] Height= "<<d_height_m<<" [m]" <<std::endl;
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled==true) { if(d_flag_dump_enabled==true) {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file

1
src/algorithms/PVT/libs/kml_printer.h
View File

@ -36,6 +36,7 @@
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
#include "gps_l1_ca_ls_pvt.h" #include "gps_l1_ca_ls_pvt.h"
#include "gps_navigation_message.h"
/*! /*!
* \brief Prints PVT information to OGC KML format file (can be viewed with Google Earth) * \brief Prints PVT information to OGC KML format file (can be viewed with Google Earth)

60
src/algorithms/PVT/libs/rinex_2_1_printer.cc
View File

@ -46,12 +46,23 @@
using google::LogMessage; using google::LogMessage;
//std::ofstream getNavFileStream() {
// return navFile;
//}
//std::ofstream getObsFileStream() {
// return obsFile;
//}
std::ofstream getObsFileStream() ;
rinex_printer::rinex_printer() rinex_printer::rinex_printer()
{ {
rinex_printer::navFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_GPS_NAV")); rinex_printer::navFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_GPS_NAV"), std::ios::out | std::ios::app);
rinex_printer::obsFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_OBS")); rinex_printer::obsFile.open(rinex_printer::createFilename("RINEX_FILE_TYPE_OBS"), std::ios::out | std::ios::app);
//rinex_printer::Rinex2NavHeader(rinex_printer::navFile, gps_navigation_message nav); //rinex_printer::Rinex2NavHeader(rinex_printer::navFile, nav);
//rinex_printer::Rinex2ObsHeader(rinex_printer::navFile, nav);
satelliteSystem["GPS"]="G"; satelliteSystem["GPS"]="G";
satelliteSystem["GLONASS"]="R"; satelliteSystem["GLONASS"]="R";
@ -62,7 +73,7 @@ rinex_printer::rinex_printer()
observationCode["GPS_L1_CA"] = "1C"; //!< "1C" GPS L1 C/A observationCode["GPS_L1_CA"] = "1C"; //!< "1C" GPS L1 C/A
observationCode["GPS_L1_P"] = "1P"; //!< "1P" GPS L1 P observationCode["GPS_L1_P"] = "1P"; //!< "1P" GPS L1 P
observationCode["GPS_L1_Z_TRACKING"] = "1W"; //!< "1W" GPS L1 Z-tracking and similar (AS on) observationCode["GPS_L1_Z_TRACKING"] = "1W"; //!< "1W" GPS L1 Z-tracking and similar (AS on)
observationCode["RINEX_GPS_L1_Y"] = "1Y"; //!< "1Y" GPS L1 Y observationCode["GPS_L1_Y"] = "1Y"; //!< "1Y" GPS L1 Y
observationCode["GPS_L1_M "]= "1M"; //!< "1M" GPS L1 M observationCode["GPS_L1_M "]= "1M"; //!< "1M" GPS L1 M
observationCode["GPS_L1_CODELESS"] = "1N"; //!< "1N" GPS L1 codeless observationCode["GPS_L1_CODELESS"] = "1N"; //!< "1N" GPS L1 codeless
observationCode["GPS_L2_CA"]= "2C"; //!< "2C" GPS L2 C/A observationCode["GPS_L2_CA"]= "2C"; //!< "2C" GPS L2 C/A
@ -529,7 +540,7 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out, gps_navigation_message n
out << line << std::endl; out << line << std::endl;
// -------- Line 6 // -------- Line OBSERVER / AGENCY
line.clear(); line.clear();
std::string username=getenv("USER"); std::string username=getenv("USER");
line += leftJustify(username,20); line += leftJustify(username,20);
@ -540,7 +551,7 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out, gps_navigation_message n
// -------- Line 6 REC / TYPE VERS // -------- Line REC / TYPE VERS
line.clear(); line.clear();
line += rinex_printer::leftJustify("GNSS-SDR",20); // add flag and property line += rinex_printer::leftJustify("GNSS-SDR",20); // add flag and property
line += rinex_printer::leftJustify("Software Receiver",20); // add flag and property line += rinex_printer::leftJustify("Software Receiver",20); // add flag and property
@ -608,23 +619,22 @@ void rinex_printer::Rinex2ObsHeader(std::ofstream& out, gps_navigation_message n
// -------- TIME OF FIRST OBS // -------- TIME OF FIRST OBS
line.clear(); line.clear();
line += std::string("GPS");
line += std::string(5,' ');
/////////////////////////////////////////// ///////////////////////////////////////////
// 4-digit-year, month,day,hour,min,sec // 4-digit-year, month,day,hour,min,sec
double year=2012; boost::posix_time::ptime p_utc_time = rinex_printer::computeTime(nav_msg);
double month=1;
double day=4; tm pt_utc_tm=boost::posix_time::to_tm(p_utc_time);
double hour=8;
double minute =43; double seconds =(double)(pt_utc_tm.tm_sec);
double second = GPS_PI;
line += rightJustify(asString<short>(year), 6); line += rightJustify(asString<short>(pt_utc_tm.tm_year+1900), 6);
line += rightJustify(asString<short>(month), 6); line += rightJustify(asString<short>(pt_utc_tm.tm_mon), 6);
line += rightJustify(asString<short>(day), 6); line += rightJustify(asString<short>(pt_utc_tm.tm_mday), 6);
line += rightJustify(asString<short>(hour), 6); line += rightJustify(asString<short>(pt_utc_tm.tm_hour), 6);
line += rightJustify(asString<short>(minute), 6); line += rightJustify(asString<short>(pt_utc_tm.tm_min), 6);
line += rightJustify(asString(second,7), 13); line += rightJustify(asString(seconds,7), 13);
line += rightJustify(std::string("GPS"), 8); line += rightJustify(std::string("GPS"), 8);
line += rinex_printer::leftJustify("TIME OF FIRST OBS",20); line += rinex_printer::leftJustify("TIME OF FIRST OBS",20);
@ -770,7 +780,15 @@ int rinex_printer::signalStrength(double snr)
} }
boost::posix_time::ptime rinex_printer::computeTime(gps_navigation_message nav_msg)
{
// 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 http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm
double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW));
boost::posix_time::time_duration t = boost::posix_time::seconds(utc_t+ 604800*(double)(nav_msg.i_GPS_week));// should be i_WN_T?
boost::posix_time::ptime p_time(boost::gregorian::date(1999,8,22),t);
return p_time;
}
/* /*

29
src/algorithms/PVT/libs/rinex_2_1_printer.h
View File

@ -37,6 +37,7 @@
#include <sstream> // for stringstream #include <sstream> // for stringstream
#include <iomanip> // for setprecision #include <iomanip> // for setprecision
#include "gps_navigation_message.h" #include "gps_navigation_message.h"
#include "boost/date_time/posix_time/posix_time.hpp"
/*! /*!
* \brief Class that handles the generation of Receiver * \brief Class that handles the generation of Receiver
@ -45,18 +46,9 @@
class rinex_printer class rinex_printer
{ {
private: private:
std::ofstream navFile ;
std::ofstream obsFile ;
/*
* Generates the Navigation Data header
*/
void Rinex2NavHeader(std::ofstream& out, gps_navigation_message nav);
/*
* Generates the Observation data header
*/
void Rinex2ObsHeader(std::ofstream& out, gps_navigation_message nav);
/* /*
* Generation of RINEX signal strength indicators * Generation of RINEX signal strength indicators
@ -262,13 +254,28 @@ private:
public: public:
/*! /*!
* \brief Default constructor. Creates GPS Navigation and Observables RINEX files and their headers * \brief Default constructor. Creates GPS Navigation and Observables RINEX files and their headers
*/ */
rinex_printer(); rinex_printer();
std::ofstream obsFile ;
std::ofstream navFile ;
/*!
* \brief Generates the Navigation Data header
*/
void Rinex2NavHeader(std::ofstream& out, gps_navigation_message nav);
/*!
* \brief Generates the Observation data header
*/
void Rinex2ObsHeader(std::ofstream& out, gps_navigation_message nav);
boost::posix_time::ptime computeTime(gps_navigation_message nav_msg);
/*! /*!
* \brief Default destructor. Closes GPS Navigation and Observables RINEX files * \brief Default destructor. Closes GPS Navigation and Observables RINEX files
*/ */

3
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@ -230,7 +230,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++) for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{ {
// #### compute the pseudorrange for this satellite ### // #### compute the pseudorange for this satellite ###
current_prn_delay_ms=current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID); current_prn_delay_ms=current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID);
traveltime_ms=current_prn_delay_ms-actual_min_prn_delay_ms+GPS_STARTOFFSET_ms; //[ms] traveltime_ms=current_prn_delay_ms-actual_min_prn_delay_ms+GPS_STARTOFFSET_ms; //[ms]
@ -271,6 +271,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
} }
} }
consume_each(1); //one by one consume_each(1); //one by one
if ((d_sample_counter%d_output_rate_ms)==0) if ((d_sample_counter%d_output_rate_ms)==0)
{ {
return 1; //Output the observables return 1; //Output the observables

682
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
View File

@ -122,26 +122,26 @@ gps_l1_ca_dll_fll_pll_tracking_cc::gps_l1_ca_dll_fll_pll_tracking_cc(unsigned in
void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){ void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
/* /*
* correct the code phase according to the delay between acq and trk * correct the code phase according to the delay between acq and trk
*/ */
unsigned long int acq_trk_diff_samples; unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds; float acq_trk_diff_seconds;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp;//-d_vector_length; acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp;//-d_vector_length;
//std::cout<<"acq_trk_diff_samples="<<acq_trk_diff_samples<<"\r\n"; //std::cout<<"acq_trk_diff_samples="<<acq_trk_diff_samples<<"\r\n";
acq_trk_diff_seconds=(float)acq_trk_diff_samples/(float)d_fs_in; acq_trk_diff_seconds=(float)acq_trk_diff_samples/(float)d_fs_in;
//doppler effect //doppler effect
// Fd=(C/(C+Vr))*F // Fd=(C/(C+Vr))*F
float radial_velocity; float radial_velocity;
radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ; radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler // new chip and prn sequence periods based on acq Doppler
float T_chip_mod_seconds; float T_chip_mod_seconds;
float T_prn_mod_seconds; float T_prn_mod_seconds;
float T_prn_mod_samples; float T_prn_mod_samples;
d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ; d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds=1/d_code_freq_hz; T_chip_mod_seconds=1/d_code_freq_hz;
T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in; T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
d_next_prn_length_samples=round(T_prn_mod_samples); d_next_prn_length_samples=round(T_prn_mod_samples);
@ -155,66 +155,66 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
corrected_acq_phase_samples=fmod((d_acq_code_phase_samples+T_prn_diff_seconds*N_prn_diff*(float)d_fs_in),T_prn_true_samples); corrected_acq_phase_samples=fmod((d_acq_code_phase_samples+T_prn_diff_seconds*N_prn_diff*(float)d_fs_in),T_prn_true_samples);
if (corrected_acq_phase_samples<0) if (corrected_acq_phase_samples<0)
{ {
corrected_acq_phase_samples=T_prn_mod_samples+corrected_acq_phase_samples; corrected_acq_phase_samples=T_prn_mod_samples+corrected_acq_phase_samples;
} }
delay_correction_samples=d_acq_code_phase_samples-corrected_acq_phase_samples; delay_correction_samples=d_acq_code_phase_samples-corrected_acq_phase_samples;
d_acq_code_phase_samples=corrected_acq_phase_samples; d_acq_code_phase_samples=corrected_acq_phase_samples;
d_carrier_doppler_hz=d_acq_carrier_doppler_hz; d_carrier_doppler_hz=d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization // DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz);
d_FLL_wait=1; d_FLL_wait=1;
// generate local reference ALWAYS starting at chip 1 (1 sample per chip) // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
code_gen_conplex(&d_ca_code[1],d_satellite,0); code_gen_conplex(&d_ca_code[1],d_satellite,0);
d_ca_code[0]=d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS]; d_ca_code[0]=d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1]; d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1];
d_carrier_lock_fail_counter=0; d_carrier_lock_fail_counter=0;
d_Prompt_prev=0; d_Prompt_prev=0;
d_rem_code_phase_samples=0; d_rem_code_phase_samples=0;
d_rem_carr_phase=0; d_rem_carr_phase=0;
d_FLL_discriminator_hz=0; d_FLL_discriminator_hz=0;
d_rem_code_phase_samples=0; d_rem_code_phase_samples=0;
d_next_rem_code_phase_samples=0; d_next_rem_code_phase_samples=0;
d_acc_carrier_phase_rad=0; d_acc_carrier_phase_rad=0;
d_code_phase_samples = d_acq_code_phase_samples; d_code_phase_samples = d_acq_code_phase_samples;
// DEBUG OUTPUT // DEBUG OUTPUT
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl; std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl;
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received "; DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
// enable tracking // enable tracking
d_pull_in=true; d_pull_in=true;
d_enable_tracking=true; d_enable_tracking=true;
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" Code Phase correction [samples]="<<delay_correction_samples<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n"; std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" Code Phase correction [samples]="<<delay_correction_samples<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
} }
void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_code() void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_code()
{ {
float tcode_chips; float tcode_chips;
float rem_code_phase_chips; float rem_code_phase_chips;
float code_phase_step_chips; float code_phase_step_chips;
int associated_chip_index; int associated_chip_index;
int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS;
code_phase_step_chips=d_code_freq_hz/((float)d_fs_in); code_phase_step_chips=d_code_freq_hz/((float)d_fs_in);
rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in); rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in);
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
tcode_chips=-rem_code_phase_chips; tcode_chips=-rem_code_phase_chips;
for (int i=0;i<d_current_prn_length_samples;i++) for (int i=0;i<d_current_prn_length_samples;i++)
{ {
associated_chip_index=1+round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips)); associated_chip_index=1+round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips)); associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips));
d_prompt_code[i] = d_ca_code[associated_chip_index]; d_prompt_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1+round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips));
d_late_code[i] = d_ca_code[associated_chip_index]; d_late_code[i] = d_ca_code[associated_chip_index];
tcode_chips=tcode_chips+code_phase_step_chips; tcode_chips=tcode_chips+code_phase_step_chips;
} }
//d_code_phase_samples=d_code_phase_samples+(float)d_fs_in*GPS_L1_CA_CODE_LENGTH_CHIPS*(1/d_code_freq_hz-1/GPS_L1_CA_CODE_RATE_HZ); //d_code_phase_samples=d_code_phase_samples+(float)d_fs_in*GPS_L1_CA_CODE_LENGTH_CHIPS*(1/d_code_freq_hz-1/GPS_L1_CA_CODE_RATE_HZ);
} }
void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_carrier() void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_carrier()
@ -223,15 +223,15 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_carrier()
phase_step = (float)TWO_PI*d_carrier_doppler_hz/(float)d_fs_in; phase_step = (float)TWO_PI*d_carrier_doppler_hz/(float)d_fs_in;
phase=d_rem_carr_phase; phase=d_rem_carr_phase;
for(int i = 0; i < d_current_prn_length_samples; i++) { for(int i = 0; i < d_current_prn_length_samples; i++) {
d_carr_sign[i] = gr_complex(cos(phase),sin(phase)); d_carr_sign[i] = gr_complex(cos(phase),sin(phase));
phase += phase_step; phase += phase_step;
} }
d_rem_carr_phase=fmod(phase,TWO_PI); d_rem_carr_phase=fmod(phase,TWO_PI);
d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+d_rem_carr_phase; d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+d_rem_carr_phase;
} }
gps_l1_ca_dll_fll_pll_tracking_cc::~gps_l1_ca_dll_fll_pll_tracking_cc() { gps_l1_ca_dll_fll_pll_tracking_cc::~gps_l1_ca_dll_fll_pll_tracking_cc() {
d_dump_file.close(); d_dump_file.close();
delete[] d_ca_code; delete[] d_ca_code;
delete[] d_early_code; delete[] d_early_code;
delete[] d_prompt_code; delete[] d_prompt_code;
@ -245,323 +245,323 @@ gps_l1_ca_dll_fll_pll_tracking_cc::~gps_l1_ca_dll_fll_pll_tracking_cc() {
*/ */
int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
// if ((unsigned int)ninput_items[0]<(d_vector_length*2)) // if ((unsigned int)ninput_items[0]<(d_vector_length*2))
// { // {
// std::cout<<"End of signal detected\r\n"; // std::cout<<"End of signal detected\r\n";
// const int samples_available = ninput_items[0]; // const int samples_available = ninput_items[0];
// consume_each(samples_available); // consume_each(samples_available);
// return 0; // return 0;
// } // }
// process vars // process vars
float code_error_chips=0; float code_error_chips=0;
float correlation_time_s=0; float correlation_time_s=0;
float PLL_discriminator_hz=0; float PLL_discriminator_hz=0;
float carr_nco_hz=0; float carr_nco_hz=0;
d_Prompt_prev=d_Prompt; // for the FLL discriminator d_Prompt_prev=d_Prompt; // for the FLL discriminator
d_Early=gr_complex(0,0); d_Early=gr_complex(0,0);
d_Prompt=gr_complex(0,0); d_Prompt=gr_complex(0,0);
d_Late=gr_complex(0,0); d_Late=gr_complex(0,0);
if (d_enable_tracking==true){ if (d_enable_tracking==true){
/* /*
* Receiver signal alignment * Receiver signal alignment
*/ */
if (d_pull_in==true) if (d_pull_in==true)
{ {
int samples_offset; int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION // 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples=d_sample_counter-d_acq_sample_stamp; acq_to_trk_delay_samples=d_sample_counter-d_acq_sample_stamp;
acq_trk_shif_correction_samples=d_next_prn_length_samples-fmod((float)acq_to_trk_delay_samples,(float)d_next_prn_length_samples); acq_trk_shif_correction_samples=d_next_prn_length_samples-fmod((float)acq_to_trk_delay_samples,(float)d_next_prn_length_samples);
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n"; //std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
samples_offset=round(d_acq_code_phase_samples+acq_trk_shif_correction_samples); samples_offset=round(d_acq_code_phase_samples+acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE // /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset)/(double)d_fs_in); d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset)/(double)d_fs_in);
d_sample_counter=d_sample_counter+samples_offset; //count for the processed samples d_sample_counter=d_sample_counter+samples_offset; //count for the processed samples
d_pull_in=false; d_pull_in=false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n"; //std::cout<<" samples_offset="<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica consume_each(samples_offset); //shift input to perform alignement with local replica
return 1; return 1;
} }
// get the sample in and out pointers // get the sample in and out pointers
const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer
double **out = (double **) &output_items[0]; //block output streams pointer double **out = (double **) &output_items[0]; //block output streams pointer
// check for samples consistency // check for samples consistency
for(int i=0;i<d_current_prn_length_samples;i++) { for(int i=0;i<d_current_prn_length_samples;i++) {
if (std::isnan(in[i].real())==true or std::isnan(in[i].imag())==true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan(in[i].real())==true or std::isnan(in[i].imag())==true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{ {
const int samples_available= ninput_items[0]; const int samples_available= ninput_items[0];
d_sample_counter=d_sample_counter+samples_available; d_sample_counter=d_sample_counter+samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<<d_sample_counter; LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<<d_sample_counter;
consume_each(samples_available); consume_each(samples_available);
return 0; return 0;
} }
} }
// Update the prn length based on code freq (variable) and // Update the prn length based on code freq (variable) and
// sampling frequency (fixed) // sampling frequency (fixed)
// variable code PRN sample block size // variable code PRN sample block size
d_current_prn_length_samples=d_next_prn_length_samples; d_current_prn_length_samples=d_next_prn_length_samples;
update_local_code(); update_local_code();
update_local_carrier(); update_local_carrier();
gr_complex bb_signal_sample(0,0); gr_complex bb_signal_sample(0,0);
// perform Early, Prompt and Late correlation // perform Early, Prompt and Late correlation
/*! /*!
* \todo Use SIMD-enabled correlators * \todo Use SIMD-enabled correlators
*/ */
for(int i=0;i<d_current_prn_length_samples;i++) { for(int i=0;i<d_current_prn_length_samples;i++) {
//Perform the carrier wipe-off //Perform the carrier wipe-off
bb_signal_sample = in[i] * d_carr_sign[i]; bb_signal_sample = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each // Now get early, late, and prompt values for each
d_Early += bb_signal_sample*d_early_code[i]; d_Early += bb_signal_sample*d_early_code[i];
d_Prompt += bb_signal_sample*d_prompt_code[i]; d_Prompt += bb_signal_sample*d_prompt_code[i];
d_Late += bb_signal_sample*d_late_code[i]; d_Late += bb_signal_sample*d_late_code[i];
} }
/* /*
* DLL, FLL, and PLL discriminators * DLL, FLL, and PLL discriminators
*/ */
// Compute DLL error // Compute DLL error
code_error_chips=dll_nc_e_minus_l_normalized(d_Early,d_Late); code_error_chips=dll_nc_e_minus_l_normalized(d_Early,d_Late);
//compute FLL error //compute FLL error
correlation_time_s=((float)d_current_prn_length_samples)/(float)d_fs_in; correlation_time_s=((float)d_current_prn_length_samples)/(float)d_fs_in;
if (d_FLL_wait==1) if (d_FLL_wait==1)
{ {
d_Prompt_prev=d_Prompt; d_Prompt_prev=d_Prompt;
d_FLL_wait=0; d_FLL_wait=0;
}else{ }else{
d_FLL_discriminator_hz=fll_four_quadrant_atan(d_Prompt_prev, d_Prompt, 0, correlation_time_s)/(float)TWO_PI; d_FLL_discriminator_hz=fll_four_quadrant_atan(d_Prompt_prev, d_Prompt, 0, correlation_time_s)/(float)TWO_PI;
d_Prompt_prev=d_Prompt; d_Prompt_prev=d_Prompt;
d_FLL_wait=1; d_FLL_wait=1;
} }
// Compute PLL error // Compute PLL error
PLL_discriminator_hz=pll_cloop_two_quadrant_atan(d_Prompt)/(float)TWO_PI; PLL_discriminator_hz=pll_cloop_two_quadrant_atan(d_Prompt)/(float)TWO_PI;
/*! /*!
* \todo Update FLL assistance algorithm! * \todo Update FLL assistance algorithm!
*/ */
if (((float)d_sample_counter-(float)d_acq_sample_stamp)/(float)d_fs_in>3) if (((float)d_sample_counter-(float)d_acq_sample_stamp)/(float)d_fs_in>3)
{ {
d_FLL_discriminator_hz=0; //disconnect the FLL after the initial lock d_FLL_discriminator_hz=0; //disconnect the FLL after the initial lock
} }
/*! /*!
* DLL and FLL+PLL filter and get current carrier Doppler and code frequency * DLL and FLL+PLL filter and get current carrier Doppler and code frequency
*/ */
carr_nco_hz=d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz,PLL_discriminator_hz,correlation_time_s); carr_nco_hz=d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz,PLL_discriminator_hz,correlation_time_s);
d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz; d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz;
d_code_freq_hz= GPS_L1_CA_CODE_RATE_HZ- (((d_carrier_doppler_hz - (float)d_if_freq)*GPS_L1_CA_CODE_RATE_HZ)/GPS_L1_FREQ_HZ)-code_error_chips; d_code_freq_hz= GPS_L1_CA_CODE_RATE_HZ- (((d_carrier_doppler_hz - (float)d_if_freq)*GPS_L1_CA_CODE_RATE_HZ)/GPS_L1_FREQ_HZ)-code_error_chips;
/*! /*!
* \todo Improve the lock detection algorithm! * \todo Improve the lock detection algorithm!
*/ */
// ####### CN0 ESTIMATION AND LOCK DETECTORS ###### // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES) if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
{ {
// fill buffer with prompt correlator output values // fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt; d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt;
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
}else{ }else{
d_cn0_estimation_counter=0; d_cn0_estimation_counter=0;
d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in); d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in);
d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES); d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
// ###### TRACKING UNLOCK NOTIFICATION ##### // ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message; int tracking_message;
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>MINIMUM_VALID_CN0) if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>MINIMUM_VALID_CN0)
{ {
d_carrier_lock_fail_counter++; d_carrier_lock_fail_counter++;
}else{ }else{
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
} }
if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER) if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER)
{ {
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n"; std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
tracking_message=3; //loss of lock tracking_message=3; //loss of lock
d_channel_internal_queue->push(tracking_message); d_channel_internal_queue->push(tracking_message);
d_carrier_lock_fail_counter=0; d_carrier_lock_fail_counter=0;
d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
} }
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n"; //std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
} }
/*! /*!
* \todo Output the CN0 * \todo Output the CN0
*/ */
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
// Output channel 0: Prompt correlator output Q // Output channel 0: Prompt correlator output Q
*out[0]=(double)d_Prompt.real(); *out[0]=(double)d_Prompt.real();
// Output channel 1: Prompt correlator output I // Output channel 1: Prompt correlator output I
*out[1]=(double)d_Prompt.imag(); *out[1]=(double)d_Prompt.imag();
// Output channel 2: PRN absolute delay [s] // Output channel 2: PRN absolute delay [s]
*out[2]=d_sample_counter_seconds; *out[2]=d_sample_counter_seconds;
// Output channel 3: d_acc_carrier_phase_rad [rad] // Output channel 3: d_acc_carrier_phase_rad [rad]
*out[3]=(double)d_acc_carrier_phase_rad; *out[3]=(double)d_acc_carrier_phase_rad;
// Output channel 4: PRN code phase [s] // Output channel 4: PRN code phase [s]
*out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in); *out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in);
// ########## DEBUG OUTPUT // ########## DEBUG OUTPUT
/*! /*!
* \todo The stop timer has to be moved to the signal source! * \todo The stop timer has to be moved to the signal source!
*/ */
// debug: Second counter in channel 0 // debug: Second counter in channel 0
if (d_channel==0) if (d_channel==0)
{ {
if (floor(d_sample_counter/d_fs_in)!=d_last_seg) if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{ {
d_last_seg=floor(d_sample_counter/d_fs_in); d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"Current input signal time="<<d_last_seg<<" [s]"<<std::endl; std::cout<<"Current input signal time="<<d_last_seg<<" [s]"<<std::endl;
std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl; std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl; //std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock! //if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
} }
}else }else
{ {
if (floor(d_sample_counter/d_fs_in)!=d_last_seg) if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{ {
d_last_seg=floor(d_sample_counter/d_fs_in); d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl; std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl; //std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
} }
} }
//predict the next loop PRN period length prediction //predict the next loop PRN period length prediction
float T_chip_seconds; float T_chip_seconds;
float T_prn_seconds; float T_prn_seconds;
float T_prn_samples; float T_prn_samples;
float K_blk_samples; float K_blk_samples;
T_chip_seconds=1/d_code_freq_hz; T_chip_seconds=1/d_code_freq_hz;
T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples=T_prn_seconds*(float)d_fs_in; T_prn_samples=T_prn_seconds*(float)d_fs_in;
d_rem_code_phase_samples=d_next_rem_code_phase_samples; d_rem_code_phase_samples=d_next_rem_code_phase_samples;
K_blk_samples=T_prn_samples+d_rem_code_phase_samples; K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
// Update the current PRN delay (code phase in samples) // Update the current PRN delay (code phase in samples)
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ; float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in; float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in;
d_code_phase_samples=d_code_phase_samples+T_prn_samples-T_prn_true_samples; d_code_phase_samples=d_code_phase_samples+T_prn_samples-T_prn_true_samples;
if (d_code_phase_samples<0) if (d_code_phase_samples<0)
{ {
d_code_phase_samples=T_prn_true_samples+d_code_phase_samples; d_code_phase_samples=T_prn_true_samples+d_code_phase_samples;
} }
d_code_phase_samples=fmod(d_code_phase_samples,T_prn_true_samples); d_code_phase_samples=fmod(d_code_phase_samples,T_prn_true_samples);
d_next_prn_length_samples=round(K_blk_samples);//round to a discrete samples d_next_prn_length_samples=round(K_blk_samples);//round to a discrete samples
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples; //rounding error d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples; //rounding error
}else{ }else{
double **out = (double **) &output_items[0]; //block output streams pointer double **out = (double **) &output_items[0]; //block output streams pointer
*out[0]=0; *out[0]=0;
*out[1]=0; *out[1]=0;
*out[2]=0; *out[2]=0;
*out[3]=0; *out[3]=0;
*out[4]=0; *out[4]=0;
} }
if(d_dump) { if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I; float prompt_I;
float prompt_Q; float prompt_Q;
float tmp_E,tmp_P,tmp_L; float tmp_E,tmp_P,tmp_L;
float tmp_float; float tmp_float;
prompt_I=d_Prompt.imag(); prompt_I=d_Prompt.imag();
prompt_Q=d_Prompt.real(); prompt_Q=d_Prompt.real();
tmp_E=std::abs<float>(d_Early); tmp_E=std::abs<float>(d_Early);
tmp_P=std::abs<float>(d_Prompt); tmp_P=std::abs<float>(d_Prompt);
tmp_L=std::abs<float>(d_Late); tmp_L=std::abs<float>(d_Late);
try { try {
// EPR // EPR
d_dump_file.write((char*)&tmp_E, sizeof(float)); d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float)); d_dump_file.write((char*)&tmp_P, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float)); d_dump_file.write((char*)&tmp_L, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols) // PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float)); d_dump_file.write((char*)&prompt_I, sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float)); d_dump_file.write((char*)&prompt_Q, sizeof(float));
// PRN start sample stamp // PRN start sample stamp
//tmp_float=(float)d_sample_counter; //tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// accumulated carrier phase // accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float)); d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
// carrier and code frequency // carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float)); d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float)); d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
//PLL commands //PLL commands
d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float)); d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float));
d_dump_file.write((char*)&carr_nco_hz, sizeof(float)); d_dump_file.write((char*)&carr_nco_hz, sizeof(float));
//DLL commands //DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float)); d_dump_file.write((char*)&code_error_chips, sizeof(float));
d_dump_file.write((char*)&d_code_phase_samples, sizeof(float)); d_dump_file.write((char*)&d_code_phase_samples, sizeof(float));
// CN0 and carrier lock test // CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float)); d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float)); d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float=0; tmp_float=0;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double)); d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
} }
catch (std::ifstream::failure e) { catch (std::ifstream::failure e) {
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n"; std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
} }
} }
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter_seconds = d_sample_counter_seconds + (((double)d_current_prn_length_samples)/(double)d_fs_in); d_sample_counter_seconds = d_sample_counter_seconds + (((double)d_current_prn_length_samples)/(double)d_fs_in);
d_sample_counter+=d_current_prn_length_samples; //count for the processed samples d_sample_counter+=d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
} }
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_code_phase(float code_phase) { void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
d_acq_code_phase_samples=code_phase; d_acq_code_phase_samples=code_phase;
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples; LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
} }
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_doppler(float doppler) { void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_doppler(float doppler) {
d_acq_carrier_doppler_hz = doppler; d_acq_carrier_doppler_hz = doppler;
LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz; LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
} }
void gps_l1_ca_dll_fll_pll_tracking_cc::set_satellite(unsigned int satellite) { void gps_l1_ca_dll_fll_pll_tracking_cc::set_satellite(unsigned int satellite) {
d_satellite = satellite; d_satellite = satellite;
LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite; LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite;
} }
void gps_l1_ca_dll_fll_pll_tracking_cc::set_channel(unsigned int channel) { void gps_l1_ca_dll_fll_pll_tracking_cc::set_channel(unsigned int channel) {
d_channel = channel; d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel; LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump==true) if (d_dump==true)
{ {
if (d_dump_file.is_open()==false) if (d_dump_file.is_open()==false)
{ {
try { try {
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel)); d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat"); d_dump_filename.append(".dat");
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl; std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
} }
catch (std::ifstream::failure e) { catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n"; std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
} }
} }
} }
} }
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp) void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)

99
src/core/system_parameters/gps_navigation_message.cc
View File

@ -1,6 +1,6 @@
/*! /*!
* \file gps_navigation_message.cc * \file gps_navigation_message.cc
* \brief Implementation of a GPS NAV Data message decoder * \brief Implementation of a GPS NAV Data message decoder as described in IS-GPS-200E
* *
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
* \author Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2011. jarribas(at)cttc.es
@ -31,6 +31,8 @@
*/ */
#include "gps_navigation_message.h" #include "gps_navigation_message.h"
#include <math.h>
#include "boost/date_time/posix_time/posix_time.hpp"
#define num_of_slices(x) sizeof(x)/sizeof(bits_slice) #define num_of_slices(x) sizeof(x)/sizeof(bits_slice)
@ -252,11 +254,13 @@ void gps_navigation_message::master_clock(double transmitTime)
*/ */
// 20.3.3.3.3.1 User Algorithm for SV Clock Correction. // 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
void gps_navigation_message::sv_clock_correction(double transmitTime) double gps_navigation_message::sv_clock_correction(double transmitTime)
{ {
double dt; double dt;
dt = check_t(transmitTime - d_Toc); dt = check_t(transmitTime - d_Toc);
d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr; d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr;
double correctedTime = transmitTime - d_satClkCorr;
return correctedTime;
} }
@ -285,18 +289,19 @@ void gps_navigation_message::satellitePosition(double transmitTime)
// Restore semi-major axis // Restore semi-major axis
a = d_sqrt_A*d_sqrt_A; a = d_sqrt_A*d_sqrt_A;
// Time correction // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe); tk = check_t(transmitTime - d_Toe);
// Initial mean motion // Computed mean motion
n0 = sqrt(GM / (a*a*a)); n0 = sqrt(GM / (a*a*a));
// Mean motion
// Corrected mean motion
n = n0 + d_Delta_n; n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 360 deg
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2*GPS_PI),(2*GPS_PI)); M = fmod((M + 2*GPS_PI),(2*GPS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
@ -318,12 +323,12 @@ void gps_navigation_message::satellitePosition(double transmitTime)
// Compute relativistic correction term // Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E); d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
// Calculate the true anomaly // Compute the true anomaly
double tmp_Y=sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E); double tmp_Y=sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E);
double tmp_X=cos(E)-d_e_eccentricity; double tmp_X=cos(E)-d_e_eccentricity;
nu = atan2(tmp_Y, tmp_X); nu = atan2(tmp_Y, tmp_X);
// Compute angle phi // Compute angle phi (argument of Latitude)
phi = nu + d_OMEGA; phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
@ -337,10 +342,11 @@ void gps_navigation_message::satellitePosition(double transmitTime)
// Correct inclination // Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) +d_Cis * sin(2*phi); i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi);
// Compute the angle between the ascending node and the Greenwich meridian // Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe; Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2*GPS_PI),(2*GPS_PI)); Omega = fmod((Omega + 2*GPS_PI),(2*GPS_PI));
@ -359,7 +365,7 @@ void gps_navigation_message::satellitePosition(double transmitTime)
} }
*/ */
// --- Compute satellite coordinates ------------------------------------ // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega); d_satpos_X = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega);
d_satpos_Y = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega); d_satpos_Y = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega);
d_satpos_Z = sin(u)*r * sin(i); d_satpos_Z = sin(u)*r * sin(i);
@ -654,6 +660,79 @@ int gps_navigation_message::subframe_decoder(char *subframe)
} }
double gps_navigation_message::utc_time(double gpstime_corrected)
{
double t_utc;
double t_utc_daytime;
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
// Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF-i_GPS_week;
if ((weeksToLeapSecondEvent) >= 0) // is not in the past
{
//Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
int secondOfLeapSecondEvent = i_DN * 24 * 60 * 60;
if (weeksToLeapSecondEvent > 0)
{
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400);
}
else //we are in the same week than the leap second event
{
if (abs(gpstime_corrected-secondOfLeapSecondEvent) > 21600)
{
/* 20.3.3.5.2.4a
* Whenever the effectivity time indicated by the WN_LSF and the DN values
* is not in the past (relative to the user's present time), and the user's
* present time does not fall in the time span which starts at six hours prior
* to the effectivity time and ends at six hours after the effectivity time,
* the UTC/GPS-time relationship is given by
*/
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400);
}
else
{
/* 20.3.3.5.2.4b
* Whenever the user's current time falls within the time span of six hours
* prior to the effectivity time to six hours after the effectivity time,
* proper accommodation of the leap second event with a possible week number
* transition is provided by the following expression for UTC:
*/
int W = fmod(gpstime_corrected-Delta_t_UTC-43200,86400)+43200;
t_utc_daytime =fmod(W,86400+d_DeltaT_LSF-d_DeltaT_LS);
//implement something to handle a leap second event!
}
if ( (gpstime_corrected - secondOfLeapSecondEvent ) > 21600)
{
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400);
}
}
}
else // the effectivity time is in the past
{
/* 20.3.3.5.2.4c
* Whenever the effectivity time of the leap second event, as indicated by the
* WNLSF and DN values, is in the "past" (relative to the user's current time),
* and the userÕs current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400);
}
double secondsOfWeekBeforeToday= 43200*floor(gpstime_corrected/43200);
t_utc = secondsOfWeekBeforeToday+t_utc_daytime;
return t_utc;
}

20
src/core/system_parameters/gps_navigation_message.h
View File

@ -118,7 +118,7 @@ public:
// Almanac // Almanac
double d_Toa; //!< Almanac reference time [s] double d_Toa; //!< Almanac reference time [s]
int i_WN_A; //!< Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced int i_WN_A; //!< Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced
std::map<int,int> almanacHealth; std::map<int,int> almanacHealth; //!< Map that stores the health information stored in the almanac
// Flags // Flags
@ -139,7 +139,7 @@ public:
// clock terms // clock terms
//double d_master_clock; // GPS transmission time //double d_master_clock; // GPS transmission time
double d_satClkCorr; // GPS clock error double d_satClkCorr; // GPS clock error
double d_dtr; // relativistic clock correction term double d_dtr; // relativistic clock correction term
@ -187,12 +187,6 @@ public:
*/ */
int subframe_decoder(char *subframe); int subframe_decoder(char *subframe);
/*
* User Algorithm for SV Clock Correction
*
* Implementation of paragraph 20.3.3.3.3.1 (IS-GPS-200E)
*/
//void master_clock(double transmitTime);
/*! /*!
* \brief Computes the position of the satellite * \brief Computes the position of the satellite
@ -202,10 +196,16 @@ public:
void satellitePosition(double transmitTime); void satellitePosition(double transmitTime);
/*! /*!
* \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction (IS-GPS-200E, 20.3.3.3.3.1) * \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction
* and returns the corrected clock (IS-GPS-200E, 20.3.3.3.3.1)
*/ */
void sv_clock_correction(double transmitTime); double sv_clock_correction(double transmitTime);
/*!
* \brief Computes the Coordinated Universal Time (UTC) and
* returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4)
*/
double utc_time(double gpstime_corrected);
bool satellite_validation(); bool satellite_validation();

8
src/main/main.cc
View File

@ -62,7 +62,7 @@ int main(int argc, char** argv)
const std::string intro_help( const std::string intro_help(
std::string("\nGNSS-SDR is an Open Source GNSS Software Defined Receiver\n") std::string("\nGNSS-SDR is an Open Source GNSS Software Defined Receiver\n")
+ +
"Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)\n" "Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)\n"
+ +
"This program comes with ABSOLUTELY NO WARRANTY;\n" "This program comes with ABSOLUTELY NO WARRANTY;\n"
+ +
@ -78,9 +78,9 @@ int main(int argc, char** argv)
google::InitGoogleLogging(argv[0]); google::InitGoogleLogging(argv[0]);
if (FLAGS_log_dir.empty()) if (FLAGS_log_dir.empty())
{ {
// temp_directory_path() is only available from Boost 1.45. Ubuntu 10.10 ships with 1.42 // temp_directory_path() is only available from Boost 1.45. Ubuntu 10.10 ships with 1.42
//std::cout << "Logging will be done at " << boost::filesystem::temp_directory_path() << std::endl // std::cout << "Logging will be done at " << boost::filesystem::temp_directory_path() << std::endl
// << "Use gnss-sdr --log_dir=/path/to/log to change that."<< std::endl; // << "Use gnss-sdr --log_dir=/path/to/log to change that."<< std::endl;
} }
else else
{ {