mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Added J.Arribas contributions: PVT with basic least squares and rinex 2.1 output is now enabled, tracking channels now estimate the CN0 and performs a basic carrier lock detector and returns to acquisition if the tracking loss the lock. 

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@75 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas 2011-10-28 15:01:46 +00:00
parent 0069bef236
commit 1040e6865d
25 changed files with 961 additions and 373 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
conf/mercurio.conf
View File

@ -7,11 +7,11 @@ ControlThread.wait_for_flowgraph=false
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=File_Signal_Source
;SignalSource.filename=/media/My Passport/spirent scenario 2/data/sc2_d8.dat
;SignalSource.filename=/home/luis/Project/signals/GPS_cap1.dat
SignalSource.filename=/media/My Passport/KINGSTON (G)/Project Luis/GPSL1_Fs_8MHz_ID_1_CN0_60.dat
;SignalSource.filename=/media/DATALOGGER/signals/spirent scenario 2/data/sc2_d8.dat
;SignalSource.filename=/media/My Passport/KINGSTON (G)/Project Luis/GPSL1_Fs_8MHz_ID_1_CN0_60.dat
SignalSource.filename=/media/DATALOGGER/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
SignalSource.item_type=gr_complex
SignalSource.sampling_frequency=8000000
SignalSource.sampling_frequency=4000000
SignalSource.samples=0
SignalSource.repeat=false
SignalSource.dump=false
@ -20,58 +20,72 @@ SignalSource.enable_throttle_control=true
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Pass_Through
SignalConditioner.item_type=gr_complex
SignalConditioner.sample_freq_in=8000000
SignalConditioner.sample_freq_out=8000000
SignalConditioner.sample_freq_in=4000000
SignalConditioner.sample_freq_out=4000000
SignalConditioner.dump=false
;######### CHANNELS CONFIGURATION CONFIG ############
Channels.count=1
Channels.count=6
;######### ACQUISITION CONFIG ############
Acquisition.dump=false
Acquisition.dump_filename=./acq_dump.dat
Acquisition.item_type=gr_complex
Acquisition.fs_in=8000000
Acquisition.fs_in=4000000
Acquisition.if=0
Acquisition.sampled_ms=1
;######### ACQUISITION 0 CONFIG ############
Acquisition0.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition0.threshold=0.006
Acquisition0.threshold=0.06
Acquisition0.doppler_max=10000
Acquisition0.doppler_step=250
Acquisition0.satellite=1
Acquisition0.repeat_satellite=true
Acquisition0.doppler_step=500
Acquisition0.satellite=11
Acquisition0.repeat_satellite=false
;######### ACQUISITION 1 CONFIG ############
Acquisition1.implementation=GPS_L1_CA_GPS_SDR_Acquisition
Acquisition1.threshold=30
Acquisition1.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition1.threshold=0.06
Acquisition1.doppler_max=10000
Acquisition1.doppler_step=250
Acquisition1.satellite=16
Acquisition1.repeat_satellite=true
Acquisition1.doppler_step=500
Acquisition1.repeat_satellite=false
;######### ACQUISITION 2 CONFIG ############
Acquisition2.implementation=GPS_L1_CA_TONG_PCPS_Acquisition
Acquisition2.threshold=
Acquisition2.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition2.threshold=0.06
Acquisition2.doppler_max=10000
Acquisition2.doppler_step=250
Acquisition2.satellite=21
Acquisition2.repeat_satellite=true
Acquisition2.doppler_step=500
Acquisition2.repeat_satellite=false
;######### ACQUISITION 3 CONFIG ############
Acquisition3.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition3.threshold=105000
Acquisition3.threshold=0.06
Acquisition3.doppler_max=10000
Acquisition3.doppler_step=500
Acquisition3.repeat_satellite=false
;######### ACQUISITION 3 CONFIG ############
Acquisition4.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition4.threshold=0.06
Acquisition4.doppler_max=10000
Acquisition4.doppler_step=500
Acquisition4.repeat_satellite=false
;######### ACQUISITION 3 CONFIG ############
Acquisition5.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition5.threshold=0.06
Acquisition5.doppler_max=10000
Acquisition5.doppler_step=500
Acquisition5.repeat_satellite=false
;######### TRACKING CONFIG ############
Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
Tracking.item_type=gr_complex
Tracking.vector_length=8000
Tracking.fs_in=8000000
Tracking.vector_length=4000
Tracking.fs_in=4000000
Tracking.if=0
Tracking.dump=true
Tracking.dump_filename=./trk_dump.dat
@ -82,7 +96,7 @@ TelemetryDecoder.item_type=gr_complex
;######### OBSERVABLES CONFIG ############
Observables.implementation=GPS_L1_CA_Observables
Observables.fs_in=8000000;
Observables.fs_in=4000000;
;######### PVT CONFIG ############
PVT.implementation=Pass_Through

4
jamroot.jam
View File

@ -8,10 +8,12 @@ lib gnuradio-usrp ;
lib profiler ;
lib gsl ;
lib gslcblas ;
lib itpp ;
project : requirements
<define>OMNITHREAD_POSIX
<cxxflags>"-std=c++0x"
<cxxflags>"-std=c++0x `pkg-config --cflags itpp`"
<linkflags>"`pkg-config --libs itpp`"
# <include>src/utils
# <include>src/utils/INIReader
<include>src/algorithms/acquisition/adapters

25
src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_pcps_acquisition_cc.cc
View File

@ -195,6 +195,7 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items,
int doppler;
unsigned int indext = 0;
float magt = 0.0;
float tmp_magt = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
bool positive_acquisition = false;
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
@ -215,8 +216,8 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items,
{
d_input_power += std::abs(in[i]) * std::abs(in[i]);
}
d_input_power = d_input_power / ((float)d_fft_size
* (float)d_fft_size);
//d_input_power = d_input_power / ((float)d_fft_size * (float)d_fft_size);
d_input_power = d_input_power / (float)d_fft_size;
// 2º- Doppler frequency search loop
for (doppler = (int)(-d_doppler_max); doppler < (int)d_doppler_max; doppler
@ -238,11 +239,23 @@ int gps_l1_ca_pcps_acquisition_cc::general_work(int noutput_items,
}
d_ifft->execute();
x86_gr_complex_mag(d_ifft->get_outbuf(), d_fft_size); // d_ifft->get_outbuf()=|abs(·)|^2 and the array is converted from CPX->Float
x86_float_max((float*)d_ifft->get_outbuf(), &indext, &magt,
d_fft_size); // find max of |abs(·)|^2 -> index and magt
// ASM accelerated version
//x86_gr_complex_mag(d_ifft->get_outbuf(), d_fft_size); // d_ifft->get_outbuf()=|abs(·)|^2 and the array is converted from CPX->Float
//x86_float_max((float*)d_ifft->get_outbuf(), &indext, &magt,
// d_fft_size); // find max of |abs(·)|^2 -> index and magt
magt = magt / (float)d_fft_size;
// C++ version
indext=0;
magt=0;
for (i = 0; i < d_fft_size; i++)
{
tmp_magt=std::norm(d_ifft->get_outbuf()[i]);
if (tmp_magt>magt){
magt=tmp_magt;
indext=i;
}
}
//magt = magt / (float)d_fft_size;
// Record results to files
if (d_dump)
{

2
src/algorithms/channel/adapters/channel.cc
View File

@ -128,6 +128,8 @@ void Channel::connect(gr_top_block_sptr top_block)
top_block->connect(trk_->get_right_block(), 0, nav_->get_left_block(), 0); // channel 1
top_block->connect(trk_->get_right_block(), 1, nav_->get_left_block(), 1); // channel 2
top_block->connect(trk_->get_right_block(), 2, nav_->get_left_block(), 2); // channel 3
top_block->connect(trk_->get_right_block(), 3, nav_->get_left_block(), 3); // channel 4
top_block->connect(trk_->get_right_block(), 4, nav_->get_left_block(), 4); // channel 5
DLOG(INFO) << "tracking -> telemetry_decoder";
connected_ = true;

7
src/algorithms/observables/adapters/gps_l1_ca_observables.cc
View File

@ -45,13 +45,6 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
/* The extern keyword declares a variable or function and specifies
* that it has external linkage (its name is visible from files other
* than the one in which it's defined). When modifying a variable,
* extern specifies that the variable has static duration (it is allocated
* when the program begins and deallocated when the program ends).
* The variable is defined in main.cc
*/
extern concurrent_queue<gps_navigation_message> global_gps_nav_msg_queue;
using google::LogMessage;

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

@ -52,18 +52,34 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
d_dump = dump;
d_nchannels = nchannels;
d_rinex_printer.set_headers("GNSS-SDR"); //TODO: read filename from config
d_ls_pvt=new gps_l1_ca_ls_pvt(nchannels);
d_ephemeris_clock_s=0.0;
if (d_dump==true)
{
std::stringstream d_dump_filename_str;//create a stringstream to form the dump filename
d_dump_filename_str<<"./data/obs.dat"; //TODO: get filename and path from config in the adapter
d_dump_filename=d_dump_filename_str.str();
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
}
}
gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc() {
delete d_ls_pvt;
}
bool pairCompare( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
bool pairCompare_min( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
{
return a.second.last_preamble_index < b.second.last_preamble_index;
return (a.second.preamble_delay_ms+a.second.prn_delay_ms) < (b.second.preamble_delay_ms+b.second.prn_delay_ms);
}
bool pairCompare_max( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
{
return (a.second.preamble_delay_ms+a.second.prn_delay_ms) > (b.second.preamble_delay_ms+b.second.prn_delay_ms);
}
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) {
@ -71,7 +87,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
// CONSTANTS TODO: place in a file
const float code_length_s=0.001; //1 ms
const float C_m_ms= GPS_C_m_s/1000; // The speed of light, [m/ms]
const float C_m_ms= GPS_C_m_s/1000.0; // The speed of light, [m/ms]
const float nav_sol_period_ms=1000;
//--- Find number of samples per spreading code ----------------------------
@ -84,46 +100,60 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
map<int,float> pseudoranges;
map<int,float>::iterator pseudoranges_iter;
map<int,float> pseudoranges_dump;
float min_preamble_delay_ms;
float max_preamble_delay_ms;
bool flag_valid_pseudoranges=false;
unsigned long int min_preamble_index;
float prn_delay_ms;
float pseudoranges_timestamp_ms;
float traveltime_ms;
float pseudorange_m;
int pseudoranges_reference_sat_ID=0;
for (unsigned int i=0; i<d_nchannels ; i++)
{
if (in[i][0].valid_word) //if this channel have valid word
{
gps_words.insert(pair<int,gnss_synchro>(in[i][0].satellite_PRN,in[i][0])); //record the word structure in a map for pseudoranges
gps_words.insert(pair<int,gnss_synchro>(in[i][0].channel_ID,in[i][0])); //record the word structure in a map for pseudoranges
}
}
if(gps_words.size()>0)
{
// find the minimum index (nearest satellite, will be the reference)
gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare);
min_preamble_index=gps_words_iter->second.last_preamble_index;
//compute the pseudoranges
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{
prn_delay_ms=gps_words_iter->second.prn_delay/(float)samples_per_code;
traveltime_ms=(float)(1000*(gps_words_iter->second.last_preamble_index-min_preamble_index))/(float)samples_per_code+GPS_STARTOFFSET_ms+prn_delay_ms;
//cout<<"traveltime ms"<<gps_words_iter->first<<" ="<<traveltime_ms<<endl;
pseudorange_m=traveltime_ms*C_m_ms;
pseudoranges.insert(pair<int,float>(gps_words_iter->first,pseudorange_m)); //record the preamble index in a map
}
// find the minimum index (nearest satellite, will be the reference)
gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare_min);
min_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms+gps_words_iter->second.prn_delay_ms; //[ms]
pseudoranges_timestamp_ms=min_preamble_delay_ms; //save the shortest pseudorange timestamp to compute the RINEX timestamp
pseudoranges_reference_sat_ID=gps_words_iter->second.satellite_PRN;
gps_words_iter=max_element(gps_words.begin(),gps_words.end(),pairCompare_max);
max_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; //[ms]
if ((max_preamble_delay_ms-min_preamble_delay_ms)<70) flag_valid_pseudoranges=true;
//compute the pseudoranges
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{
std::cout<<"prn_delay="<<gps_words_iter->second.prn_delay_ms<<std::endl;
traveltime_ms=gps_words_iter->second.preamble_delay_ms+gps_words_iter->second.prn_delay_ms-min_preamble_delay_ms+GPS_STARTOFFSET_ms; //[ms]
pseudorange_m=traveltime_ms*C_m_ms; // [m]
pseudoranges.insert(pair<int,float>(gps_words_iter->second.satellite_PRN,pseudorange_m)); //record the preamble index in a map
if (d_dump==true)
{
pseudoranges_dump.insert(pair<int,float>(gps_words_iter->second.channel_ID,pseudorange_m));
}
}
// write the pseudoranges to RINEX OBS file
// 1- need a valid clock
if (d_last_nav_msg.d_satellite_PRN>0)
if (flag_valid_pseudoranges==true and d_last_nav_msg.d_satellite_PRN>0)
{
std::cout<<"d_inter_frame_sec_counter="<<d_inter_frame_sec_counter<<std::endl;
d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_inter_frame_sec_counter,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
d_ls_pvt->get_PVT(pseudoranges,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0);
}
d_inter_frame_sec_counter+=((float)NAVIGATION_OUTPUT_RATE_MS)/1000.0; //TODO: synchronize the gps time of the ephemeris with the obs
}
//debug
@ -134,29 +164,36 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{
cout<<"Pseudoranges =("<<pseudoranges_iter->first<<","<<pseudoranges_iter->second<<")"<<endl;
}
gps_navigation_message nav_msg;
if (d_nav_queue->try_pop(nav_msg)==true)
{
cout<<"New ephemeris record has arrived!"<<endl;
cout<<"d_channel_ID="<<nav_msg.d_channel_ID<<endl;
cout<<"d_satellite_PRN="<<nav_msg.d_satellite_PRN<<endl;
cout<<"d_satpos_X="<<nav_msg.d_satpos_X<<endl;
cout<<"d_satpos_Y="<<nav_msg.d_satpos_Y<<endl;
cout<<"d_satpos_Z="<<nav_msg.d_satpos_Z<<endl;
cout<<"d_master_clock="<<nav_msg.d_master_clock<<endl;
cout<<"d_satClkCorr="<<nav_msg.d_satClkCorr<<endl;
cout<<"d_dtr="<<nav_msg.d_dtr<<endl;
// write ephemeris to RINES NAV file
// TODO: check operation ok
cout<<"New ephemeris record has arrived from SAT ID "<<nav_msg.d_satellite_PRN<<endl;
d_last_nav_msg=nav_msg;
d_inter_frame_sec_counter=0; //reset the interframe seconds counter
d_rinex_printer.LogRinex2Nav(nav_msg);
d_ls_pvt->d_ephemeris[nav_msg.d_channel_ID]=nav_msg;
// **** update pseudoranges clock ****
if (nav_msg.d_satellite_PRN==pseudoranges_reference_sat_ID)
{
d_ephemeris_clock_s=d_last_nav_msg.d_TOW;
d_ephemeris_timestamp_ms=d_last_nav_msg.d_subframe1_timestamp_ms;
}
// **** write ephemeris to RINES NAV file
//d_rinex_printer.LogRinex2Nav(nav_msg);
}
if (d_dump==true)
{
float tmp_float=0.0;
for (int i=0;i<d_nchannels;i++)
{
pseudoranges_iter=pseudoranges_dump.find(i);
if (pseudoranges_iter!=pseudoranges_dump.end())
{
d_dump_file.write((char*)&pseudoranges_iter->second, sizeof(float));
}else{
d_dump_file.write((char*)&tmp_float, sizeof(float));
}
}
}
consume_each(1); //one by one
return 0;
}

9
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
View File

@ -24,6 +24,7 @@
#include "gps_navigation_message.h"
#include "rinex_2_1_printer.h"
#include "gps_l1_ca_ls_pvt.h"
#include "GPS_L1_CA.h"
@ -56,10 +57,14 @@ private:
rinex_printer d_rinex_printer; // RINEX printer class
double d_inter_frame_sec_counter; // counter for seconds between GPS frames
gps_navigation_message d_last_nav_msg; //last navigation message
double d_ephemeris_clock_s;
double d_ephemeris_timestamp_ms;
gps_l1_ca_ls_pvt *d_ls_pvt;
public:
~gps_l1_ca_observables_cc ();

316
src/algorithms/observables/libs/gps_l1_ca_ls_pvt.cc Normal file
View File

@ -0,0 +1,316 @@
/**
* Copyright notice
*/
/**
* Author: Javier Arribas, 2011. jarribas(at)cttc.es
*/
#include <itpp/itbase.h>
#include <itpp/stat/misc_stat.h>
#include <itpp/base/matfunc.h>
using namespace itpp;
#include "gps_l1_ca_ls_pvt.h"
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels)
{
// init empty ephemerids for all the available GNSS channels
d_nchannels=nchannels;
d_ephemeris=new gps_navigation_message[nchannels];
}
gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt()
{
delete d_ephemeris;
}
vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, vec X_sat) {
/*
%E_R_CORR Returns rotated satellite ECEF coordinates due to Earth
%rotation during signal travel time
%
%X_sat_rot = e_r_corr(traveltime, X_sat);
%
% Inputs:
% travelTime - signal travel time
% X_sat - satellite's ECEF coordinates
%
% Outputs:
% X_sat_rot - rotated satellite's coordinates (ECEF)
*/
double Omegae_dot = 7.292115147e-5; // rad/sec
//--- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = Omegae_dot * traveltime;
//--- Make a rotation matrix -----------------------------------------------
mat R3=zeros(3,3);
R3(0, 0)= cos(omegatau);
R3(0, 1)= sin(omegatau);
R3(0, 2)= 0.0;
R3(1, 0)=-sin(omegatau);
R3(1, 1)= cos(omegatau);
R3(1, 2)=0.0;
R3(2, 0)= 0.0;
R3(2, 1)= 0.0;
R3(2, 2)= 1;
//--- Do the rotation ------------------------------------------------------
vec X_sat_rot;
X_sat_rot = R3 * X_sat;
return X_sat_rot;
}
vec gps_l1_ca_ls_pvt::leastSquarePos(mat satpos, vec obs, mat w) {
//Function calculates the Least Square Solution.
//pos= leastSquarePos(satpos, obs, w);
// Inputs:
// satpos - Satellites positions (in ECEF system: [X; Y; Z;]
// obs - Observations - the pseudorange measurements to each
// satellite
// w - weigths vector
// Outputs:
// pos - receiver position and receiver clock error
// (in ECEF system: [X, Y, Z, dt])
//////////////////////////////////////////////////////////////////////////
// el - Satellites elevation angles (degrees)
// az - Satellites azimuth angles (degrees)
// dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
//=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config
//double dtr = GPS_PI / 180.0;
int nmbOfSatellites;
nmbOfSatellites = satpos.cols();
vec pos = "0.0 0.0 0.0 0.0";
mat A;
mat omc;
mat az;
mat el;
A=zeros(nmbOfSatellites,4);
omc=zeros(nmbOfSatellites,1);
az=zeros(1,nmbOfSatellites);
el=zeros(1,nmbOfSatellites);
for (int i = 0; i < nmbOfSatellites; i++) {
for (int j = 0; j < 4; j++) {
A.set(i, j, 0.0);
}
omc(i, 0)=0.0;
az(0, i)=0.0;
}
el = az;
mat X = satpos;
vec Rot_X;
double rho2;
double traveltime;
double trop;
mat mat_tmp;
vec x;
//=== Iteratively find receiver position ===================================
for (int iter = 0; iter < nmbOfIterations; iter++) {
for (int i = 0; i < nmbOfSatellites; i++) {
if (iter == 0) {
//--- Initialize variables at the first iteration --------------
Rot_X=X.get_col(i);
trop = 0.0;
} else {
//--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0))*(X(0, i) - pos(0)) + (X(1, i) - pos(1))*(X(1, i) - pos(1))+ (X(2,i) - pos(2))*(X(2,i) - pos(2));
traveltime = sqrt(rho2) / GPS_C_m_s;
//--- Correct satellite position (do to earth rotation) --------
Rot_X = e_r_corr(traveltime, X.get_col(i));
//--- Find the elevation angel of the satellite ----------------
//[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
}
//--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.left(3)) - pos(4) - trop);
//--- Construct the A matrix ---------------------------------------
A.set(i, 0, (-(Rot_X(0) - pos(0))) / obs(i));
A.set(i, 1, (-(Rot_X(1) - pos(1))) / obs(i));
A.set(i, 2, (-(Rot_X(2) - pos(2))) / obs(i));
A.set(i, 3, 1.0);
}
// These lines allow the code to exit gracefully in case of any errors
//if (rank(A) != 4) {
// pos.clear();
// return pos;
//}
//--- Find position update ---------------------------------------------
x = ls_solve_od(w*A,w*omc);
//--- Apply position update --------------------------------------------
pos = pos + x;
}
return pos;
}
void gps_l1_ca_ls_pvt::get_PVT(std::map<int,float> pseudoranges,double GPS_current_time)
{
std::map<int,float>::iterator pseudoranges_iter;
mat satpos;
mat W=eye(d_nchannels); //channels weights matrix
vec obs=zeros(d_nchannels); // pseudoranges observation vector
satpos=zeros(3,d_nchannels); //satellite positions matrix
int valid_obs=0; //valid observations counter
for (int i=0; i<d_nchannels; i++)
{
if (d_ephemeris[i].satellite_validation()==true)
{
pseudoranges_iter=pseudoranges.find(d_ephemeris[i].d_satellite_PRN);
if (pseudoranges_iter!=pseudoranges.end())
{
W(i,i)=1; // TODO: Place here the satellite CN0 (power level, or weight factor)
// compute the GPS master clock
d_ephemeris[i].master_clock(GPS_current_time);
// compute the satellite current ECEF position
d_ephemeris[i].satpos();
// compute the clock error including relativistic effects
d_ephemeris[i].relativistic_clock_correction(GPS_current_time);
satpos(0,i)=d_ephemeris[i].d_satpos_X;
satpos(1,i)=d_ephemeris[i].d_satpos_Y;
satpos(2,i)=d_ephemeris[i].d_satpos_Z;
obs(i)=pseudoranges_iter->second+d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
valid_obs++;
}else{
// no valid pseudorange for the current channel
W(i,i)=0; // channel de-activated
obs(i)=1; // to avoid algorithm problems (divide by zero)
}
}else{
// no valid ephemeris for the current channel
W(i,i)=0; // channel de-activated
obs(i)=1; // to avoid algorithm problems (divide by zero)
}
}
std::cout<<"PVT: valid observations="<<valid_obs<<std::endl;
if (valid_obs>=4)
{
vec mypos;
mypos=leastSquarePos(satpos,obs,W);
std::cout << "Position ("<<GPS_current_time<<") ECEF = " << mypos << std::endl;
cart2geo(mypos(0), mypos(1), mypos(2), 4);
std::cout << "Position ("<<GPS_current_time<<") Lat = " << d_latitude_d << " Long ="<< d_longitude_d <<" Height="<<d_height_m<< std::endl;
}
}
void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
//function [phi, lambda, h] = cart2geo(X, Y, Z, i)
//CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
//coordinates (phi, lambda, h) on a selected reference ellipsoid.
//
//[phi, lambda, h] = cart2geo(X, Y, Z, i);
//
// Choices i of Reference Ellipsoid for Geographical Coordinates
// 0. International Ellipsoid 1924
// 1. International Ellipsoid 1967
// 2. World Geodetic System 1972
// 3. Geodetic Reference System 1980
// 4. World Geodetic System 1984
double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137};
double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda;
lambda = atan2(Y,X);
double ex2;
ex2 = (2-f[elipsoid_selection])*f[elipsoid_selection]/((1-f[elipsoid_selection])*(1-f[elipsoid_selection]));
double c;
c = a[elipsoid_selection]*sqrt(1+ex2);
double phi;
phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection]))*f[elipsoid_selection])));
double h = 0.1;
double oldh = 0;
double N;
int iterations = 0;
do{
oldh = h;
N = c/sqrt(1+ex2*(cos(phi)*cos(phi)));
phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection])*f[elipsoid_selection]*N/(N+h)))));
h = sqrt(X*X+Y*Y)/cos(phi)-N;
iterations = iterations + 1;
if (iterations > 100)
{
std::cout<<"Failed to approximate h with desired precision. h-oldh= "<<h-oldh<<std::endl;
break;
}
}while (abs(h-oldh) > 1.0e-12);
d_latitude_d = phi*180.0/GPS_PI;
d_longitude_d = lambda*180/pi;
d_height_m = h;
}
//void gps_l1_ca_ls_pvt::topocent(traveltime, X_sat)
//{
/*
%function [Az, El, D] = topocent(X, dx)
%TOPOCENT Transformation of vector dx into topocentric coordinate
% system with origin at X.
% Both parameters are 3 by 1 vectors.
%
%[Az, El, D] = topocent(X, dx);
%
% Inputs:
% X - vector origin corrdinates (in ECEF system [X; Y; Z;])
% dx - vector ([dX; dY; dZ;]).
%
% Outputs:
% D - vector length. Units like units of the input
% Az - azimuth from north positive clockwise, degrees
% El - elevation angle, degrees
dtr = pi/180;
[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
cl = cos(lambda * dtr);
sl = sin(lambda * dtr);
cb = cos(phi * dtr);
sb = sin(phi * dtr);
F = [-sl -sb*cl cb*cl;
cl -sb*sl cb*sl;
0 cb sb];
local_vector = F' * dx;
E = local_vector(1);
N = local_vector(2);
U = local_vector(3);
hor_dis = sqrt(E^2 + N^2);
if hor_dis < 1.e-20
Az = 0;
El = 90;
else
Az = atan2(E, N)/dtr;
El = atan2(U, hor_dis)/dtr;
end
if Az < 0
Az = Az + 360;
end
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%%
*/
//}

57
src/algorithms/observables/libs/gps_l1_ca_ls_pvt.h Normal file
View File

@ -0,0 +1,57 @@
/**
* Copyright notice
*/
/**
* Author: Javier Arribas, 2011. jarribas(at)cttc.es
* Luis Esteve, 2011. luis(at)epsilon-formacion.com
*/
#ifndef GPS_L1_CA_LS_PVT_H_
#define GPS_L1_CA_LS_PVT_H_
#include <string>
#include <iostream>
#include <sstream>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>
#include <map>
#include <algorithm>
#include "gps_navigation_message.h"
#include "GPS_L1_CA.h"
#include <itpp/itbase.h>
#include <itpp/stat/misc_stat.h>
#include <itpp/base/matfunc.h>
using namespace itpp;
class gps_l1_ca_ls_pvt
{
private:
vec leastSquarePos(mat satpos, vec obs, mat w);
vec e_r_corr(double traveltime, vec X_sat);
//void cart2geo();
//void topocent();
public:
int d_nchannels;
gps_navigation_message* d_ephemeris;
double d_pseudoranges_time_ms;
double d_latitude_d;
double d_longitude_d;
double d_height_m;
double d_x_m;
double d_y_m;
double d_z_m;
gps_l1_ca_ls_pvt(int nchannels);
~gps_l1_ca_ls_pvt();
void get_PVT(std::map<int,float> pseudoranges,double GPS_current_time);
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
};
#endif

3
src/algorithms/observables/libs/jamfile.jam
View File

@ -1,3 +1,4 @@
project : build-dir ../../../../build ;
obj rinex_2_1_printer : rinex_2_1_printer.cc ;
obj rinex_2_1_printer : rinex_2_1_printer.cc ;
obj gps_l1_ca_ls_pvt : gps_l1_ca_ls_pvt.cc ;

6
src/algorithms/observables/libs/rinex_2_1_printer.cc
View File

@ -192,7 +192,7 @@ void rinex_printer::Rinex2ObsHeader()
}
void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double interframe_seconds, std::map<int,float> pseudoranges)
void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double pseudoranges_clock, std::map<int,float> pseudoranges)
{
int ss;
char sat_vis[36];
@ -224,7 +224,7 @@ void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double interfram
double decimalday,daydecimalhour,decimalhour,decimalmin,decimalsec;
double day,hour,minutes,seconds,enterseconds,a;
double gpstime;
gpstime=nav_msg.d_master_clock;
gpstime=pseudoranges_clock; //[s]
//calculate date of gps time:
//Days & weeks between 00h 1 Jan 1970 and 00h 6 Jan 1980
//520 weeks and 12 days.
@ -233,7 +233,7 @@ void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double interfram
tmPtr = gmtime(&temps);
strftime( cad1, 20, " %y %m %d", tmPtr );
strftime( cad2, 20, " %Y %m %d", tmPtr );
decimalday=((gpstime+interframe_seconds)/(24*3600));//Dies dins de la semana
decimalday=(gpstime/(24*3600));//Dies dins de la semana
daydecimalhour=modf(decimalday,&day);//day=#dies sencers, daydecimalhour=porcio de dia
daydecimalhour=daydecimalhour*24;//porcio de dia en hores
decimalhour=modf(daydecimalhour,&hour);//hour=hora del dia; decimalhour=porcio d'hora

7
src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc
View File

@ -45,13 +45,6 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
/* The extern keyword declares a variable or function and specifies
* that it has external linkage (its name is visible from files other
* than the one in which it's defined). When modifying a variable,
* extern specifies that the variable has static duration (it is allocated
* when the program begins and deallocated when the program ends).
* The variable is defined in main.cc
*/
extern concurrent_queue<gps_navigation_message> global_gps_nav_msg_queue;
using google::LogMessage;

26
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -47,7 +47,7 @@ void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items,
gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump) :
gr_block ("gps_navigation_cc", gr_make_io_signature (3, 3, sizeof(float)),
gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(float)),
gr_make_io_signature(1, 1, sizeof(gnss_synchro))) {
// initialize internal vars
d_queue = queue;
@ -106,10 +106,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
d_sample_counter++; //count for the processed samples
const float **in = (const float **) &input_items[0]; //Get the input samples pointer
//std::cout<<"CH1="<<in[0][0]; // Q
//std::cout<<"CH2="<<in[1][0]; // I
//std::cout<<"CH3="<<in[2][0]; // Delay
//std::cout<<"N_input_streams="<<ninput_items.size();
// TODO Optimize me!
//******* preamble correlation ********
@ -123,13 +119,13 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
}
//******* frame sync ******************
if (abs(corr_value)>=153){
if (abs(corr_value)>=160){
//TODO: Rewrite with state machine
if (d_stat==0)
{
d_GPS_FSM.Event_gps_word_preamble();
d_preamble_index=d_sample_counter;//record the preamble sample stamp
std::cout<<"Pre-detection"<<std::endl;
std::cout<<"Pre-detection SAT "<<this->d_satellite+1<<std::endl;
d_symbol_accumulator=0; //sync the symbol to bits integrator
d_symbol_accumulator_counter=0;
d_frame_bit_index=8;
@ -140,15 +136,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if (abs(preamble_diff-6000)<1)
{
d_GPS_FSM.Event_gps_word_preamble();
d_preamble_index=d_sample_counter;//record the preamble sample stamp
d_preamble_index=d_sample_counter;//record the preamble sample stamp (t_P)
d_preamble_phase=in[2][0]; //record the PRN start sample index associated to the preamble
if (!d_flag_frame_sync){
d_flag_frame_sync=true;
std::cout<<" Frame sync with phase "<<in[2][0]<<std::endl;
std::cout<<" Frame sync SAT "<<this->d_satellite+1<<" with preamble start at "<<in[2][0]<<" [ms]"<<std::endl;
}
}else
{
if (preamble_diff>7000){
std::cout<<"lost of frame sync"<<std::endl;
std::cout<<"lost of frame sync SAT "<<this->d_satellite+1<<std::endl;
d_stat=0; //lost of frame sync
d_flag_frame_sync=false;
}
@ -156,6 +154,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
}
}
//******* code error accumulator *****
//d_preamble_phase-=in[3][0];
//******* SYMBOL TO BIT *******
d_symbol_accumulator+=in[1][d_samples_per_bit*8-1]; // accumulate the input value in d_symbol_accumulator
@ -194,6 +194,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
}
if (gps_word_parityCheck(d_GPS_frame_4bytes)) {
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes,&d_GPS_frame_4bytes,sizeof(char)*4);
d_GPS_FSM.d_preamble_time_ms=d_preamble_phase;
d_GPS_FSM.Event_gps_word_valid();
d_flag_parity=true;
}else{
@ -213,8 +214,9 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if ((d_sample_counter%NAVIGATION_OUTPUT_RATE_MS)==0)
{
gps_synchro.valid_word=(d_flag_frame_sync==true and d_flag_parity==true);
gps_synchro.last_preamble_index=d_preamble_index;
gps_synchro.prn_delay=in[2][0];
//gps_synchro.preamble_delay_ms=(float)d_preamble_index;
gps_synchro.preamble_delay_ms=(float)d_preamble_index;
gps_synchro.prn_delay_ms=in[3][0];
gps_synchro.satellite_PRN=d_satellite+1;
gps_synchro.channel_ID=d_channel;
*out[0]=gps_synchro;

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@ -76,6 +76,8 @@ private:
int d_satellite;
int d_channel;
float d_preamble_phase;
std::string d_dump_filename;
std::ofstream d_dump_file;

37
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc
View File

@ -12,7 +12,7 @@ public:
// sc::transition(evento,estado_destino)
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;
//std::cout<<"Enter S0 "<<std::endl;
}
};
struct gps_subframe_fsm_S1: public sc::state<gps_subframe_fsm_S1, GpsL1CaSubframeFsm > {
@ -21,7 +21,7 @@ public:
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;
//std::cout<<"Enter S1 "<<std::endl;
}
};
@ -31,7 +31,7 @@ public:
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;
//std::cout<<"Enter S2 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(0);
}
};
@ -41,7 +41,7 @@ public:
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;
//std::cout<<"Enter S3 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(1);
}
};
@ -51,7 +51,7 @@ public:
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;
//std::cout<<"Enter S4 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(2);
}
};
@ -61,7 +61,7 @@ public:
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;
//std::cout<<"Enter S5 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(3);
}
};
@ -71,7 +71,7 @@ public:
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;
//std::cout<<"Enter S6 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(4);
}
};
@ -81,7 +81,7 @@ public:
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;
//std::cout<<"Enter S7 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(5);
}
};
@ -91,7 +91,7 @@ public:
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;
//std::cout<<"Enter S8 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(6);
}
};
@ -101,7 +101,7 @@ public:
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;
//std::cout<<"Enter S9 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(7);
}
};
@ -111,7 +111,7 @@ public:
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;
//std::cout<<"Enter S10 "<<std::endl;
context< GpsL1CaSubframeFsm >().gps_word_to_subframe(8);
}
};
@ -120,11 +120,11 @@ 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;
//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;
//std::cout<<"Enter S11"<<std::endl;
}
};
@ -149,17 +149,14 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
d_nav.d_satellite_PRN=d_satellite_PRN+1;
d_nav.d_channel_ID=d_channel_ID;
if (subframe_ID==1) {
d_nav.d_subframe1_timestamp_ms=this->d_preamble_time_ms-6002;
std::cout<<"FSM: set subframe1 preamble timestamp for sat "<<d_nav.d_satellite_PRN<<std::endl;
}
//TODO: change to subframe 5
if (subframe_ID==3) { // if the subframe is the 5th, then
if (d_nav.satellite_validation()) // if all the satellite ephemeris parameters are good, then
{
// compute the GPS master clock
d_nav.master_clock();
// compute the satellite current ECEF position
d_nav.satpos();
// compute the clock error including relativistic effects
d_nav.relativistic_clock_correction();
// Send the procesed satellite ephemeris packet
d_nav_queue->push(d_nav);
}

2
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h
View File

@ -61,6 +61,8 @@ public:
char d_subframe[GPS_SUBFRAME_LENGTH];
char d_GPS_frame_4bytes[GPS_WORD_LENGTH];
double d_preamble_time_ms;
void gps_word_to_subframe(int position);
void gps_subframe_to_nav_msg();

482
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

@ -19,12 +19,11 @@
#include "gps_l1_ca_dll_pll_tracking_cc.h"
#include "gps_sdr_signal_processing.h"
//#include "gps_sdr_signaldef.h"
#include "gps_sdr_simd.h"
#include "gps_sdr_x86.h"
#include "control_message_factory.h"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
@ -37,6 +36,11 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
/*!
* \todo Include in definition header file
*/
#define CN0_ESTIMATION_SAMPLES 10
using google::LogMessage;
gps_l1_ca_dll_pll_tracking_cc_sptr
@ -47,11 +51,18 @@ gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs
fs_in, vector_length, queue, dump));
}
void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required){
ninput_items_required[0] =d_vector_length*2; //set the required available samples in each call
}
gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump) :
gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
gr_block ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
gr_make_io_signature(5, 5, sizeof(float))) {
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars
d_queue = queue;
d_dump = dump;
@ -61,38 +72,39 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
d_vector_length = vector_length;
// Initialize tracking variables ==========================================
//TODO: Include this setting in configuration file
/*!
* \todo Include PLL and DLL filter setting in configuration file
*/
//--- DLL variables --------------------------------------------------------
d_early_late_spc = 0.5; // Define early-late offset (in chips)
d_pdi_code = 0.001;// Summation interval for code
d_dllnoisebandwidth=2.0; //Hz
d_dllnoisebandwidth=1; //Hz
d_dlldampingratio=0.7;
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values
//--- PLL variables --------------------------------------------------------
d_pdi_carr = 0.001;// Summation interval for carrier
d_plldampingratio=0.7;
d_pllnoisebandwidth=25;
d_pllnoisebandwidth=50;
//Calculate filter coefficient values
calculate_lopp_coef(&d_tau1_carr, &d_tau2_carr, d_pllnoisebandwidth, d_plldampingratio,0.25);// Calculate filter coefficient values
// Initialization of local code replica
// Get a vector with the C/A code sampled 1x/chip
d_code_length=1023;
d_ca_code=(gr_complex*)malloc(sizeof(gr_complex)*(d_code_length+2));
// Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code=new gr_complex[d_code_length+2];
// Get space for the resampled early / prompt / late local replicas
d_early_code=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
// Get space for the resampled early / prompt / late local replicas
d_prompt_code=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
// Get space for the resampled early / prompt / late local replicas
d_late_code=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
d_early_code= new gr_complex[d_vector_length*2];
d_prompt_code=new gr_complex[d_vector_length*2];
d_late_code=new gr_complex[d_vector_length*2];
d_carr_sign=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
d_bb_sign=(gr_complex*)malloc(sizeof(gr_complex)*d_vector_length);
// space for carrier wipeoff and signal baseband vectors
d_carr_sign=new gr_complex[d_vector_length*2];
d_bb_sign=new gr_complex[d_vector_length*2];
//--- Perform initializations ------------------------------
@ -114,167 +126,194 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
d_old_carr_nco = 0.0;
d_old_carr_error = 0.0;
d_absolute_code_phase_chips = 0;
d_absolute_code_phase_samples = 0;
// sample synchronization
d_sample_counter=0;
d_acq_sample_stamp=0;
d_dump_filename="./data/trk_epr.dat";
d_enable_tracking=false;
d_last_seg=0;
d_pull_in=false;
d_last_seg=0;
d_blksize=d_vector_length;
d_loops_count=0;
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter=0;
d_P_I_buffer=new float[CN0_ESTIMATION_SAMPLES];
d_P_Q_buffer=new float[CN0_ESTIMATION_SAMPLES];
d_carrier_lock_test=1;
d_SNR_SNV=0;
d_SNR_SNV_dB_Hz=0;
d_SNR_MM=0;
d_carrier_lock_fail_counter=0;
d_carrier_lock_threshold=5;
}
void gps_l1_ca_dll_pll_tracking_cc::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
// Solve natural frequency
float Wn;
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
// solve for t1 & t2
*tau1 = k / (Wn * Wn);
*tau2 = (2.0 * zeta) / Wn;
//std::cout<<"Tau1= "<<*tau1<<std::endl;
//std::cout<<"Tau2= "<<*tau2<<std::endl;
}
void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
unsigned long int acq_sample_difference;
int trk_corrected_code_phase;
acq_sample_difference=this->d_sample_counter-d_acq_sample_stamp-d_vector_length;
float velocity_ratio,code_freq_mod,T_prn,T_prn_mod,T_chip_mod;
const float carrier_freq=1575420000;
velocity_ratio=(carrier_freq+d_carrier_doppler)/carrier_freq;
code_freq_mod=velocity_ratio*d_code_freq;
T_prn=(1/d_code_freq)*(float)d_code_length;
T_chip_mod=1/code_freq_mod;
T_prn_mod=T_chip_mod*(float)d_code_length;
//compute the code phase chips prediction
trk_corrected_code_phase=round(fmod((d_code_phase+(float)acq_sample_difference+(T_prn-T_prn_mod)*((float)acq_sample_difference/(float)d_vector_length)*(float)d_fs_in),(float)d_vector_length));
if (trk_corrected_code_phase<0)
{
trk_corrected_code_phase=d_vector_length+trk_corrected_code_phase;
}
d_absolute_code_phase_samples=(float)trk_corrected_code_phase;
// generate local reference ALWAYS starting at chip 1, not corrected
code_gen_conplex(&d_ca_code[1],d_satellite,0);
// Then make it possible to do early and late versions
d_ca_code[0]=d_ca_code[1023];
d_ca_code[1024]=d_ca_code[1];
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
if (d_dump==true)
{
//std::stringstream d_dump_filename_str;//create a stringstream to form the dump filename
//d_dump_filename_str<<"./data/trk_epl_CH_"<<this->d_channel<<"_SAT_"<<this->d_satellite<<".dat";
//d_dump_filename=d_dump_filename_str.str();
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename="track_ch"; //base path and name for the tracking log file
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
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);
std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
d_carrier_lock_fail_counter=0;
d_pull_in=true;
d_enable_tracking=true;
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID "<< this->d_satellite+1 << std::endl;
}
void gps_l1_ca_dll_pll_tracking_cc::update_local_code_refs()
{
float tcode;
float max_tcode;
unsigned int i=0;
int tmp_index;
float block_correction;
int associated_chip_index;
// unified loop for E, P, L code vectors
for (unsigned int i=0;i<d_blksize;i++)
{
tcode=i*d_code_phase_step+d_rem_code_phase-d_early_late_spc;
associated_chip_index=ceil(fmod(tcode,d_code_length));
d_early_code[i] = d_ca_code[associated_chip_index];
tcode += d_early_late_spc;
associated_chip_index = ceil(fmod(tcode, d_code_length));
d_prompt_code[i] = d_ca_code[associated_chip_index];
tcode += d_early_late_spc;
associated_chip_index = ceil(fmod(tcode, d_code_length));
d_late_code[i] = d_ca_code[associated_chip_index];
}
// std::cout<<"d_code_phase_step"<<d_code_phase_step<<std::endl;
// std::cout<<"d_rem_code_phase"<<d_rem_code_phase<<std::endl;
// std::cout<<"d_blk_size"<<d_blk_size<<std::endl;
//**** Option 1: Keep the number of samples per PRN period constant and equal to the nominal value
//**** and record the size mismatch in a var: d_rem_code_phase
//max_tcode=((float)d_vector_length-1.0)*d_code_phase_step+d_rem_code_phase;
//d_rem_code_phase = (max_tcode + d_code_phase_step) - 1023.0;
//d_rem_code_phase = d_rem_code_phase+((float)d_vector_length-1023.0*(1.0/d_code_freq)*(float)d_fs_in)*d_code_phase_step;
// Define index into early code vector
max_tcode=((float)d_blk_size-1.0)*d_code_phase_step+d_rem_code_phase-d_early_late_spc;
for (tcode=d_rem_code_phase-d_early_late_spc;tcode<max_tcode;tcode+=d_code_phase_step)
{
tmp_index=ceil(fmod(tcode,d_code_length));
d_early_code[i]=d_ca_code[tmp_index];
i++;
// if (i==d_vector_length){
// std::cout<<"Break "<<std::endl;
// break;
// }
}
// Define index into late code vector
i=0;
max_tcode=((float)d_blk_size-1.0)*d_code_phase_step+d_rem_code_phase+d_early_late_spc;
for (tcode=d_rem_code_phase+d_early_late_spc;tcode<max_tcode;tcode+=d_code_phase_step)
{
tmp_index=ceil(fmod(tcode,d_code_length));
d_late_code[i]=d_ca_code[tmp_index];
i++;
}
// Define index into prompt code vector
i=0;
max_tcode=((float)d_blk_size-1.0)*d_code_phase_step+d_rem_code_phase;
for (tcode=d_rem_code_phase;tcode<max_tcode;tcode+=d_code_phase_step)
{
tmp_index=ceil(fmod(tcode,d_code_length));
d_prompt_code[i]=d_ca_code[tmp_index];
i++;
}
block_correction=(float)d_blk_size*(1/(float)d_fs_in)-(float)d_code_length*(1/d_code_freq);
d_rem_code_phase = (max_tcode + d_code_phase_step - block_correction) - 1023.0;
//std::cout<<"d_rem_code_phase="<<d_rem_code_phase<<std::endl;
//**** Option 2: Each loop, compute the new PRN sequence code length according to the estimated Doppler
tcode=d_blksize*d_code_phase_step+d_rem_code_phase;
d_rem_code_phase = tcode - 1023.0; //prompt remaining code phase
}
void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
{
float phase, phase_step;
float phase, phase_step;
phase_step = (float)TWO_PI*d_carrier_doppler/d_fs_in;
phase=d_rem_carr_phase;
for(unsigned int i = 0; i < d_vector_length; i++) {
d_carr_sign[i] = std::complex<float>(cos(phase),sin(phase));
phase += phase_step;
}
d_rem_carr_phase=fmod(phase,TWO_PI);
phase_step = (float)TWO_PI*d_carrier_doppler/d_fs_in;
phase=d_rem_carr_phase;
for(unsigned int i = 0; i < d_blksize; i++) {
d_carr_sign[i] = std::complex<float>(cos(phase),sin(phase));
phase += phase_step;
}
d_rem_carr_phase=fmod(phase,TWO_PI);
}
gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
/*!
* \todo free memory!!
*/
d_dump_file.close();
}
void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
unsigned long int acq_sample_difference;
int trk_corrected_code_phase;
acq_sample_difference=this->d_sample_counter-d_acq_sample_stamp-d_vector_length;
float velocity_ratio,code_freq_mod,T_prn,T_prn_mod,T_chip_mod;
const float carrier_freq=1575420000;
velocity_ratio=(carrier_freq+d_carrier_doppler)/carrier_freq;
code_freq_mod=velocity_ratio*d_code_freq;
T_prn=(1/d_code_freq)*(float)d_code_length;
T_chip_mod=1/code_freq_mod;
T_prn_mod=T_chip_mod*(float)d_code_length;
trk_corrected_code_phase=round(fmod((d_code_phase+(float)acq_sample_difference+(T_prn-T_prn_mod)*((float)acq_sample_difference/(float)d_vector_length)*(float)d_fs_in),(float)d_vector_length));
if (trk_corrected_code_phase<0)
{
trk_corrected_code_phase=d_vector_length+trk_corrected_code_phase;
}
d_absolute_code_phase_chips=(float)trk_corrected_code_phase;
/*
std::cout<<"Acq sample stamp"<<d_acq_sample_stamp<<std::endl;
std::cout<<"Acq - Trk sample difference "<<acq_sample_difference<<std::endl;
std::cout<<"Trk local code correction "<<trk_corrected_code_phase<<std::endl;
std::cout<<"Rounded"<<round((float)d_code_length*(float)trk_corrected_code_phase/(float)d_vector_length);
*/
// generate local reference with the acquisition shift corrected
code_gen_conplex(&d_ca_code[1],d_satellite,1023-round((float)d_code_length*(float)trk_corrected_code_phase/(float)d_vector_length));
// Then make it possible to do early and late versions
d_ca_code[0]=d_ca_code[1023];
d_ca_code[1024]=d_ca_code[1];
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
d_enable_tracking=true;
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID "<< this->d_satellite << std::endl;
}
/*! Tracking signal processing
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
*/
int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
d_sample_counter+=d_vector_length; //count for the processed samples
int gps_l1_ca_dll_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) {
d_loops_count++;
if (d_enable_tracking==true){
if (d_pull_in==true)
{
int samples_offset=ceil(d_absolute_code_phase_samples);
d_code_phase_ms=(d_absolute_code_phase_samples*1000.0)/(float)d_fs_in;
consume_each(samples_offset); //shift input to perform alignement with local replica
d_sample_counter+=samples_offset; //count for the processed samples
d_pull_in=false;
return 1;
}
float carr_error;
float carr_nco;
float code_error;
float code_nco;
float tmp_E,tmp_P,tmp_L;
const gr_complex* in = (gr_complex*) input_items[0];
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
float **out = (float **) &output_items[0];
// Find the size of a "block" or code period in whole samples
// Update the phasestep based on code freq (variable) and
// sampling frequency (fixed)
d_code_phase_step = d_code_freq / d_fs_in;
// code_phase_step_per_sample = T_sample/T_chip
d_code_phase_step = d_code_freq / (float)d_fs_in; //[chips]
// variable code PRN sample block size
d_blksize=ceil((1023.0-d_rem_code_phase) / d_code_phase_step); //[samples]
// original variable block size (MATLAB)
//d_blk_size = ceil(((float)d_code_length-d_rem_code_phase) / d_code_phase_step);
// fixed block size (Javi)
d_blk_size = ceil((float)d_code_length*(1/1023000.0)*(float)d_fs_in);
//float rem_code_phase_samples=d_rem_code_phase/d_code_phase_step;
//d_absolute_code_phase_samples-=floor(rem_code_phase_samples);
//d_rem_code_phase=d_code_phase_step*(rem_code_phase_samples-floor(rem_code_phase_samples));
this->update_local_code_refs();
this->update_local_carrier();
@ -287,7 +326,8 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
d_P_Q=0.0;
d_L_Q=0.0;
for(unsigned int i=0;i<d_vector_length;i++) {
// perform Early, Prompt and Late correlation
for(unsigned int i=0;i<d_blksize;i++) {
//Perform the carrier wipe-off
d_bb_sign[i] = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each
@ -302,11 +342,14 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
d_L_I += d_late_code[i].real()*d_bb_sign[i].imag();
}
//float block_error_samples;
//block_error_samples=(float)d_vector_length-d_blksize;
//d_absolute_code_phase_samples=d_absolute_code_phase_samples+block_error_samples;
// Find PLL error and update carrier NCO -
// Implement carrier loop discriminator (phase detector)
//carr_error = atan(d_P.imag() / d_P.real()) / TWO_PI;
carr_error = atan(d_P_Q / d_P_I) / TWO_PI;
carr_error = atan(d_P_Q / d_P_I) / (float)TWO_PI;
// Implement carrier loop filter and generate NCO command
@ -325,33 +368,102 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
// Implement code loop filter and generate NCO command
code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(code_error - d_old_code_error) + code_error * (d_pdi_code/d_tau1_code);
d_old_code_nco = code_nco;
d_old_code_error = code_error;
d_absolute_code_phase_chips+=d_old_code_error; //accumulate the error to get the pseudorange
d_old_code_error = code_error; //[chips]
// Modify code freq based on NCO command
d_code_freq = 1023000 - code_nco;
// Output the tracking data to navigation and PVT
d_code_phase_ms+=(1023000/d_code_freq)-1.0;
/*!
* \todo Code lock detector
*/
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_P_I_buffer[d_cn0_estimation_counter]=d_P_I;
d_P_Q_buffer[d_cn0_estimation_counter]=d_P_Q;
d_cn0_estimation_counter++;
}else{
// estimate CN0 and lock status using buffered values
// MATLAB CODE
//Psig=((1/N)*sum(abs(imag(x((n-N+1):n)))))^2;
//Ptot=(1/N)*sum(abs(x((n-N+1):n)).^2);
//M2=Ptot;
//M4=(1/N)*sum(abs(x((n-N+1):n)).^4);
//SNR_SNV(count)=Psig/(Ptot-Psig);
//SNR_MM(count)=sqrt(2*M2^2-M4)/(M2-Psig);
// lock detector operation
//NBD=sum(abs(imag(x((n-N+1):n))))^2 + sum(abs(real(x((n-N+1):n))))^2;
//NBP=sum(imag(x((n-N+1):n)).^2) - sum(real(x((n-N+1):n)).^2);
//LOCK(count)=NBD/NBP;
//CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length);
float tmp_abs_I,tmp_abs_Q;
float tmp_sum_abs_I,tmp_sum_abs_Q;
float tmp_sum_sqr_I,tmp_sum_sqr_Q;
float Psig,Ptot,M2,M4;
float NBD,NBP;
Psig=0;
Ptot=0;
NBD=0;
NBP=0;
tmp_sum_abs_I=0;
tmp_sum_abs_Q=0;
tmp_sum_sqr_I=0;
tmp_sum_sqr_Q=0;
for (int i=0;i<CN0_ESTIMATION_SAMPLES;i++)
{
tmp_abs_I=std::abs(d_P_I_buffer[i]);
tmp_abs_Q=std::abs(d_P_Q_buffer[i]);
Psig+=tmp_abs_I;
Ptot+=d_P_I_buffer[i]*d_P_I_buffer[i]+d_P_Q_buffer[i]*d_P_Q_buffer[i];
tmp_sum_abs_I+=tmp_abs_I;
tmp_sum_abs_Q+=tmp_abs_Q;
tmp_sum_sqr_I+=(d_P_I_buffer[i]*d_P_I_buffer[i]);
tmp_sum_sqr_Q+=(d_P_Q_buffer[i]*d_P_Q_buffer[i]);
}
Psig=Psig/(float)CN0_ESTIMATION_SAMPLES;
Psig=Psig*Psig;
d_SNR_SNV=Psig/(Ptot/(float)CN0_ESTIMATION_SAMPLES-Psig);
d_SNR_SNV_dB_Hz=10*std::log10(d_SNR_SNV)+10*log10(d_fs_in/2)-10*log10(d_code_length);
NBD=tmp_sum_abs_I*tmp_sum_abs_I+tmp_sum_abs_Q*tmp_sum_abs_Q;
NBP=tmp_sum_sqr_I-tmp_sum_sqr_Q;
d_carrier_lock_test=NBD/NBP;
d_cn0_estimation_counter=0;
}
// ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message;
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
{
d_carrier_lock_fail_counter++;
}else{
d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter>200)
{
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
tracking_message=3; //loss of lock
d_channel_internal_queue->push(tracking_message);
d_carrier_lock_fail_counter=0;
d_enable_tracking=false; // TODO: check if disabling tranking is consistent with the channel state machine
}
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
// Output the tracking data to navigation and PVT
// Output channel 1: Prompt correlator output Q
*out[0]=d_P_Q;
// Output channel 2: Prompt correlator output I
*out[1]=d_P_I;
// Output channel 3: Prompt correlator output
*out[2]=d_absolute_code_phase_chips;
//Equivalent to the "absolute spreading code starting position" information of the matlab tracking algorithm
// std::cout<<"tmp_E= "<<tmp_E<<std::endl;
// std::cout<<"tmp_P= "<<tmp_P<<std::endl;
// std::cout<<"tmp_L= "<<tmp_L<<std::endl;
//
// std::cout<<"trk pllDiscr carr_error= "<<carr_error<<std::endl;
// std::cout<<"trk dllDiscr code error= "<<code_error<<std::endl;
// std::cout<<"trk dllDiscrFilt code_nco= "<<code_nco<<std::endl;
// std::cout<<"trk pllDiscrFilt carr_nco= "<<carr_nco<<std::endl;
// Output channel 3: PRN absolute delay [ms]
*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
// Output channel 4: PRN code error [ms]
*out[3]=d_code_phase_ms;//(code_error*1000.0)/d_code_freq;
if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file
@ -359,23 +471,35 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
tmp_P=sqrt(d_P_I*d_P_I+d_P_Q*d_P_Q);
tmp_L=sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q);
// EPR
d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float));
// DLL
d_dump_file.write((char*)&code_error, sizeof(float));
d_dump_file.write((char*)&code_nco, sizeof(float));
//PLL
d_dump_file.write((char*)&carr_error, sizeof(float));
d_dump_file.write((char*)&carr_nco, sizeof(float));
//FREQ AND PHASE
d_dump_file.write((char*)&d_code_freq, sizeof(float));
d_dump_file.write((char*)&d_carrier_doppler, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&d_P_I, sizeof(float));
d_dump_file.write((char*)&d_P_Q, sizeof(float));
try {
// EPR
d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float));
// DLL
d_dump_file.write((char*)&code_error, sizeof(float));
d_dump_file.write((char*)&code_nco, sizeof(float));
//PLL
d_dump_file.write((char*)&carr_error, sizeof(float));
d_dump_file.write((char*)&carr_nco, sizeof(float));
//FREQ AND PHASE
d_dump_file.write((char*)&d_code_freq, sizeof(float));
d_dump_file.write((char*)&d_carrier_doppler, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&d_P_I, sizeof(float));
d_dump_file.write((char*)&d_P_Q, sizeof(float));
// Absolute PRN start sample (MATLAB version)
//d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
//d_dump_file.write((char*)&d_loops_count, sizeof(unsigned long int));
d_dump_file.write((char*)&d_SNR_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
}
}
// debug: Second counter in channel 0
if (d_channel==0)
@ -384,21 +508,33 @@ int gps_l1_ca_dll_pll_tracking_cc::work (int noutput_items,gr_vector_const_void_
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"t="<<d_last_seg<<std::endl;
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_SNR_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
if (d_sample_counter>round((float)this->d_fs_in*39)){ //stop after some seconds debug only!
d_enable_tracking=false;
std::cout<<"Stop tracking at sample "<<d_sample_counter<<" and acq at sample "<<d_acq_sample_stamp<<std::endl;
if(d_queue != gr_msg_queue_sptr()) {
ControlMessageFactory* cmf = new ControlMessageFactory();
d_queue->handle(cmf->GetQueueMessage(200,0)); //send stop to the control_thread
delete cmf;
std::cout<<"stop sent from tracking" << std::endl;
if (d_sample_counter>round((float)this->d_fs_in*70)){ //stop after some seconds debug only!
d_enable_tracking=false;
std::cout<<"Stop tracking at sample "<<d_sample_counter<<" and acq at sample "<<d_acq_sample_stamp<<std::endl;
if(d_queue != gr_msg_queue_sptr()) {
ControlMessageFactory* cmf = new ControlMessageFactory();
d_queue->handle(cmf->GetQueueMessage(200,0)); //send stop to the control_thread
delete cmf;
std::cout<<"stop sent from tracking";
}
}
}else
{
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_SNR_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
}
//consume_each(1); //not necesary for gr_sync_block derivates
return 1; //one group of d_vector_lenght samples at time, and output one set of results ALWAYS even in the case of d_enable_tracking==false
consume_each(d_blksize); // this is necesary in gr_block derivates
d_sample_counter+=d_blksize; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}

34
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
View File

@ -37,7 +37,7 @@
#include <gnuradio/gr_block.h>
#include <gnuradio/gr_msg_queue.h>
#include <gnuradio/gr_sync_decimator.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "gps_sdr_signal_processing.h"
@ -59,7 +59,8 @@ gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq,
int vector_length,
gr_msg_queue_sptr queue, bool dump);
class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
class gps_l1_ca_dll_pll_tracking_cc: public gr_block
{
private:
@ -143,11 +144,30 @@ private:
float d_L_I;
float d_L_Q;
float d_absolute_code_phase_chips;
float d_absolute_code_phase_samples;
float d_code_phase_ms;
unsigned int d_blksize;
unsigned long int d_sample_counter;
unsigned long int d_acq_sample_stamp;
unsigned long int d_loops_count;
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
float* d_P_I_buffer;
float* d_P_Q_buffer;
float d_carrier_lock_test;
float d_SNR_SNV;
float d_SNR_MM;
float d_SNR_SNV_dB_Hz;
float d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
bool d_enable_tracking;
bool d_pull_in;
std::string d_dump_filename;
std::ofstream d_dump_file;
@ -181,8 +201,12 @@ public:
// gr_vector_const_void_star &input_items,
// gr_vector_void_star &output_items) = 0;
int work(int noutput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
//int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
};

13
src/core/system_parameters/GPS_L1_CA.h
View File

@ -40,7 +40,7 @@
// SEPARATE FILE GPS.H
const float GPS_C_m_s= 299792458; // The speed of light, [m/ms]
const float GPS_C_m_s= 299792458.0; // The speed of light, [m/ms]
const float GPS_STARTOFFSET_ms= 68.802; //[ms] Initial sign. travel time
const float GPS_PI = 3.1415926535898; // Pi used in the GPS coordinate system
//-- Constants for satellite position calculation -------------------------
@ -60,8 +60,7 @@ const double F = -4.442807633e-10; // Constant, [sec/(meter)^(1/2)
/*! @ingroup GPS_DEFINES
* @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
*/
* the start position inside the subframe and the length in number of bits */
typedef struct bits_slice{
int position;
int length;
@ -74,12 +73,12 @@ typedef struct bits_slice{
/*! @ingroup GPS_DEFINES
* @brief Demodulator gnss_synchro structure, used to feed the pseudorange block
*/
* @brief Demodulator gnss_synchro structure, used to feed the pseudorange block */
typedef struct gnss_synchro
{
unsigned long int last_preamble_index;
float prn_delay;
float preamble_delay_ms;
float prn_delay_ms;
int satellite_PRN;
int channel_ID;
bool valid_word;

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

@ -1,6 +1,6 @@
/*!
* \file gps_navigation_message.cc
* \brief Implementation of GPS L1 C/A navigation message decoding
* \brief Navigation message structure for GPS L1 C/A signal
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
@ -30,7 +30,6 @@
#include "gps_navigation_message.h"
//! Sets all the ephemeris parameters to zero
void gps_navigation_message::reset()
{
d_TOW=0;
@ -65,9 +64,9 @@ void gps_navigation_message::reset()
d_SV_accuracy=0;
d_SV_health=0;
d_TGD=0;
d_IODC=0;
d_IODC=-1;
//broadcast orbit 7
d_transmission_time=0;
d_fit_interval=0;
d_spare1=0;
d_spare2=0;
@ -89,15 +88,17 @@ void gps_navigation_message::reset()
// info
d_channel_ID=0;
d_satellite_PRN=0;
// time synchro
d_subframe1_timestamp_ms=0;
}
//! Default constructor
gps_navigation_message::gps_navigation_message()
{
reset();
}
//! Read unsigned values
}
unsigned long int gps_navigation_message::read_navigation_unsigned(std::bitset<GPS_SUBFRAME_BITS> bits, const bits_slice *slices, int num_of_slices)
{
unsigned long int value;
@ -117,7 +118,6 @@ unsigned long int gps_navigation_message::read_navigation_unsigned(std::bitset<G
return value;
}
//! Read signed values
signed long int gps_navigation_message::read_navigation_signed(std::bitset<GPS_SUBFRAME_BITS> bits, const bits_slice *slices, int num_of_slices)
{
signed long int value=0;
@ -145,13 +145,7 @@ signed long int gps_navigation_message::read_navigation_signed(std::bitset<GPS_S
return value;
}
/*!
* \brief Accounts for beginning or end of week crossover
*
* Inspired in a Matlab function by Kai Borre
* \param[in] time (in seconds)
* \result Returns corrected time
*/
double gps_navigation_message::check_t(double time)
{
/*
@ -163,6 +157,13 @@ corrTime = check_t(time);
Outputs:
corrTime - corrected time (seconds)
Kai Borre 04-01-96
Copyright (c) by Kai Borre
CVS record:
$Id: check_t.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
==========================================================================
*/
double corrTime;
double half_week = 302400; // seconds
@ -177,23 +178,21 @@ corrTime = check_t(time);
return corrTime;
}
void gps_navigation_message::master_clock()
void gps_navigation_message::master_clock(double transmitTime)
{
double dt;
double satClkCorr;
// Find initial satellite clock correction --------------------------------
// --- Find time difference ---------------------------------------------
dt = check_t(d_TOW - d_Toc);
dt = check_t(transmitTime - d_Toc);
//--- Calculate clock correction ---------------------------------------
satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 - d_TGD;
d_master_clock = d_TOW - satClkCorr;
d_master_clock = transmitTime - satClkCorr;
}
//! Computes satellite position from ephemeris data
void gps_navigation_message::satpos()
{
double tk;
@ -239,7 +238,7 @@ void gps_navigation_message::satpos()
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old,2*GPS_PI);
std::cout<<"dE="<<dE<<std::endl;
//std::cout<<"dE="<<dE<<std::endl;
if (abs(dE) < 1E-12)
{
//Necessary precision is reached, exit from the loop
@ -289,19 +288,17 @@ void gps_navigation_message::satpos()
d_satpos_Z = sin(u)*r * sin(i);
}
void gps_navigation_message::relativistic_clock_correction()
void gps_navigation_message::relativistic_clock_correction(double transmitTime)
{
double dt;
// Find initial satellite clock correction --------------------------------
// Find final satellite clock correction --------------------------------
// --- Find time difference ---------------------------------------------
dt = check_t(d_TOW - d_Toc);
dt = check_t(transmitTime - d_Toc);
//Include relativistic correction in clock correction --------------------
d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 -d_TGD + d_dtr;
}
//! Subframe decoder
int gps_navigation_message::subframe_decoder(char *subframe)
{
int subframe_ID=0;
@ -333,7 +330,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
}
*/
subframe_ID=(int)read_navigation_unsigned(subframe_bits,SUBFRAME_ID,num_of_slices(SUBFRAME_ID));
std::cout<<"subframe ID="<<subframe_ID<<std::endl;
//std::cout<<"subframe ID="<<subframe_ID<<std::endl;
// Decode all 5 sub-frames
switch (subframe_ID){
@ -371,6 +368,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_A_f2=d_A_f2*A_F2_LSB;
/* debug print */
/*
std::cout<<"d_TOW="<<d_TOW<<std::endl;
std::cout<<"GPS week="<<d_GPS_week<<std::endl;
std::cout<<"SV_accuracy="<<d_SV_accuracy<<std::endl;
@ -381,7 +379,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
std::cout<<"A_F0="<<d_A_f0<<std::endl;
std::cout<<"A_F1="<<d_A_f1<<std::endl;
std::cout<<"A_F2="<<d_A_f2<<std::endl;
*/
/*
eph.weekNumber = bin2dec(subframe(61:70)) + 1024;
eph.accuracy = bin2dec(subframe(73:76));
@ -396,13 +394,10 @@ int gps_navigation_message::subframe_decoder(char *subframe)
break;
case 2:
tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
//tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
//std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
// --- It is subframe 2 -------------------------------------
// It contains first part of ephemeris parameters
d_IODE_SF2=(double)read_navigation_unsigned(subframe_bits,IODE_SF2,num_of_slices(IODE_SF2));
d_Crs=(double)read_navigation_signed(subframe_bits,C_RS,num_of_slices(C_RS));
d_Crs=d_Crs*C_RS_LSB;
@ -422,6 +417,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_Toe=d_Toe*T_OE_LSB;
/* debug print */
/*
std::cout<<"d_IODE_SF2="<<d_IODE_SF2<<std::endl;
std::cout<<"d_Crs="<<d_Crs<<std::endl;
std::cout<<"d_Delta_n="<<d_Delta_n<<std::endl;
@ -431,7 +427,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
std::cout<<"d_Cus="<<d_Cus<<std::endl;
std::cout<<"d_sqrt_A="<<d_sqrt_A<<std::endl;
std::cout<<"d_Toe="<<d_Toe<<std::endl;
*/
break;
/*
eph.IODE_sf2 = bin2dec(subframe(61:68));
@ -445,9 +441,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
eph.t_oe = bin2dec(subframe(271:286)) * 2^4;
*/
case 3:
tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
//tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
//std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
// --- It is subframe 3 -------------------------------------
// It contains second part of ephemeris parameters
d_Cic=(double)read_navigation_signed(subframe_bits,C_IC,num_of_slices(C_IC));
@ -469,6 +464,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_IDOT=d_IDOT*I_DOT_LSB;
/* debug print */
/*
std::cout<<"d_Cic="<<d_Cic<<std::endl;
std::cout<<"d_OMEGA0="<<d_OMEGA0<<std::endl;
std::cout<<"d_Cis="<<d_Cis<<std::endl;
@ -478,6 +474,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
std::cout<<"d_OMEGA_DOT="<<d_OMEGA_DOT<<std::endl;
std::cout<<"d_IODE_SF3="<<d_IODE_SF3<<std::endl;
std::cout<<"d_IDOT="<<d_IDOT<<std::endl;
*/
break;
/*
@ -492,9 +489,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
eph.iDot = twosComp2dec(subframe(279:292)) * 2^(-43) * gpsPi;
*/
case 4:
tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
//tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
//std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
// --- It is subframe 4 -------------------------------------
// Almanac, ionospheric model, UTC parameters.
// SV health (PRN: 25-32)
@ -502,14 +498,14 @@ int gps_navigation_message::subframe_decoder(char *subframe)
SV_page=(int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE));
/* debug print */
/*
std::cout<<"SF4 SV_data_ID="<<SV_data_ID<<std::endl;
std::cout<<"SF4 SV_page="<<SV_page<<std::endl;
*/
break;
case 5:
tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
tmp_TOW=tmp_TOW*6-6; //we are in the first subframe (need no correction) !
std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
//tmp_TOW=(double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW));
//std::cout<<"tmp_TOW="<<tmp_TOW<<std::endl;
//--- It is subframe 5 -------------------------------------
// SV almanac and health (PRN: 1-24).
// Almanac reference week number and time.
@ -517,8 +513,10 @@ int gps_navigation_message::subframe_decoder(char *subframe)
SV_page=(int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE));
/* debug print */
/*
std::cout<<"SF5 SV_data_ID="<<SV_data_ID<<std::endl;
std::cout<<"SF5 SV_page="<<SV_page<<std::endl;
*/
break;
default:
break;
@ -534,7 +532,7 @@ bool gps_navigation_message::satellite_validation()
// first step: check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception
// and check if the data has been filled (!=0)
if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!=0)
if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!=-1)
{
flag_data_valid=true;
}

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

@ -1,6 +1,6 @@
/*!
* \file gps_navigation_message.h
* \brief This class implements GPS L1 C/A navigation message decoding
* \brief Navigation message structure for GPS L1 C/A signal
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
@ -28,6 +28,7 @@
* -------------------------------------------------------------------------
*/
#ifndef GPS_NAVIGATION_MESSAGE_H
#define GPS_NAVIGATION_MESSAGE_H
@ -44,13 +45,6 @@ using namespace boost::assign;
#include "GPS_L1_CA.h"
/*!
* \brief This class implements GPS L1 C/A navigation message decoding
* and computes satellite position
*
* It defines the specific GPS navigation (NAV) data structure,
* as defined in the document Interface Specification IS-GPS-200 Revision E, Appendix II
*/
class gps_navigation_message
{
@ -93,7 +87,7 @@ public:
double d_TGD;
double d_IODC;
//broadcast orbit 7
double d_transmission_time;
double d_fit_interval;
double d_spare1;
double d_spare2;
@ -118,12 +112,15 @@ public:
int d_channel_ID;
int d_satellite_PRN;
// time synchro
double d_subframe1_timestamp_ms; //[ms]
// public functions
void reset();
int subframe_decoder(char *subframe);
void master_clock();
void master_clock(double transmitTime);
void satpos();
void relativistic_clock_correction();
void relativistic_clock_correction(double transmitTime);
bool satellite_validation();
gps_navigation_message();
};

3
src/core/system_parameters/gps_telemetry.cc
View File

@ -1,7 +1,6 @@
/*!
* \file gps_telemetry.cc
* \brief Implementation of low-levels functions for GPS L1 C/A
* navigation message decoding
* \brief GPS L1 C/A telemetry processing
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------

4
src/core/system_parameters/gps_telemetry.h
View File

@ -1,7 +1,6 @@
/*!
* \file gps_telemetry.h
* \brief Define low-level constants and functions for GPS L1 C/A
* navigation message decoding
* \brief GPS L1 C/A telemetry processing
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
@ -28,7 +27,6 @@
*
* -------------------------------------------------------------------------
*/
#ifndef GPS_TELEMETRY_H_
#define GPS_TELEMETRY_H_

1
src/main/jamfile.jam
View File

@ -21,6 +21,7 @@ exe mercurio : main.cc
../algorithms/observables/adapters//gps_l1_ca_observables
../algorithms/observables/gnuradio_blocks//gps_l1_ca_observables_cc
../algorithms/observables/libs//rinex_2_1_printer
../algorithms/observables/libs//gps_l1_ca_ls_pvt
../algorithms/output_filter/adapters//file_output_filter
../algorithms/output_filter/adapters//null_sink_output_filter
../algorithms/signal_source/adapters//file_signal_source

2
src/main/main.cc
View File

@ -71,5 +71,5 @@ int main(int argc, char** argv)
control_thread->run();
delete control_thread;
google::ShutDownCommandLineFlags();
//google::ShutDownCommandLineFlags();
}