mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-06 02:03:04 +00:00
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.

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files

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);
};