mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-27 23:43:16 +00:00
Code Issues Releases Wiki Activity

Some code cleaning

Browse Source 
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@120 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez 2012-01-11 09:01:24 +00:00
parent dab517aff0
commit bc62d8d5be
16 changed files with 1052 additions and 979 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc
View File

@ -58,10 +58,10 @@ GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration,
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
int averaging_depth; int averaging_depth;
averaging_depth=configuration->property(role + ".averaging_depth", 10); averaging_depth = configuration->property(role + ".averaging_depth", 10);
bool flag_averaging; bool flag_averaging;
flag_averaging=configuration->property(role + ".flag_averaging", false); flag_averaging = configuration->property(role + ".flag_averaging", false);
dump_ = configuration->property(role + ".dump", false); dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
@ -73,7 +73,7 @@ GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration,
pvt_->set_navigation_queue(&global_gps_nav_msg_queue); pvt_->set_navigation_queue(&global_gps_nav_msg_queue);
DLOG(INFO) << "global navigation message queue assigned to pvt ("<< pvt_->unique_id() << ")"; DLOG(INFO) << "global navigation message queue assigned to pvt (" << pvt_->unique_id() << ")";
} }

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

@ -98,8 +98,8 @@ bool pseudoranges_pairCompare_min( std::pair<int,gnss_pseudorange> a, std::pair<
int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
d_sample_counter++; d_sample_counter++;
std::map<int,gnss_pseudorange> gnss_pseudoranges_map; std::map<int,gnss_pseudorange> gnss_pseudoranges_map;

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

@ -40,26 +40,26 @@ using google::LogMessage;
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file) gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file)
{ {
// init empty ephemeris for all the available GNSS channels // init empty ephemeris for all the available GNSS channels
d_nchannels=nchannels; d_nchannels = nchannels;
d_ephemeris=new gps_navigation_message[nchannels]; d_ephemeris = new gps_navigation_message[nchannels];
d_dump_filename=dump_filename; d_dump_filename = dump_filename;
d_flag_dump_enabled=flag_dump_to_file; d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth=0; d_averaging_depth = 0;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled==true) if (d_flag_dump_enabled == true)
{ {
if (d_dump_file.is_open()==false) if (d_dump_file.is_open() == false)
{ {
try try
{ {
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"PVT lib dump enabled Log file: "<<d_dump_filename.c_str()<<std::endl; std::cout << "PVT lib dump enabled Log file: "<< d_dump_filename.c_str() << std::endl;
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
{ {
std::cout << "Exception opening PVT lib dump file "<<e.what()<<"\r\n"; std::cout << "Exception opening PVT lib dump file "<< e.what() << std::endl;
} }
} }
} }
@ -68,7 +68,7 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool
void gps_l1_ca_ls_pvt::set_averaging_depth(int depth) void gps_l1_ca_ls_pvt::set_averaging_depth(int depth)
{ {
d_averaging_depth=depth; d_averaging_depth = depth;
} }
@ -100,16 +100,16 @@ arma::vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, arma::vec X_sat)
omegatau = OMEGA_EARTH_DOT * traveltime; omegatau = OMEGA_EARTH_DOT * traveltime;
//--- Make a rotation matrix ----------------------------------------------- //--- Make a rotation matrix -----------------------------------------------
arma::mat R3=arma::zeros(3,3); arma::mat R3 = arma::zeros(3,3);
R3(0, 0)= cos(omegatau); R3(0, 0) = cos(omegatau);
R3(0, 1)= sin(omegatau); R3(0, 1) = sin(omegatau);
R3(0, 2)= 0.0; R3(0, 2) = 0.0;
R3(1, 0)=-sin(omegatau); R3(1, 0) = -sin(omegatau);
R3(1, 1)= cos(omegatau); R3(1, 1) = cos(omegatau);
R3(1, 2)=0.0; R3(1, 2) = 0.0;
R3(2, 0)= 0.0; R3(2, 0) = 0.0;
R3(2, 1)= 0.0; R3(2, 1) = 0.0;
R3(2, 2)= 1; R3(2, 2) = 1;
//--- Do the rotation ------------------------------------------------------ //--- Do the rotation ------------------------------------------------------
arma::vec X_sat_rot; arma::vec X_sat_rot;
X_sat_rot = R3 * X_sat; X_sat_rot = R3 * X_sat;
@ -144,18 +144,18 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
arma::mat omc; arma::mat omc;
arma::mat az; arma::mat az;
arma::mat el; arma::mat el;
A=arma::zeros(nmbOfSatellites,4); A=arma::zeros(nmbOfSatellites, 4);
omc=arma::zeros(nmbOfSatellites,1); omc=arma::zeros(nmbOfSatellites, 1);
az=arma::zeros(1,nmbOfSatellites); az=arma::zeros(1, nmbOfSatellites);
el=arma::zeros(1,nmbOfSatellites); el=arma::zeros(1, nmbOfSatellites);
for (int i = 0; i < nmbOfSatellites; i++) for (int i = 0; i < nmbOfSatellites; i++)
{ {
for (int j = 0; j < 4; j++) for (int j = 0; j < 4; j++)
{ {
A(i,j)=0.0; //Armadillo A(i, j) = 0.0; //Armadillo
} }
omc(i, 0)=0.0; omc(i, 0) = 0.0;
az(0, i)=0.0; az(0, i)= 0.0;
} }
el = az; el = az;
@ -174,7 +174,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
if (iter == 0) if (iter == 0)
{ {
//--- Initialize variables at the first iteration -------------- //--- Initialize variables at the first iteration --------------
Rot_X=X.col(i); //Armadillo Rot_X = X.col(i); //Armadillo
trop = 0.0; trop = 0.0;
} }
else else
@ -194,10 +194,10 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
//--- Construct the A matrix --------------------------------------- //--- Construct the A matrix ---------------------------------------
//Armadillo //Armadillo
A(i,0)=(-(Rot_X(0) - pos(0))) / obs(i); A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
A(i,1)=(-(Rot_X(1) - pos(1))) / obs(i); A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
A(i,2)=(-(Rot_X(2) - pos(2))) / obs(i); A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
A(i,3)=1.0; A(i,3) = 1.0;
} }
@ -208,7 +208,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
//} //}
//--- Find position update --------------------------------------------- //--- Find position update ---------------------------------------------
x = arma::solve(w*A,w*omc); // Armadillo x = arma::solve(w*A, w*omc); // Armadillo
//--- Apply position update -------------------------------------------- //--- Apply position update --------------------------------------------
pos = pos + x; pos = pos + x;
@ -221,9 +221,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
{ {
std::map<int,gnss_pseudorange>::iterator gnss_pseudoranges_iter; std::map<int,gnss_pseudorange>::iterator gnss_pseudoranges_iter;
arma::mat W=arma::eye(d_nchannels,d_nchannels); //channels weights matrix arma::mat W = arma::eye(d_nchannels,d_nchannels); //channels weights matrix
arma::vec obs=arma::zeros(d_nchannels); // pseudoranges observation vector arma::vec obs = arma::zeros(d_nchannels); // pseudoranges observation vector
arma::mat satpos=arma::zeros(3,d_nchannels); //satellite positions matrix arma::mat satpos = arma::zeros(3,d_nchannels); //satellite positions matrix
int GPS_week = 0; int GPS_week = 0;
double GPS_corrected_time = 0; double GPS_corrected_time = 0;
@ -232,15 +232,15 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
int valid_obs=0; //valid observations counter int valid_obs=0; //valid observations counter
for (int i=0; i<d_nchannels; i++) for (int i=0; i<d_nchannels; i++)
{ {
if (d_ephemeris[i].satellite_validation()==true) if (d_ephemeris[i].satellite_validation() == true)
{ {
gnss_pseudoranges_iter=gnss_pseudoranges_map.find(d_ephemeris[i].i_satellite_PRN); gnss_pseudoranges_iter = gnss_pseudoranges_map.find(d_ephemeris[i].i_satellite_PRN);
if (gnss_pseudoranges_iter!=gnss_pseudoranges_map.end()) if (gnss_pseudoranges_iter != gnss_pseudoranges_map.end())
{ {
/*! /*!
* \todo Place here the satellite CN0 (power level, or weight factor) * \todo Place here the satellite CN0 (power level, or weight factor)
*/ */
W(i,i)=1; W(i,i) = 1;
// compute the GPS master clock // compute the GPS master clock
// d_ephemeris[i].master_clock(GPS_current_time); ????? // d_ephemeris[i].master_clock(GPS_current_time); ?????
@ -248,31 +248,31 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
GPS_corrected_time = d_ephemeris[i].sv_clock_correction(GPS_current_time); GPS_corrected_time = d_ephemeris[i].sv_clock_correction(GPS_current_time);
GPS_week = d_ephemeris[i].i_GPS_week; GPS_week = d_ephemeris[i].i_GPS_week;
utc =d_ephemeris[i].utc_time(GPS_corrected_time); utc = d_ephemeris[i].utc_time(GPS_corrected_time);
// compute the satellite current ECEF position // compute the satellite current ECEF position
d_ephemeris[i].satellitePosition(GPS_corrected_time); d_ephemeris[i].satellitePosition(GPS_corrected_time);
satpos(0,i)=d_ephemeris[i].d_satpos_X; satpos(0,i) = d_ephemeris[i].d_satpos_X;
satpos(1,i)=d_ephemeris[i].d_satpos_Y; satpos(1,i) = d_ephemeris[i].d_satpos_Y;
satpos(2,i)=d_ephemeris[i].d_satpos_Z; satpos(2,i) = d_ephemeris[i].d_satpos_Z;
LOG_AT_LEVEL(INFO)<<"ECEF satellite SV ID="<<d_ephemeris[i].i_satellite_PRN<<" X="<<d_ephemeris[i].d_satpos_X LOG_AT_LEVEL(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X
<<" [m] Y="<<d_ephemeris[i].d_satpos_Y<<" [m] Z="<<d_ephemeris[i].d_satpos_Z<<" [m]\r\n"; << " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
obs(i)=gnss_pseudoranges_iter->second.pseudorange_m+d_ephemeris[i].d_satClkCorr*GPS_C_m_s; obs(i) = gnss_pseudoranges_iter->second.pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
valid_obs++; valid_obs++;
} }
else else
{ {
// no valid pseudorange for the current channel // no valid pseudorange for the current channel
W(i,i)=0; // channel de-activated W(i,i) = 0; // channel de-activated
obs(i)=1; // to avoid algorithm problems (divide by zero) obs(i) = 1; // to avoid algorithm problems (divide by zero)
} }
} }
else else
{ {
// no valid ephemeris for the current channel // no valid ephemeris for the current channel
W(i,i)=0; // channel de-activated W(i,i) = 0; // channel de-activated
obs(i)=1; // to avoid algorithm problems (divide by zero) obs(i) = 1; // to avoid algorithm problems (divide by zero)
} }
} }
std::cout<<"PVT: valid observations="<<valid_obs<<std::endl; std::cout<<"PVT: valid observations="<<valid_obs<<std::endl;
@ -280,43 +280,43 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
{ {
arma::vec mypos; arma::vec mypos;
mypos=leastSquarePos(satpos,obs,W); mypos=leastSquarePos(satpos,obs,W);
LOG_AT_LEVEL(INFO) << "Position at TOW="<<GPS_current_time<<" in ECEF (X,Y,Z) = " << mypos << std::endl; LOG_AT_LEVEL(INFO) << "Position at TOW="<< GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
cart2geo(mypos(0), mypos(1), mypos(2), 4); cart2geo(mypos(0), mypos(1), mypos(2), 4);
// Compute UTC time and print PVT solution // Compute UTC time and print PVT solution
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800*(double)GPS_week); boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800*(double)GPS_week);
boost::posix_time::ptime p_time(boost::gregorian::date(1999,8,22),t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
std::cout << "Position at "<<boost::posix_time::to_simple_string(p_time)<<" is Lat = " << d_latitude_d << " [deg] Long = "<< d_longitude_d <<" [deg] Height= "<<d_height_m<<" [m]" <<std::endl; std::cout << "Position at " << boost::posix_time::to_simple_string(p_time) <<" is Lat = " << d_latitude_d << " [deg] Long = " << d_longitude_d << " [deg] Height= " << d_height_m << " [m]" << std::endl;
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled==true) if(d_flag_dump_enabled == true)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
{ {
double tmp_double; double tmp_double;
// PVT GPS time // PVT GPS time
tmp_double=GPS_current_time; tmp_double = GPS_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m] // ECEF User Position East [m]
tmp_double=mypos(0); tmp_double = mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m] // ECEF User Position North [m]
tmp_double=mypos(1); tmp_double = mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m] // ECEF User Position Up [m]
tmp_double=mypos(2); tmp_double = mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s] // User clock offset [s]
tmp_double=mypos(3); tmp_double = mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg] // GEO user position Latitude [deg]
tmp_double=d_latitude_d; tmp_double = d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg] // GEO user position Longitude [deg]
tmp_double=d_longitude_d; tmp_double = d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m] // GEO user position Height [m]
tmp_double=d_height_m; tmp_double = d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
@ -326,9 +326,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
} }
// MOVING AVERAGE PVT // MOVING AVERAGE PVT
if (flag_averaging==true) if (flag_averaging == true)
{ {
if (d_hist_longitude_d.size()==(unsigned int)d_averaging_depth) if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
{ {
// Pop oldest value // Pop oldest value
d_hist_longitude_d.pop_back(); d_hist_longitude_d.pop_back();
@ -339,18 +339,18 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
d_hist_latitude_d.push_front(d_latitude_d); d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m); d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d=0; d_avg_latitude_d = 0;
d_avg_longitude_d=0; d_avg_longitude_d = 0;
d_avg_height_m=0; d_avg_height_m = 0;
for (unsigned int i=0;i<d_hist_longitude_d.size();i++) for (unsigned int i=0; i<d_hist_longitude_d.size(); i++)
{ {
d_avg_latitude_d=d_avg_latitude_d+d_hist_latitude_d.at(i); d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d=d_avg_longitude_d+d_hist_longitude_d.at(i); d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m=d_avg_height_m+d_hist_height_m.at(i); d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
} }
d_avg_latitude_d=d_avg_latitude_d/(double)d_averaging_depth; d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
d_avg_longitude_d=d_avg_longitude_d/(double)d_averaging_depth; d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
d_avg_height_m=d_avg_height_m/(double)d_averaging_depth; d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
return true; //indicates that the returned position is valid return true; //indicates that the returned position is valid
} }
else else
@ -361,9 +361,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
d_hist_latitude_d.push_front(d_latitude_d); d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m); d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d=d_latitude_d; d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d=d_longitude_d; d_avg_longitude_d = d_longitude_d;
d_avg_height_m=d_height_m; d_avg_height_m = d_height_m;
return false;//indicates that the returned position is not valid yet return false;//indicates that the returned position is not valid yet
// output the average, although it will not have the full historic available // output the average, although it will not have the full historic available
// d_avg_latitude_d=0; // d_avg_latitude_d=0;
@ -411,11 +411,11 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
double lambda; double lambda;
lambda = atan2(Y,X); lambda = atan2(Y,X);
double ex2; double ex2;
ex2 = (2-f[elipsoid_selection])*f[elipsoid_selection]/((1-f[elipsoid_selection])*(1-f[elipsoid_selection])); ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection]));
double c; double c;
c = a[elipsoid_selection]*sqrt(1+ex2); c = a[elipsoid_selection] * sqrt(1+ex2);
double phi; double phi;
phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection]))*f[elipsoid_selection]))); phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1; double h = 0.1;
double oldh = 0; double oldh = 0;
@ -424,19 +424,19 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
do do
{ {
oldh = h; oldh = h;
N = c/sqrt(1+ex2*(cos(phi)*cos(phi))); 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))))); 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; h = sqrt(X*X + Y*Y) / cos(phi) - N;
iterations = iterations + 1; iterations = iterations + 1;
if (iterations > 100) if (iterations > 100)
{ {
std::cout<<"Failed to approximate h with desired precision. h-oldh= "<<h-oldh<<std::endl; std::cout << "Failed to approximate h with desired precision. h-oldh= " << h-oldh << std::endl;
break; break;
} }
} }
while (abs(h-oldh) > 1.0e-12); while (abs(h - oldh) > 1.0e-12);
d_latitude_d = phi*180.0/GPS_PI; d_latitude_d = phi * 180.0 / GPS_PI;
d_longitude_d = lambda*180/GPS_PI; d_longitude_d = lambda * 180 / GPS_PI;
d_height_m = h; d_height_m = h;
} }

63
src/algorithms/PVT/libs/kml_printer.cc
View File

@ -45,31 +45,31 @@ bool kml_printer::set_headers(std::string filename)
kml_file.open(filename.c_str()); kml_file.open(filename.c_str());
if (kml_file.is_open()) if (kml_file.is_open())
{ {
DLOG(INFO)<<"KML printer writing on "<<filename.c_str(); DLOG(INFO) << "KML printer writing on " << filename.c_str();
kml_file<<"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\r\n" kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<<"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\r\n" << "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
<<" <Document>\r\n" << " <Document>" << std::endl
<<" <name>GNSS Track</name>\r\n" << " <name>GNSS Track</name>" << std::endl
<<" <description>GNSS-SDR Receiver position log file created at "<<asctime (timeinfo) << " <description>GNSS-SDR Receiver position log file created at " << asctime (timeinfo)
<<" </description>\r\n" << " </description>" << std::endl
<<"<Style id=\"yellowLineGreenPoly\">\r\n" << "<Style id=\"yellowLineGreenPoly\">" << std::endl
<<" <LineStyle>\r\n" << " <LineStyle>" << std::endl
<<" <color>7f00ffff</color>\r\n" << " <color>7f00ffff</color>" << std::endl
<<" <width>1</width>\r\n" << " <width>1</width>" << std::endl
<<" </LineStyle>\r\n" << " </LineStyle>" << std::endl
<<"<PolyStyle>\r\n" << "<PolyStyle>" << std::endl
<<" <color>7f00ff00</color>\r\n" << " <color>7f00ff00</color>" << std::endl
<<"</PolyStyle>\r\n" << "</PolyStyle>" << std::endl
<<"</Style>\r\n" << "</Style>" << std::endl
<<"<Placemark>\r\n" << "<Placemark>" << std::endl
<<"<name>GNSS-SDR PVT</name>\r\n" << "<name>GNSS-SDR PVT</name>" << std::endl
<<"<description>GNSS-SDR position log</description>\r\n" << "<description>GNSS-SDR position log</description>" << std::endl
<<"<styleUrl>#yellowLineGreenPoly</styleUrl>\r\n" << "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
<<"<LineString>\r\n" << "<LineString>" << std::endl
<<"<extrude>0</extrude>\r\n" << "<extrude>0</extrude>" << std::endl
<<"<tessellate>1</tessellate>\r\n" << "<tessellate>1</tessellate>" << std::endl
<<"<altitudeMode>absolute</altitudeMode>\r\n" << "<altitudeMode>absolute</altitudeMode>" << std::endl
<<"<coordinates>\r\n"; << "<coordinates>" << std::endl;
return true; return true;
} }
else else
@ -84,7 +84,7 @@ bool kml_printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v
double latitude; double latitude;
double longitude; double longitude;
double height; double height;
if (print_average_values==false) if (print_average_values == false)
{ {
latitude=position->d_latitude_d; latitude=position->d_latitude_d;
longitude=position->d_longitude_d; longitude=position->d_longitude_d;
@ -96,9 +96,10 @@ bool kml_printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v
longitude=position->d_avg_longitude_d; longitude=position->d_avg_longitude_d;
height=position->d_avg_height_m; height=position->d_avg_height_m;
} }
if (kml_file.is_open()) if (kml_file.is_open())
{ {
kml_file<<longitude<<","<<latitude<<","<<height<<"\r\n"; kml_file << longitude << "," << latitude << "," << height << std::endl;
return true; return true;
} }
else else
@ -111,10 +112,10 @@ bool kml_printer::close_file()
{ {
if (kml_file.is_open()) if (kml_file.is_open())
{ {
kml_file<<"</coordinates>\r\n" kml_file<<"</coordinates>" << std::endl
<<"</LineString>\r\n" <<"</LineString>" << std::endl
<<"</Placemark>\r\n" <<"</Placemark>" << std::endl
<<"</Document>\r\n" <<"</Document>" << std::endl
<<"</kml>"; <<"</kml>";
kml_file.close(); kml_file.close();
return true; return true;

18
src/algorithms/PVT/libs/rinex_printer.cc
View File

@ -264,12 +264,12 @@ std::string Rinex_Printer::getLocalTime()
std::stringstream strmHour; std::stringstream strmHour;
int utc_hour = pt_tm.tm_hour; int utc_hour = pt_tm.tm_hour;
if (utc_hour<10) strmHour << "0"; // two digits for hours if (utc_hour < 10) strmHour << "0"; // two digits for hours
strmHour << utc_hour; strmHour << utc_hour;
std::stringstream strmMin; std::stringstream strmMin;
int utc_minute = pt_tm.tm_min; int utc_minute = pt_tm.tm_min;
if (utc_minute<10) strmMin << "0"; // two digits for minutes if (utc_minute < 10) strmMin << "0"; // two digits for minutes
strmMin << utc_minute; strmMin << utc_minute;
if (version == 2) if (version == 2)
@ -371,7 +371,7 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, gps_navigation_message
// -------- Line COMMENT // -------- Line COMMENT
line.clear(); line.clear();
line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60);
line += Rinex_Printer::leftJustify("COMMENT",20); line += Rinex_Printer::leftJustify("COMMENT", 20);
Rinex_Printer::lengthCheck(line); Rinex_Printer::lengthCheck(line);
out << line << std::endl; out << line << std::endl;
@ -547,7 +547,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, gps_navigation_message nav
line += satelliteSystem["GPS"]; line += satelliteSystem["GPS"];
if (nav_msg.i_satellite_PRN < 10) line += std::string("0"); if (nav_msg.i_satellite_PRN < 10) line += std::string("0");
line += boost::lexical_cast<std::string>(nav_msg.i_satellite_PRN); line += boost::lexical_cast<std::string>(nav_msg.i_satellite_PRN);
std::string year (timestring,0,4); std::string year (timestring, 0, 4);
line += std::string(1, ' '); line += std::string(1, ' ');
line += year; line += year;
line += std::string(1, ' '); line += std::string(1, ' ');
@ -932,13 +932,13 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, gps_navigation_message
std::string hour (timestring, 9, 2); std::string hour (timestring, 9, 2);
std::string minutes (timestring, 11, 2); std::string minutes (timestring, 11, 2);
double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW)); double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW));
double seconds = fmod(utc_t,60); double seconds = fmod(utc_t, 60);
line += Rinex_Printer::rightJustify(year, 6); line += Rinex_Printer::rightJustify(year, 6);
line += Rinex_Printer::rightJustify(month, 6); line += Rinex_Printer::rightJustify(month, 6);
line += Rinex_Printer::rightJustify(day, 6); line += Rinex_Printer::rightJustify(day, 6);
line += Rinex_Printer::rightJustify(hour, 6); line += Rinex_Printer::rightJustify(hour, 6);
line += Rinex_Printer::rightJustify(minutes, 6); line += Rinex_Printer::rightJustify(minutes, 6);
line += Rinex_Printer::rightJustify(asString(seconds,7), 13); line += Rinex_Printer::rightJustify(asString(seconds, 7), 13);
line += Rinex_Printer::rightJustify(std::string("GPS"), 8); line += Rinex_Printer::rightJustify(std::string("GPS"), 8);
line += std::string(9, ' '); line += std::string(9, ' ');
line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20);
@ -965,7 +965,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, gps_navigation_message nav
if (version == 2) if (version == 2)
{ {
line += "OBSERVATION DATA FILE FOR VERSION 2.11 STILL NOT IMPLEMENTED"; line += "OBSERVATION DATA FILE FOR VERSION 2.11 STILL NOT IMPLEMENTED";
line += std::string(80-line.size(), ' '); line += std::string(80 - line.size(), ' ');
Rinex_Printer::lengthCheck(line); Rinex_Printer::lengthCheck(line);
out << line << std::endl; out << line << std::endl;
} }
@ -995,7 +995,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, gps_navigation_message nav
line += std::string(1, ' '); line += std::string(1, ' ');
double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW)); double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(nav_msg.d_TOW));
line += Rinex_Printer::asString(fmod(utc_t,60), 7); line += Rinex_Printer::asString(fmod(utc_t, 60), 7);
line += std::string(2, ' '); line += std::string(2, ' ');
// Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event
line += std::string(1, '0'); line += std::string(1, '0');
@ -1051,7 +1051,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, gps_navigation_message nav
{ {
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<short>(ssi), 1); lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<short>(ssi), 1);
} }
if (lineObs.size()<80) lineObs += std::string(80 - lineObs.size(), ' '); if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' ');
out << lineObs << std::endl; out << lineObs << std::endl;
} }
} }

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

@ -72,6 +72,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
repeat_ = configuration->property("Acquisition" + boost::lexical_cast< repeat_ = configuration->property("Acquisition" + boost::lexical_cast<
std::string>(channel_) + ".repeat_satellite", false); std::string>(channel_) + ".repeat_satellite", false);
DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_ DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_
<< std::endl; << std::endl;
@ -139,8 +140,8 @@ void Channel::disconnect(gr_top_block_sptr top_block)
return; return;
} }
top_block->disconnect(acq_->get_right_block(), 0, trk_->get_left_block(),0); top_block->disconnect(acq_->get_right_block(), 0, trk_->get_left_block(), 0);
top_block->disconnect(trk_->get_right_block(), 0, nav_->get_left_block(),0); top_block->disconnect(trk_->get_right_block(), 0, nav_->get_left_block(), 0);
acq_->disconnect(top_block); acq_->disconnect(top_block);
trk_->disconnect(top_block); trk_->disconnect(top_block);
@ -160,7 +161,6 @@ gr_basic_block_sptr Channel::get_left_block()
gr_basic_block_sptr Channel::get_right_block() gr_basic_block_sptr Channel::get_right_block()
{ {
return nav_->get_right_block(); return nav_->get_right_block();
} }

92
src/algorithms/channel/libs/gps_l1_ca_channel_fsm.cc
View File

@ -34,33 +34,26 @@
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
struct Ev_gps_channel_start_acquisition: sc::event< struct Ev_gps_channel_start_acquisition: sc::event<Ev_gps_channel_start_acquisition>
Ev_gps_channel_start_acquisition>
{}; {};
struct Ev_gps_channel_valid_acquisition: sc::event< struct Ev_gps_channel_valid_acquisition: sc::event<Ev_gps_channel_valid_acquisition>
Ev_gps_channel_valid_acquisition>
{}; {};
struct Ev_gps_channel_failed_acquisition_repeat: sc::event< struct Ev_gps_channel_failed_acquisition_repeat: sc::event<Ev_gps_channel_failed_acquisition_repeat>
Ev_gps_channel_failed_acquisition_repeat>
{}; {};
struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event< struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event<Ev_gps_channel_failed_acquisition_no_repeat>
Ev_gps_channel_failed_acquisition_no_repeat>
{}; {};
struct Ev_gps_channel_failed_tracking: sc::event< struct Ev_gps_channel_failed_tracking: sc::event<Ev_gps_channel_failed_tracking>
Ev_gps_channel_failed_tracking>
{}; {};
struct gps_channel_idle_fsm_S0: public sc::state<gps_channel_idle_fsm_S0, struct gps_channel_idle_fsm_S0: public sc::state<gps_channel_idle_fsm_S0,GpsL1CaChannelFsm>
GpsL1CaChannelFsm>
{ {
public: public:
// sc::transition(event, next state) // sc::transition(event, next state)
typedef sc::transition<Ev_gps_channel_start_acquisition, typedef sc::transition<Ev_gps_channel_start_acquisition, gps_channel_acquiring_fsm_S1> reactions;
gps_channel_acquiring_fsm_S1> reactions;
gps_channel_idle_fsm_S0(my_context ctx) : gps_channel_idle_fsm_S0(my_context ctx) :
my_base(ctx) my_base(ctx)
{ {
@ -68,72 +61,74 @@ public:
} }
}; };
struct gps_channel_acquiring_fsm_S1: public sc::state<
gps_channel_acquiring_fsm_S1, GpsL1CaChannelFsm> struct gps_channel_acquiring_fsm_S1: public sc::state<gps_channel_acquiring_fsm_S1, GpsL1CaChannelFsm>
{ {
public: public:
typedef mpl::list<sc::transition< typedef mpl::list<sc::transition<Ev_gps_channel_failed_acquisition_no_repeat, gps_channel_waiting_fsm_S3>,
Ev_gps_channel_failed_acquisition_no_repeat, sc::transition<Ev_gps_channel_failed_acquisition_repeat, gps_channel_acquiring_fsm_S1>,
gps_channel_waiting_fsm_S3>, sc::transition< sc::transition<Ev_gps_channel_valid_acquisition, gps_channel_tracking_fsm_S2> >reactions;
Ev_gps_channel_failed_acquisition_repeat,
gps_channel_acquiring_fsm_S1>, sc::transition<
Ev_gps_channel_valid_acquisition, gps_channel_tracking_fsm_S2> >
reactions;
gps_channel_acquiring_fsm_S1(my_context ctx) : gps_channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx)
my_base(ctx)
{ {
//std::cout << "Enter Channel_Acq_S1 " << std::endl; //std::cout << "Enter Channel_Acq_S1 " << std::endl;
context<GpsL1CaChannelFsm> ().start_acquisition(); context<GpsL1CaChannelFsm> ().start_acquisition();
} }
}; };
struct gps_channel_tracking_fsm_S2: public sc::state<
gps_channel_tracking_fsm_S2, GpsL1CaChannelFsm>
struct gps_channel_tracking_fsm_S2: public sc::state<gps_channel_tracking_fsm_S2, GpsL1CaChannelFsm>
{ {
public: public:
typedef sc::transition<Ev_gps_channel_failed_tracking, typedef sc::transition<Ev_gps_channel_failed_tracking, gps_channel_acquiring_fsm_S1> reactions;
gps_channel_acquiring_fsm_S1> reactions;
gps_channel_tracking_fsm_S2(my_context ctx) : gps_channel_tracking_fsm_S2(my_context ctx) : my_base(ctx)
my_base(ctx)
{ {
//std::cout << "Enter Channel_tracking_S2 " << std::endl; //std::cout << "Enter Channel_tracking_S2 " << std::endl;
context<GpsL1CaChannelFsm> ().start_tracking(); context<GpsL1CaChannelFsm> ().start_tracking();
} }
}; };
struct gps_channel_waiting_fsm_S3: public sc::state<
gps_channel_waiting_fsm_S3, GpsL1CaChannelFsm>
struct gps_channel_waiting_fsm_S3: public sc::state<gps_channel_waiting_fsm_S3, GpsL1CaChannelFsm>
{ {
public: public:
typedef sc::transition<Ev_gps_channel_start_acquisition, typedef sc::transition<Ev_gps_channel_start_acquisition, gps_channel_acquiring_fsm_S1> reactions;
gps_channel_acquiring_fsm_S1> reactions;
gps_channel_waiting_fsm_S3(my_context ctx) : gps_channel_waiting_fsm_S3(my_context ctx) : my_base(ctx)
my_base(ctx)
{ {
//std::cout << "Enter Channel_waiting_S3 " << std::endl; //std::cout << "Enter Channel_waiting_S3 " << std::endl;
context<GpsL1CaChannelFsm> ().request_satellite(); context<GpsL1CaChannelFsm> ().request_satellite();
} }
}; };
GpsL1CaChannelFsm::GpsL1CaChannelFsm() GpsL1CaChannelFsm::GpsL1CaChannelFsm()
{ {
initiate(); //start the FSM initiate(); //start the FSM
} }
GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) :
acq_(acquisition)
GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) : acq_(acquisition)
{ {
initiate(); //start the FSM initiate(); //start the FSM
} }
void GpsL1CaChannelFsm::Event_gps_start_acquisition() void GpsL1CaChannelFsm::Event_gps_start_acquisition()
{ {
this->process_event(Ev_gps_channel_start_acquisition()); this->process_event(Ev_gps_channel_start_acquisition());
} }
void GpsL1CaChannelFsm::Event_gps_valid_acquisition() void GpsL1CaChannelFsm::Event_gps_valid_acquisition()
{ {
this->process_event(Ev_gps_channel_valid_acquisition()); this->process_event(Ev_gps_channel_valid_acquisition());
@ -144,40 +139,56 @@ void GpsL1CaChannelFsm::Event_gps_failed_acquisition_repeat()
this->process_event(Ev_gps_channel_failed_acquisition_repeat()); this->process_event(Ev_gps_channel_failed_acquisition_repeat());
} }
void GpsL1CaChannelFsm::Event_gps_failed_acquisition_no_repeat() void GpsL1CaChannelFsm::Event_gps_failed_acquisition_no_repeat()
{ {
this->process_event(Ev_gps_channel_failed_acquisition_no_repeat()); this->process_event(Ev_gps_channel_failed_acquisition_no_repeat());
} }
void GpsL1CaChannelFsm::Event_gps_failed_tracking() void GpsL1CaChannelFsm::Event_gps_failed_tracking()
{ {
this->process_event(Ev_gps_channel_failed_tracking()); this->process_event(Ev_gps_channel_failed_tracking());
} }
void GpsL1CaChannelFsm::set_acquisition(AcquisitionInterface *acquisition) void GpsL1CaChannelFsm::set_acquisition(AcquisitionInterface *acquisition)
{ {
acq_ = acquisition; acq_ = acquisition;
} }
void GpsL1CaChannelFsm::set_tracking(TrackingInterface *tracking) void GpsL1CaChannelFsm::set_tracking(TrackingInterface *tracking)
{ {
trk_ = tracking; trk_ = tracking;
} }
void GpsL1CaChannelFsm::set_queue(gr_msg_queue_sptr queue) void GpsL1CaChannelFsm::set_queue(gr_msg_queue_sptr queue)
{ {
queue_ = queue; queue_ = queue;
} }
void GpsL1CaChannelFsm::set_channel(unsigned int channel) void GpsL1CaChannelFsm::set_channel(unsigned int channel)
{ {
channel_ = channel; channel_ = channel;
} }
void GpsL1CaChannelFsm::start_acquisition() void GpsL1CaChannelFsm::start_acquisition()
{ {
acq_->reset(); acq_->reset();
} }
void GpsL1CaChannelFsm::start_tracking() void GpsL1CaChannelFsm::start_tracking()
{ {
LOG_AT_LEVEL(INFO) << "Channel " << channel_ LOG_AT_LEVEL(INFO) << "Channel " << channel_
@ -193,6 +204,8 @@ void GpsL1CaChannelFsm::start_tracking()
trk_->start_tracking(); trk_->start_tracking();
} }
void GpsL1CaChannelFsm::request_satellite() void GpsL1CaChannelFsm::request_satellite()
{ {
ControlMessageFactory* cmf = new ControlMessageFactory(); ControlMessageFactory* cmf = new ControlMessageFactory();
@ -201,6 +214,5 @@ void GpsL1CaChannelFsm::request_satellite()
queue_->handle(cmf->GetQueueMessage(channel_, 0)); queue_->handle(cmf->GetQueueMessage(channel_, 0));
} }
delete cmf; delete cmf;
} }

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

@ -55,14 +55,14 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
{ {
int output_rate_ms; int output_rate_ms;
output_rate_ms=configuration->property(role + ".output_rate_ms", 500); output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
std::string default_dump_filename = "./observables.dat"; std::string default_dump_filename = "./observables.dat";
DLOG(INFO) << "role " << role; DLOG(INFO) << "role " << role;
bool flag_averaging; bool flag_averaging;
flag_averaging=configuration->property(role + ".flag_averaging", false); flag_averaging = configuration->property(role + ".flag_averaging", false);
dump_ = configuration->property(role + ".dump", false); dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);

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

@ -61,23 +61,23 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
d_queue = queue; d_queue = queue;
d_dump = dump; d_dump = dump;
d_nchannels = nchannels; d_nchannels = nchannels;
d_output_rate_ms=output_rate_ms; d_output_rate_ms = output_rate_ms;
d_history_prn_delay_ms= new std::deque<double>[d_nchannels]; d_history_prn_delay_ms = new std::deque<double>[d_nchannels];
d_dump_filename=dump_filename; d_dump_filename = dump_filename;
d_flag_averaging=flag_averaging; d_flag_averaging = flag_averaging;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump==true) if (d_dump == true)
{ {
if (d_dump_file.is_open()==false) if (d_dump_file.is_open() == false)
{ {
try { try {
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"Observables dump enabled Log file: "<<d_dump_filename.c_str()<<std::endl; std::cout << "Observables dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
} }
catch (std::ifstream::failure e) { catch (std::ifstream::failure e) {
std::cout << "Exception opening observables dump file "<<e.what()<<"\r\n"; std::cout << "Exception opening observables dump file " << e.what() << std::endl;
} }
} }
} }
@ -103,7 +103,7 @@ bool pairCompare_double( std::pair<int,double> a, std::pair<int,double> b)
void clearQueue( std::deque<double> &q ) void clearQueue( std::deque<double> &q )
{ {
std::deque<double> empty; std::deque<double> empty;
std::swap( q, empty ); std::swap(q, empty);
} }
@ -127,18 +127,18 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
double pseudoranges_timestamp_ms; double pseudoranges_timestamp_ms;
double traveltime_ms; double traveltime_ms;
double pseudorange_m; double pseudorange_m;
int history_shift=0; int history_shift = 0;
double delta_timestamp_ms; double delta_timestamp_ms;
double min_delta_timestamp_ms; double min_delta_timestamp_ms;
double actual_min_prn_delay_ms; double actual_min_prn_delay_ms;
double current_prn_delay_ms; double current_prn_delay_ms;
int pseudoranges_reference_sat_ID=0; int pseudoranges_reference_sat_ID = 0;
unsigned int pseudoranges_reference_sat_channel_ID=0; unsigned int pseudoranges_reference_sat_channel_ID = 0;
d_sample_counter++; //count for the processed samples d_sample_counter++; //count for the processed samples
bool flag_history_ok=true; //flag to indicate that all the queues have filled their timestamp history bool flag_history_ok = true; //flag to indicate that all the queues have filled their timestamp history
/* /*
* 1. Read the GNSS SYNCHRO objects from available channels to obtain the preamble timestamp, current PRN start time and accumulated carrier phase * 1. Read the GNSS SYNCHRO objects from available channels to obtain the preamble timestamp, current PRN start time and accumulated carrier phase
*/ */
@ -146,12 +146,12 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{ {
if (in[i][0].valid_word) //if this channel have valid word if (in[i][0].valid_word) //if this channel have valid word
{ {
gps_words.insert(std::pair<int,gnss_synchro>(in[i][0].channel_ID,in[i][0])); //record the word structure in a map for pseudoranges gps_words.insert(std::pair<int,gnss_synchro>(in[i][0].channel_ID, in[i][0])); //record the word structure in a map for pseudoranges
// RECORD PRN start timestamps history // RECORD PRN start timestamps history
if (d_history_prn_delay_ms[i].size()<MAX_TOA_DELAY_MS) if (d_history_prn_delay_ms[i].size()<MAX_TOA_DELAY_MS)
{ {
d_history_prn_delay_ms[i].push_front(in[i][0].prn_delay_ms); d_history_prn_delay_ms[i].push_front(in[i][0].prn_delay_ms);
flag_history_ok=false; // at least one channel need more samples flag_history_ok = false; // at least one channel need more samples
} }
else else
{ {
@ -167,34 +167,34 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
*/ */
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i=0; i<d_nchannels ; i++)
{ {
current_gnss_pseudorange.valid=false; current_gnss_pseudorange.valid = false;
current_gnss_pseudorange.SV_ID=0; current_gnss_pseudorange.SV_ID = 0;
current_gnss_pseudorange.pseudorange_m=0; current_gnss_pseudorange.pseudorange_m = 0;
current_gnss_pseudorange.timestamp_ms=0; current_gnss_pseudorange.timestamp_ms = 0;
*out[i]=current_gnss_pseudorange; *out[i] = current_gnss_pseudorange;
} }
/* /*
* 2. Compute RAW pseudorranges: Use only the valid channels (channels that are tracking a satellite) * 2. Compute RAW pseudorranges: Use only the valid channels (channels that are tracking a satellite)
*/ */
if(gps_words.size()>0 and flag_history_ok==true) if(gps_words.size() > 0 and flag_history_ok == true)
{ {
/* /*
* 2.1 find the minimum preamble timestamp (nearest satellite, will be the reference) * 2.1 find the minimum preamble timestamp (nearest satellite, will be the reference)
*/ */
// The nearest satellite, first preamble to arrive // The nearest satellite, first preamble to arrive
gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare_gnss_synchro); gps_words_iter = min_element(gps_words.begin(), gps_words.end(), pairCompare_gnss_synchro);
min_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; //[ms] min_preamble_delay_ms = gps_words_iter->second.preamble_delay_ms; //[ms]
pseudoranges_reference_sat_ID=gps_words_iter->second.satellite_PRN; // it is the reference! pseudoranges_reference_sat_ID = gps_words_iter->second.satellite_PRN; // it is the reference!
pseudoranges_reference_sat_channel_ID=gps_words_iter->second.channel_ID; pseudoranges_reference_sat_channel_ID = gps_words_iter->second.channel_ID;
// The farthest satellite, last preamble to arrive // The farthest satellite, last preamble to arrive
gps_words_iter=max_element(gps_words.begin(),gps_words.end(),pairCompare_gnss_synchro); gps_words_iter = max_element(gps_words.begin(), gps_words.end(), pairCompare_gnss_synchro);
max_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; max_preamble_delay_ms = gps_words_iter->second.preamble_delay_ms;
min_delta_timestamp_ms=gps_words_iter->second.prn_delay_ms-max_preamble_delay_ms; //[ms] min_delta_timestamp_ms = gps_words_iter->second.prn_delay_ms - max_preamble_delay_ms; //[ms]
// check if this is a valid set of observations // check if this is a valid set of observations
if ((max_preamble_delay_ms-min_preamble_delay_ms)<MAX_TOA_DELAY_MS) if ((max_preamble_delay_ms - min_preamble_delay_ms) < MAX_TOA_DELAY_MS)
{ {
// Now we have to determine were we are in time, compared with the last preamble! -> we select the corresponding history // Now we have to determine were we are in time, compared with the last preamble! -> we select the corresponding history
/*! /*!
@ -204,10 +204,10 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
// find again the minimum CURRENT minimum preamble time, taking into account the preamble timeshift // find again the minimum CURRENT minimum preamble time, taking into account the preamble timeshift
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++) for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{ {
delta_timestamp_ms=(gps_words_iter->second.prn_delay_ms-gps_words_iter->second.preamble_delay_ms)-min_delta_timestamp_ms; delta_timestamp_ms = (gps_words_iter->second.prn_delay_ms - gps_words_iter->second.preamble_delay_ms) - min_delta_timestamp_ms;
history_shift=round(delta_timestamp_ms); history_shift = round(delta_timestamp_ms);
//std::cout<<"history_shift="<<history_shift<<"\r\n"; //std::cout<<"history_shift="<<history_shift<<"\r\n";
current_prn_timestamps_ms.insert(std::pair<int,double>(gps_words_iter->second.channel_ID,d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift])); current_prn_timestamps_ms.insert(std::pair<int,double>(gps_words_iter->second.channel_ID, d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]));
// debug: preamble position test // debug: preamble position test
//if ((d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]-gps_words_iter->second.preamble_delay_ms)<0.1) //if ((d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]-gps_words_iter->second.preamble_delay_ms)<0.1)
//{std::cout<<"ch "<<gps_words_iter->second.channel_ID<<" current_prn_time-last_preamble_prn_time="<< //{std::cout<<"ch "<<gps_words_iter->second.channel_ID<<" current_prn_time-last_preamble_prn_time="<<
@ -221,13 +221,12 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
//{ //{
//std::cout<<"PREAMBLE NAVIGATION NOW!\r\n"; //std::cout<<"PREAMBLE NAVIGATION NOW!\r\n";
//d_sample_counter=0; //d_sample_counter=0;
//} //}
current_prn_timestamps_ms_iter=min_element(current_prn_timestamps_ms.begin(),current_prn_timestamps_ms.end(),pairCompare_double); current_prn_timestamps_ms_iter = min_element(current_prn_timestamps_ms.begin(), current_prn_timestamps_ms.end(), pairCompare_double);
actual_min_prn_delay_ms=current_prn_timestamps_ms_iter->second; actual_min_prn_delay_ms = current_prn_timestamps_ms_iter->second;
pseudoranges_timestamp_ms=actual_min_prn_delay_ms; //save the shortest pseudorange timestamp to compute the current GNSS timestamp pseudoranges_timestamp_ms = actual_min_prn_delay_ms; //save the shortest pseudorange timestamp to compute the current GNSS timestamp
/* /*
* 2.2 compute the pseudoranges * 2.2 compute the pseudoranges
*/ */
@ -236,53 +235,54 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{ {
// #### compute the pseudorange for this satellite ### // #### compute the pseudorange for this satellite ###
current_prn_delay_ms=current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID); current_prn_delay_ms = current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID);
traveltime_ms=current_prn_delay_ms-actual_min_prn_delay_ms+GPS_STARTOFFSET_ms; //[ms] traveltime_ms = current_prn_delay_ms - actual_min_prn_delay_ms + GPS_STARTOFFSET_ms; //[ms]
//std::cout<<"delta_time_ms="<<current_prn_delay_ms-actual_min_prn_delay_ms<<"\r\n"; //std::cout<<"delta_time_ms="<<current_prn_delay_ms-actual_min_prn_delay_ms<<"\r\n";
pseudorange_m=traveltime_ms*GPS_C_m_ms; // [m] pseudorange_m = traveltime_ms*GPS_C_m_ms; // [m]
// update the pseudorange object // update the pseudorange object
current_gnss_pseudorange.pseudorange_m=pseudorange_m; current_gnss_pseudorange.pseudorange_m = pseudorange_m;
current_gnss_pseudorange.timestamp_ms=pseudoranges_timestamp_ms; current_gnss_pseudorange.timestamp_ms = pseudoranges_timestamp_ms;
current_gnss_pseudorange.SV_ID=gps_words_iter->second.satellite_PRN; current_gnss_pseudorange.SV_ID = gps_words_iter->second.satellite_PRN;
current_gnss_pseudorange.valid=true; current_gnss_pseudorange.valid = true;
// #### write the pseudorrange block output for this satellite ### // #### write the pseudorrange block output for this satellite ###
*out[gps_words_iter->second.channel_ID]=current_gnss_pseudorange; *out[gps_words_iter->second.channel_ID] = current_gnss_pseudorange;
} }
} }
} }
if(d_dump==true) { if(d_dump == true) {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try { try {
double tmp_double; double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i=0; i<d_nchannels ; i++)
{ {
tmp_double=in[i][0].preamble_delay_ms; tmp_double = in[i][0].preamble_delay_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=in[i][0].prn_delay_ms; tmp_double = in[i][0].prn_delay_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=out[i][0].pseudorange_m; tmp_double = out[i][0].pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=out[i][0].timestamp_ms; tmp_double = out[i][0].timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=out[i][0].SV_ID; tmp_double = out[i][0].SV_ID;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
} }
} }
catch (std::ifstream::failure e) { catch (std::ifstream::failure e)
std::cout << "Exception writing observables dump file "<<e.what()<<"\r\n"; {
std::cout << "Exception writing observables dump file "<< e.what() << std::endl;
} }
} }
consume_each(1); //one by one consume_each(1); //one by one
if ((d_sample_counter%d_output_rate_ms)==0) if ((d_sample_counter % d_output_rate_ms) == 0)
{ {
return 1; //Output the observables return 1; //Output the observables
} }
else else
{ {
return 0;//hold on return 0; //hold on
} }
} }

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

@ -58,16 +58,18 @@ gps_l1_ca_make_telemetry_decoder_cc(unsigned int satellite, long if_freq, long f
fs_in, vector_length, queue, dump)); fs_in, vector_length, queue, dump));
} }
void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required)
void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{ {
for (unsigned i = 0; i < 3; i++) for (unsigned i = 0; i < 3; i++)
{ {
ninput_items_required[i] =d_samples_per_bit*8; //set the required sample history ninput_items_required[i] = d_samples_per_bit*8; //set the required sample history
} }
} }
gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int satellite, long if_freq, long fs_in, unsigned 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) : int vector_length, gr_msg_queue_sptr queue, bool dump) :
gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(double)), gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(double)),
@ -78,61 +80,60 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int sate
d_dump = dump; d_dump = dump;
d_satellite = satellite; d_satellite = satellite;
d_vector_length = vector_length; d_vector_length = vector_length;
d_samples_per_bit=20; // it is exactly 1000*(1/50)=20 d_samples_per_bit = 20; // it is exactly 1000*(1/50)=20
d_fs_in=fs_in; d_fs_in = fs_in;
d_preamble_duration_seconds=(1.0/(float)GPS_CA_TELEMETRY_RATE_BITS_SECOND)*(float)GPS_CA_PREAMBLE_LENGTH_BITS; d_preamble_duration_seconds = (1.0/(float)GPS_CA_TELEMETRY_RATE_BITS_SECOND)*(float)GPS_CA_PREAMBLE_LENGTH_BITS;
//std::cout<<"d_preamble_duration_seconds="<<d_preamble_duration_seconds<<"\r\n"; //std::cout<<"d_preamble_duration_seconds="<<d_preamble_duration_seconds<<"\r\n";
// set the preamble // set the preamble
unsigned short int preambles_bits[8]=GPS_PREAMBLE; unsigned short int preambles_bits[8] = GPS_PREAMBLE;
memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, 8* sizeof(unsigned short int)); memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, 8*sizeof(unsigned short int));
// preamble bits to sampled symbols // preamble bits to sampled symbols
d_preambles_symbols=(signed int*)malloc(sizeof(signed int)*8*d_samples_per_bit); d_preambles_symbols = (signed int*)malloc(sizeof(signed int)*8*d_samples_per_bit);
int n=0; int n = 0;
for (int i=0;i<8;i++) for (int i=0; i<8; i++)
{ {
for (unsigned int j=0;j<d_samples_per_bit;j++) for (unsigned int j=0; j<d_samples_per_bit; j++)
{ {
if (d_preambles_bits[i]==1) if (d_preambles_bits[i] == 1)
{ {
d_preambles_symbols[n]=1; d_preambles_symbols[n] = 1;
} }
else else
{ {
d_preambles_symbols[n]=-1; d_preambles_symbols[n] = -1;
} }
n++; n++;
} }
} }
d_sample_counter=0; d_sample_counter = 0;
d_preamble_code_phase_seconds=0; d_preamble_code_phase_seconds = 0;
d_stat=0; d_stat = 0;
d_preamble_index=0; d_preamble_index = 0;
d_symbol_accumulator_counter=0; d_symbol_accumulator_counter = 0;
d_frame_bit_index=0; d_frame_bit_index = 0;
d_flag_frame_sync=false; d_flag_frame_sync = false;
d_GPS_frame_4bytes=0; d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes=0; d_prev_GPS_frame_4bytes = 0;
d_flag_parity=false; d_flag_parity = false;
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble //set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
} }
gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc() gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc()
{ {
delete d_preambles_symbols; delete d_preambles_symbols;
d_dump_file.close(); d_dump_file.close();
} }
bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword) bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
{ {
unsigned int d1,d2,d3,d4,d5,d6,d7,t,parity; unsigned int d1, d2, d3, d4, d5, d6, d7, t, parity;
/* XOR as many bits in parallel as possible. The magic constants pick /* XOR as many bits in parallel as possible. The magic constants pick
up bits which are to be XOR'ed together to implement the GPS parity up bits which are to be XOR'ed together to implement the GPS parity
@ -150,19 +151,19 @@ bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7; t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
// Now XOR the 5 6-bit fields together to produce the 6-bit final result. // Now XOR the 5 6-bit fields together to produce the 6-bit final result.
parity = t ^ _lrotl(t,6) ^ _lrotl(t,12) ^ _lrotl(t,18) ^ _lrotl(t,24); parity = t ^ _lrotl(t,6) ^ _lrotl(t,12) ^ _lrotl(t,18) ^ _lrotl(t,24);
parity = parity & 0x3F; parity = parity & 0x3F;
if (parity == (gpsword&0x3F)) if (parity == (gpsword&0x3F)) return(true);
return(true); else return(false);
else
return(false);
} }
int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
int corr_value=0; {
int corr_value = 0;
int preamble_diff; int preamble_diff;
gnss_synchro gps_synchro; //structure to save the synchronization information gnss_synchro gps_synchro; //structure to save the synchronization information
@ -198,15 +199,15 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
// } // }
// TODO Optimize me! // TODO Optimize me!
//******* preamble correlation ******** //******* preamble correlation ********
for (unsigned int i=0;i<d_samples_per_bit*8;i++) for (unsigned int i=0; i<d_samples_per_bit*8; i++)
{ {
if (in[1][i] < 0) // symbols clipping if (in[1][i] < 0) // symbols clipping
{ {
corr_value-=d_preambles_symbols[i]; corr_value -= d_preambles_symbols[i];
} }
else else
{ {
corr_value+=d_preambles_symbols[i]; corr_value += d_preambles_symbols[i];
} }
} }
d_flag_preamble=false; d_flag_preamble=false;
@ -214,45 +215,45 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if (abs(corr_value)>=160) if (abs(corr_value)>=160)
{ {
//TODO: Rewrite with state machine //TODO: Rewrite with state machine
if (d_stat==0) if (d_stat == 0)
{ {
d_GPS_FSM.Event_gps_word_preamble(); 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
std::cout<<"Preamble detection for SAT "<<d_satellite<<std::endl; std::cout << "Preamble detection for SAT " << d_satellite<< std::endl;
d_symbol_accumulator=0; //sync the symbol to bits integrator d_symbol_accumulator = 0; //sync the symbol to bits integrator
d_symbol_accumulator_counter=0; d_symbol_accumulator_counter = 0;
d_frame_bit_index=8; d_frame_bit_index = 8;
d_stat=1; // enter into frame pre-detection status d_stat = 1; // enter into frame pre-detection status
} }
else if (d_stat==1) //check 6 seconds of preample separation else if (d_stat == 1) //check 6 seconds of preample separation
{ {
preamble_diff=abs(d_sample_counter-d_preamble_index); preamble_diff=abs(d_sample_counter-d_preamble_index);
if (abs(preamble_diff-6000)<1) if (abs(preamble_diff - 6000) < 1)
{ {
d_GPS_FSM.Event_gps_word_preamble(); d_GPS_FSM.Event_gps_word_preamble();
d_flag_preamble=true; d_flag_preamble = true;
d_preamble_index=d_sample_counter;//record the preamble sample stamp (t_P) d_preamble_index = d_sample_counter;//record the preamble sample stamp (t_P)
d_preamble_time_seconds=in[2][0]-d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble d_preamble_time_seconds = in[2][0] - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble
d_preamble_code_phase_seconds=in[4][0]; d_preamble_code_phase_seconds = in[4][0];
if (!d_flag_frame_sync) if (!d_flag_frame_sync)
{ {
d_flag_frame_sync=true; d_flag_frame_sync = true;
std::cout<<" Frame sync SAT "<<d_satellite<<" with preamble start at "<<d_preamble_time_seconds<<" [s]"<<std::endl; std::cout <<" Frame sync SAT " << d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]" << std::endl;
} }
} }
} }
} }
else else
{ {
if (d_stat==1) if (d_stat == 1)
{ {
preamble_diff=d_sample_counter-d_preamble_index; preamble_diff = d_sample_counter - d_preamble_index;
if (preamble_diff>6001) if (preamble_diff > 6001)
{ {
std::cout<<"Lost of frame sync SAT "<<this->d_satellite<<" preamble_diff= "<<preamble_diff<<std::endl; std::cout << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff << std::endl;
d_stat=0; //lost of frame sync d_stat = 0; //lost of frame sync
d_flag_frame_sync=false; d_flag_frame_sync = false;
} }
} }
} }
@ -261,22 +262,22 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
//d_preamble_phase-=in[3][0]; //d_preamble_phase-=in[3][0];
//******* SYMBOL TO BIT ******* //******* SYMBOL TO BIT *******
d_symbol_accumulator+=in[1][d_samples_per_bit*8-1]; // accumulate the input value in d_symbol_accumulator d_symbol_accumulator += in[1][d_samples_per_bit*8 - 1]; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator_counter++; d_symbol_accumulator_counter++;
if (d_symbol_accumulator_counter==20) if (d_symbol_accumulator_counter == 20)
{ {
if (d_symbol_accumulator>0) if (d_symbol_accumulator > 0)
{ //symbol to bit { //symbol to bit
d_GPS_frame_4bytes+=1; //insert the telemetry bit in LSB d_GPS_frame_4bytes +=1; //insert the telemetry bit in LSB
} }
d_symbol_accumulator=0; d_symbol_accumulator = 0;
d_symbol_accumulator_counter=0; d_symbol_accumulator_counter = 0;
//******* bits to words ****** //******* bits to words ******
d_frame_bit_index++; d_frame_bit_index++;
if (d_frame_bit_index==30) if (d_frame_bit_index == 30)
{ {
d_frame_bit_index=0; d_frame_bit_index = 0;
//parity check //parity check
//Each word in wordbuff is composed of: //Each word in wordbuff is composed of:
// Bits 0 to 29 = the GPS data word // Bits 0 to 29 = the GPS data word
@ -284,11 +285,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
// prepare the extended frame [-2 -1 0 ... 30] // prepare the extended frame [-2 -1 0 ... 30]
if (d_prev_GPS_frame_4bytes & 0x00000001) if (d_prev_GPS_frame_4bytes & 0x00000001)
{ {
d_GPS_frame_4bytes=d_GPS_frame_4bytes|0x40000000; d_GPS_frame_4bytes = d_GPS_frame_4bytes|0x40000000;
} }
if (d_prev_GPS_frame_4bytes & 0x00000002) if (d_prev_GPS_frame_4bytes & 0x00000002)
{ {
d_GPS_frame_4bytes=d_GPS_frame_4bytes|0x80000000; d_GPS_frame_4bytes = d_GPS_frame_4bytes|0x80000000;
} }
/* Check that the 2 most recently logged words pass parity. Have to first /* Check that the 2 most recently logged words pass parity. Have to first
invert the data bits according to bit 30 of the previous word. */ invert the data bits according to bit 30 of the previous word. */
@ -299,17 +300,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes)) if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes))
{ {
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes,&d_GPS_frame_4bytes,sizeof(char)*4); memcpy(&d_GPS_FSM.d_GPS_frame_4bytes,&d_GPS_frame_4bytes,sizeof(char)*4);
d_GPS_FSM.d_preamble_time_ms=d_preamble_time_seconds*1000.0; d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds*1000.0;
d_GPS_FSM.Event_gps_word_valid(); d_GPS_FSM.Event_gps_word_valid();
d_flag_parity=true; d_flag_parity = true;
} }
else else
{ {
d_GPS_FSM.Event_gps_word_invalid(); d_GPS_FSM.Event_gps_word_invalid();
d_flag_parity=false; d_flag_parity = false;
} }
d_prev_GPS_frame_4bytes=d_GPS_frame_4bytes; // save the actual frame d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame
d_GPS_frame_4bytes=d_GPS_frame_4bytes & 0; d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0;
} }
else else
{ {
@ -320,15 +321,15 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
// output the frame // output the frame
consume_each(1); //one by one consume_each(1); //one by one
gps_synchro.valid_word=(d_flag_frame_sync==true and d_flag_parity==true); gps_synchro.valid_word = (d_flag_frame_sync == true and d_flag_parity == true);
gps_synchro.flag_preamble=d_flag_preamble; gps_synchro.flag_preamble = d_flag_preamble;
gps_synchro.preamble_delay_ms=d_preamble_time_seconds*1000.0; gps_synchro.preamble_delay_ms = d_preamble_time_seconds*1000.0;
gps_synchro.prn_delay_ms=(in[2][0]-d_preamble_duration_seconds)*1000.0; gps_synchro.prn_delay_ms = (in[2][0] - d_preamble_duration_seconds)*1000.0;
gps_synchro.preamble_code_phase_ms=d_preamble_code_phase_seconds*1000.0; gps_synchro.preamble_code_phase_ms = d_preamble_code_phase_seconds*1000.0;
gps_synchro.preamble_code_phase_correction_ms=(in[4][0]-d_preamble_code_phase_seconds)*1000.0; gps_synchro.preamble_code_phase_correction_ms = (in[4][0] - d_preamble_code_phase_seconds)*1000.0;
gps_synchro.satellite_PRN=d_satellite; gps_synchro.satellite_PRN = d_satellite;
gps_synchro.channel_ID=d_channel; gps_synchro.channel_ID = d_channel;
*out[0]=gps_synchro; *out[0] = gps_synchro;
return 1; return 1;
} }
@ -336,14 +337,14 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
void gps_l1_ca_telemetry_decoder_cc::set_satellite(int satellite) void gps_l1_ca_telemetry_decoder_cc::set_satellite(int satellite)
{ {
d_satellite = satellite; d_satellite = satellite;
d_GPS_FSM.i_satellite_PRN=satellite; d_GPS_FSM.i_satellite_PRN = satellite;
LOG_AT_LEVEL(INFO) << "Navigation Satellite set to " << d_satellite; LOG_AT_LEVEL(INFO) << "Navigation Satellite set to " << d_satellite;
} }
void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel) void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
{ {
d_channel = channel; d_channel = channel;
d_GPS_FSM.i_channel_ID=channel; d_GPS_FSM.i_channel_ID = channel;
LOG_AT_LEVEL(INFO) << "Navigation channel set to " << channel; LOG_AT_LEVEL(INFO) << "Navigation channel set to " << channel;
} }

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

@ -64,205 +64,221 @@ using google::LogMessage;
gps_l1_ca_dll_pll_tracking_cc_sptr gps_l1_ca_dll_pll_tracking_cc_sptr
gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) { int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips)
{
return gps_l1_ca_dll_pll_tracking_cc_sptr(new gps_l1_ca_dll_pll_tracking_cc(satellite, if_freq, return gps_l1_ca_dll_pll_tracking_cc_sptr(new gps_l1_ca_dll_pll_tracking_cc(satellite, if_freq,
fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips)); fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips));
} }
void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items, void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required){ gr_vector_int &ninput_items_required)
ninput_items_required[0] =(int)d_vector_length*2; //set the required available samples in each call {
ninput_items_required[0] = (int)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 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, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) : int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
gr_block ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)), 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(double))) { gr_make_io_signature(5, 5, sizeof(double)))
{
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)), //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_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars // initialize internal vars
d_queue = queue; d_queue = queue;
d_dump = dump; d_dump = dump;
d_satellite = satellite; d_satellite = satellite;
d_if_freq = if_freq; d_if_freq = if_freq;
d_fs_in = fs_in; d_fs_in = fs_in;
d_vector_length = vector_length; d_vector_length = vector_length;
d_dump_filename =dump_filename; d_dump_filename = dump_filename;
// Initialize tracking ========================================== // Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
//--- DLL variables -------------------------------------------------------- //--- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip // Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code=new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+2]; d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2];
// Get space for the resampled early / prompt / late local replicas // Get space for the resampled early / prompt / late local replicas
d_early_code= new gr_complex[d_vector_length*2]; d_early_code = new gr_complex[d_vector_length*2];
d_prompt_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_late_code = new gr_complex[d_vector_length*2];
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
d_carr_sign=new gr_complex[d_vector_length*2]; d_carr_sign = new gr_complex[d_vector_length*2];
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ; d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ;
// define residual code phase (in chips) // define residual code phase (in chips)
d_rem_code_phase_samples = 0.0; d_rem_code_phase_samples = 0.0;
// define residual carrier phase // define residual carrier phase
d_rem_carr_phase_rad = 0.0; d_rem_carr_phase_rad = 0.0;
// sample synchronization // sample synchronization
d_sample_counter=0; d_sample_counter = 0;
d_sample_counter_seconds=0; d_sample_counter_seconds = 0;
d_acq_sample_stamp=0; d_acq_sample_stamp = 0;
d_enable_tracking=false; d_enable_tracking = false;
d_pull_in=false; d_pull_in = false;
d_last_seg=0; d_last_seg = 0;
d_current_prn_length_samples=(int)d_vector_length; d_current_prn_length_samples = (int)d_vector_length;
// CN0 estimation and lock detector buffers // CN0 estimation and lock detector buffers
d_cn0_estimation_counter=0; d_cn0_estimation_counter = 0;
d_Prompt_buffer=new gr_complex[CN0_ESTIMATION_SAMPLES]; d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES];
d_carrier_lock_test=1; d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz=0; d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter=0; d_carrier_lock_fail_counter = 0;
d_carrier_lock_threshold=5; d_carrier_lock_threshold = 5;
} }
void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){ void gps_l1_ca_dll_pll_tracking_cc::start_tracking()
/* {
* correct the code phase according to the delay between acq and trk /*
*/ * correct the code phase according to the delay between acq and trk
unsigned long int acq_trk_diff_samples; */
float acq_trk_diff_seconds; unsigned long int acq_trk_diff_samples;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp;//-d_vector_length; float acq_trk_diff_seconds;
std::cout<<"acq_trk_diff_samples="<<acq_trk_diff_samples<<"\r\n"; acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
acq_trk_diff_seconds=(float)acq_trk_diff_samples/(float)d_fs_in; std::cout << "acq_trk_diff_samples=" << acq_trk_diff_samples << std::endl;
//doppler effect acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in;
// Fd=(C/(C+Vr))*F //doppler effect
float radial_velocity; // Fd=(C/(C+Vr))*F
radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ; float radial_velocity;
// new chip and prn sequence periods based on acq Doppler radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
float T_chip_mod_seconds; // new chip and prn sequence periods based on acq Doppler
float T_prn_mod_seconds; float T_chip_mod_seconds;
float T_prn_mod_samples; float T_prn_mod_seconds;
d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ; float T_prn_mod_samples;
T_chip_mod_seconds=1/d_code_freq_hz; d_code_freq_hz = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS; T_chip_mod_seconds = 1/d_code_freq_hz;
T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
#ifdef GNSS_SDR_USE_BOOST_ROUND
#ifdef GNSS_SDR_USE_BOOST_ROUND d_next_prn_length_samples = round(T_prn_mod_samples);
d_next_prn_length_samples=round(T_prn_mod_samples); #else
#else d_next_prn_length_samples = std::round(T_prn_mod_samples);
d_next_prn_length_samples=std::round(T_prn_mod_samples); #endif
#endif
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ; float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in;
float T_prn_diff_seconds; float T_prn_diff_seconds;
T_prn_diff_seconds=T_prn_true_seconds-T_prn_mod_seconds; T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
float N_prn_diff; float N_prn_diff;
N_prn_diff=acq_trk_diff_seconds/T_prn_true_seconds; N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
float corrected_acq_phase_samples,delay_correction_samples; float corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples=fmod((d_acq_code_phase_samples+T_prn_diff_seconds*N_prn_diff*(float)d_fs_in),T_prn_true_samples); corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * (float)d_fs_in), T_prn_true_samples);
if (corrected_acq_phase_samples<0) if (corrected_acq_phase_samples < 0)
{ {
corrected_acq_phase_samples=T_prn_mod_samples+corrected_acq_phase_samples; corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
} }
delay_correction_samples=d_acq_code_phase_samples-corrected_acq_phase_samples; delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
d_acq_code_phase_samples=corrected_acq_phase_samples; d_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz=d_acq_carrier_doppler_hz; d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization // DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_carrier_doppler_hz); //initialize the carrier filter d_carrier_loop_filter.initialize(d_carrier_doppler_hz); //initialize the carrier filter
d_code_loop_filter.initialize(d_acq_code_phase_samples); //initialize the code filter d_code_loop_filter.initialize(d_acq_code_phase_samples); //initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip) // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
code_gen_conplex(&d_ca_code[1],d_satellite,0); code_gen_conplex(&d_ca_code[1], d_satellite, 0);
d_ca_code[0]=d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS]; d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1]; d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
d_carrier_lock_fail_counter=0; d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples=0; d_rem_code_phase_samples = 0;
d_rem_carr_phase_rad=0; d_rem_carr_phase_rad = 0;
d_rem_code_phase_samples=0; d_rem_code_phase_samples = 0;
d_next_rem_code_phase_samples=0; d_next_rem_code_phase_samples = 0;
d_acc_carrier_phase_rad=0; d_acc_carrier_phase_rad = 0;
d_code_phase_samples = d_acq_code_phase_samples; d_code_phase_samples = d_acq_code_phase_samples;
// DEBUG OUTPUT // DEBUG OUTPUT
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl; std::cout << "Tracking start on channel " << d_channel << " for satellite ID* " << this->d_satellite << std::endl;
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received "; DLOG(INFO) << "Start tracking for satellite "<< this->d_satellite << " received" << std::endl;
// enable tracking // enable tracking
d_pull_in=true; d_pull_in = true;
d_enable_tracking=true; d_enable_tracking = true;
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" Code Phase correction [samples]="<<delay_correction_samples<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz << " Code Phase correction [samples]=" << delay_correction_samples << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl;
} }
void gps_l1_ca_dll_pll_tracking_cc::update_local_code() void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
{ {
float tcode_chips; float tcode_chips;
float rem_code_phase_chips; float rem_code_phase_chips;
int associated_chip_index; int associated_chip_index;
int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in); rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
tcode_chips=-rem_code_phase_chips; tcode_chips = -rem_code_phase_chips;
for (int i=0;i<d_current_prn_length_samples;i++) for (int i=0; i<d_current_prn_length_samples; i++)
{ {
#ifdef GNSS_SDR_USE_BOOST_ROUND #ifdef GNSS_SDR_USE_BOOST_ROUND
associated_chip_index=1+round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips, code_length_chips));
d_prompt_code[i] = d_ca_code[associated_chip_index]; d_prompt_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips));
d_late_code[i] = d_ca_code[associated_chip_index]; d_late_code[i] = d_ca_code[associated_chip_index];
tcode_chips=tcode_chips+d_code_phase_step_chips; tcode_chips = tcode_chips + d_code_phase_step_chips;
#else #else
associated_chip_index=1+std::round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips)); associated_chip_index = 1 + std::round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+std::round(fmod(tcode_chips, code_length_chips)); associated_chip_index = 1 + std::round(fmod(tcode_chips, code_length_chips));
d_prompt_code[i] = d_ca_code[associated_chip_index]; d_prompt_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+std::round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + std::round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips));
d_late_code[i] = d_ca_code[associated_chip_index]; d_late_code[i] = d_ca_code[associated_chip_index];
tcode_chips=tcode_chips+d_code_phase_step_chips; tcode_chips = tcode_chips + d_code_phase_step_chips;
#endif #endif
} }
} }
void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier() void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
{ {
float phase_rad, phase_step_rad; float phase_rad, phase_step_rad;
phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz/(float)d_fs_in; phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz / (float)d_fs_in;
phase_rad=d_rem_carr_phase_rad; phase_rad = d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++) { for(int i = 0; i < d_current_prn_length_samples; i++)
d_carr_sign[i] = gr_complex(cos(phase_rad),sin(phase_rad)); {
d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad));
phase_rad += phase_step_rad; phase_rad += phase_step_rad;
} }
d_rem_carr_phase_rad=fmod(phase_rad,TWO_PI); d_rem_carr_phase_rad = fmod(phase_rad, TWO_PI);
d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+d_rem_carr_phase_rad; d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + d_rem_carr_phase_rad;
} }
gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
d_dump_file.close();
gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc()
{
d_dump_file.close();
delete[] d_ca_code; delete[] d_ca_code;
delete[] d_early_code; delete[] d_early_code;
delete[] d_prompt_code; delete[] d_prompt_code;
@ -271,318 +287,357 @@ gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
delete[] d_Prompt_buffer; delete[] d_Prompt_buffer;
} }
/* Tracking signal processing /* Tracking signal processing
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples * 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::general_work (int noutput_items, gr_vector_int &ninput_items, 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) { gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// if ((unsigned int)ninput_items[0]<(d_vector_length*2)) // if ((unsigned int)ninput_items[0]<(d_vector_length*2))
// { // {
// std::cout<<"End of signal detected\r\n"; // std::cout<<"End of signal detected\r\n";
// const int samples_available = ninput_items[0]; // const int samples_available = ninput_items[0];
// consume_each(samples_available); // consume_each(samples_available);
// return 0; // return 0;
// } // }
// process vars // process vars
float carr_error; float carr_error;
float carr_nco; float carr_nco;
float code_error; float code_error;
float code_nco; float code_nco;
d_Early=gr_complex(0,0); d_Early = gr_complex(0,0);
d_Prompt=gr_complex(0,0); d_Prompt = gr_complex(0,0);
d_Late=gr_complex(0,0); d_Late = gr_complex(0,0);
if (d_enable_tracking==true){ if (d_enable_tracking==true){
/* /*
* Receiver signal alignment * Receiver signal alignment
*/ */
if (d_pull_in==true) if (d_pull_in==true)
{ {
int samples_offset; int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION // 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples=d_sample_counter-d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples=d_next_prn_length_samples-fmod((float)acq_to_trk_delay_samples,(float)d_next_prn_length_samples); acq_trk_shif_correction_samples = d_next_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_next_prn_length_samples);
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n"; //std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
#ifdef GNSS_SDR_USE_BOOST_ROUND #ifdef GNSS_SDR_USE_BOOST_ROUND
samples_offset=round(d_acq_code_phase_samples+acq_trk_shif_correction_samples); samples_offset=round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
#else #else
samples_offset=std::round(d_acq_code_phase_samples+acq_trk_shif_correction_samples); samples_offset=std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
#endif #endif
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE // /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset)/(double)d_fs_in); d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / (double)d_fs_in);
d_sample_counter=d_sample_counter+samples_offset; //count for the processed samples d_sample_counter =d_sample_counter + samples_offset; //count for the processed samples
d_pull_in=false; d_pull_in = false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n"; //std::cout<<" samples_offset="<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica consume_each(samples_offset); //shift input to perform alignement with local replica
return 1; return 1;
} }
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
double **out = (double **) &output_items[0]; double **out = (double **) &output_items[0];
// check for samples consistency
for(int i=0;i<d_current_prn_length_samples;i++) {
if (std::isnan(in[i].real())==true or std::isnan(in[i].imag())==true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{
const int samples_available= ninput_items[0];
d_sample_counter=d_sample_counter+samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<<d_sample_counter;
consume_each(samples_available);
return 0;
}
}
// Update the prn length based on code freq (variable) and
// sampling frequency (fixed)
// variable code PRN sample block size
d_current_prn_length_samples=d_next_prn_length_samples;
update_local_code(); // check for samples consistency
update_local_carrier(); for(int i=0; i<d_current_prn_length_samples; i++) {
if (std::isnan(in[i].real()) == true or std::isnan(in[i].imag()) == true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{
const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<< d_sample_counter << std::endl;
consume_each(samples_available);
return 0;
}
}
// Update the prn length based on code freq (variable) and
// sampling frequency (fixed)
// variable code PRN sample block size
d_current_prn_length_samples = d_next_prn_length_samples;
gr_complex bb_signal_sample(0,0); update_local_code();
update_local_carrier();
// perform Early, Prompt and Late correlation gr_complex bb_signal_sample(0,0);
/*!
* \todo Use SIMD-enabled correlators
*/
for(int i=0;i<d_current_prn_length_samples;i++) {
//Perform the carrier wipe-off
bb_signal_sample = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each
d_Early += bb_signal_sample*d_early_code[i];
d_Prompt += bb_signal_sample*d_prompt_code[i];
d_Late += bb_signal_sample*d_late_code[i];
}
// Compute PLL error and update carrier NCO -
carr_error=pll_cloop_two_quadrant_atan(d_Prompt)/ (float)TWO_PI;
// Implement carrier loop filter and generate NCO command
carr_nco=d_carrier_loop_filter.get_carrier_nco(carr_error);
// Modify carrier freq based on NCO command
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_nco;
// Compute DLL error and update code NCO // perform Early, Prompt and Late correlation
code_error=dll_nc_e_minus_l_normalized(d_Early,d_Late); /*!
// Implement code loop filter and generate NCO command * \todo Use SIMD-enabled correlators
code_nco=d_code_loop_filter.get_code_nco(code_error); */
// Modify code freq based on NCO command for(int i=0; i<d_current_prn_length_samples; i++) {
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - code_nco; //Perform the carrier wipe-off
bb_signal_sample = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each
d_Early += bb_signal_sample * d_early_code[i];
d_Prompt += bb_signal_sample * d_prompt_code[i];
d_Late += bb_signal_sample * d_late_code[i];
}
// Update the phasestep based on code freq (variable) and // Compute PLL error and update carrier NCO -
// sampling frequency (fixed) carr_error = pll_cloop_two_quadrant_atan(d_Prompt) / (float)TWO_PI;
d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips] // Implement carrier loop filter and generate NCO command
// variable code PRN sample block size carr_nco = d_carrier_loop_filter.get_carrier_nco(carr_error);
float T_chip_seconds; // Modify carrier freq based on NCO command
float T_prn_seconds; d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_nco;
float T_prn_samples;
float K_blk_samples;
T_chip_seconds=1/d_code_freq_hz;
T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples=T_prn_seconds*d_fs_in;
d_rem_code_phase_samples=d_next_rem_code_phase_samples;
K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
// Update the current PRN delay (code phase in samples) // Compute DLL error and update code NCO
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ; code_error = dll_nc_e_minus_l_normalized(d_Early, d_Late);
float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in; // Implement code loop filter and generate NCO command
d_code_phase_samples=d_code_phase_samples+T_prn_samples-T_prn_true_samples; code_nco = d_code_loop_filter.get_code_nco(code_error);
if (d_code_phase_samples<0) // Modify code freq based on NCO command
{ d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - code_nco;
d_code_phase_samples=T_prn_true_samples+d_code_phase_samples;
}
d_code_phase_samples=fmod(d_code_phase_samples,T_prn_true_samples); // Update the phasestep based on code freq (variable) and
// sampling frequency (fixed)
d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips]
// variable code PRN sample block size
float T_chip_seconds;
float T_prn_seconds;
float T_prn_samples;
float K_blk_samples;
T_chip_seconds = 1 / d_code_freq_hz;
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * d_fs_in;
d_rem_code_phase_samples = d_next_rem_code_phase_samples;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples;
// Update the current PRN delay (code phase in samples)
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
d_code_phase_samples = d_code_phase_samples + T_prn_samples - T_prn_true_samples;
if (d_code_phase_samples < 0)
{
d_code_phase_samples = T_prn_true_samples + d_code_phase_samples;
}
d_code_phase_samples = fmod(d_code_phase_samples, T_prn_true_samples);
#ifdef GNSS_SDR_USE_BOOST_ROUND #ifdef GNSS_SDR_USE_BOOST_ROUND
d_next_prn_length_samples=round(K_blk_samples); //round to a discrete samples d_next_prn_length_samples = round(K_blk_samples); //round to a discrete samples
#else #else
d_next_prn_length_samples=std::round(K_blk_samples); //round to a discrete samples d_next_prn_length_samples = std::round(K_blk_samples); //round to a discrete samples
#endif #endif
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples; //rounding error d_next_rem_code_phase_samples = K_blk_samples - d_next_prn_length_samples; //rounding error
/*! /*!
* \todo Improve the lock detection algorithm! * \todo Improve the lock detection algorithm!
*/ */
// ####### CN0 ESTIMATION AND LOCK DETECTORS ###### // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES) if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{ {
// fill buffer with prompt correlator output values // fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt; d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
}else{ }
d_cn0_estimation_counter=0; else
d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in); {
d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES); d_cn0_estimation_counter = 0;
// ###### TRACKING UNLOCK NOTIFICATION ##### d_CN0_SNV_dB_Hz = gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in);
int tracking_message; d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>MINIMUM_VALID_CN0) // ###### TRACKING UNLOCK NOTIFICATION #####
{ int tracking_message;
d_carrier_lock_fail_counter++; if (d_carrier_lock_test < d_carrier_lock_threshold or d_carrier_lock_test > MINIMUM_VALID_CN0)
}else{ {
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--; d_carrier_lock_fail_counter++;
} }
if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER) else
{ {
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n"; if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
tracking_message=3; //loss of lock }
d_channel_internal_queue->push(tracking_message); if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
d_carrier_lock_fail_counter=0; {
d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ;
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 tracking is consistent with the channel state machine
} }
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n"; //std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
} }
/*! /*!
* \todo Output the CN0 * \todo Output the CN0
*/ */
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
// Output channel 0: Prompt correlator output Q // Output channel 0: Prompt correlator output Q
*out[0]=(double)d_Prompt.real(); *out[0] = (double)d_Prompt.real();
// Output channel 1: Prompt correlator output I // Output channel 1: Prompt correlator output I
*out[1]=(double)d_Prompt.imag(); *out[1] = (double)d_Prompt.imag();
// Output channel 2: PRN absolute delay [s] // Output channel 2: PRN absolute delay [s]
*out[2]=d_sample_counter_seconds; *out[2] = d_sample_counter_seconds;
// Output channel 3: d_acc_carrier_phase_rad [rad] // Output channel 3: d_acc_carrier_phase_rad [rad]
*out[3]=(double)d_acc_carrier_phase_rad; *out[3] = (double)d_acc_carrier_phase_rad;
// Output channel 4: PRN code phase [s] // Output channel 4: PRN code phase [s]
*out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in); *out[4] = (double)d_code_phase_samples * (1 / (float)d_fs_in);
// ########## DEBUG OUTPUT // ########## DEBUG OUTPUT
/*! /*!
* \todo The stop timer has to be moved to the signal source! * \todo The stop timer has to be moved to the signal source!
*/ */
// debug: Second counter in channel 0 // debug: Second counter in channel 0
if (d_channel==0) if (d_channel == 0)
{ {
if (floor(d_sample_counter/d_fs_in)!=d_last_seg) if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{ {
d_last_seg=floor(d_sample_counter/d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout<<"Current input signal time="<<d_last_seg<<" [s]"<<std::endl; std::cout << "Current input signal time=" << d_last_seg << " [s]" << std::endl;
std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl; std::cout << "Tracking CH " << d_channel << " CN0=" << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl; //std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock! //if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
} }
}else }
{ else
if (floor(d_sample_counter/d_fs_in)!=d_last_seg) {
{ if (floor(d_sample_counter / d_fs_in) != d_last_seg)
d_last_seg=floor(d_sample_counter/d_fs_in); {
std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl; d_last_seg = floor(d_sample_counter / d_fs_in);
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl; std::cout << "Tracking CH "<< d_channel << " CN0=" << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
} //std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
} }
}else{ }
double **out = (double **) &output_items[0]; //block output streams pointer }
*out[0]=0; else
*out[1]=0; {
*out[2]=0; double **out = (double **) &output_items[0]; //block output streams pointer
*out[3]=0; *out[0] = 0;
*out[4]=0; *out[1] = 0;
} *out[2] = 0;
*out[3] = 0;
*out[4] = 0;
}
if(d_dump) { if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I; float prompt_I;
float prompt_Q; float prompt_Q;
float tmp_E,tmp_P,tmp_L; float tmp_E, tmp_P, tmp_L;
float tmp_float; float tmp_float;
prompt_I=d_Prompt.imag(); prompt_I = d_Prompt.imag();
prompt_Q=d_Prompt.real(); prompt_Q = d_Prompt.real();
tmp_E=std::abs<float>(d_Early); tmp_E = std::abs<float>(d_Early);
tmp_P=std::abs<float>(d_Prompt); tmp_P = std::abs<float>(d_Prompt);
tmp_L=std::abs<float>(d_Late); tmp_L = std::abs<float>(d_Late);
try { try
// EPR {
d_dump_file.write((char*)&tmp_E, sizeof(float)); // EPR
d_dump_file.write((char*)&tmp_P, sizeof(float)); d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float)); d_dump_file.write((char*)&tmp_P, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols) d_dump_file.write((char*)&tmp_L, sizeof(float));
d_dump_file.write((char*)&prompt_I, sizeof(float)); // PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_Q, sizeof(float)); d_dump_file.write((char*)&prompt_I, sizeof(float));
// PRN start sample stamp d_dump_file.write((char*)&prompt_Q, sizeof(float));
//tmp_float=(float)d_sample_counter; // PRN start sample stamp
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); //tmp_float=(float)d_sample_counter;
// accumulated carrier phase d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float)); // accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
// carrier and code frequency // carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float)); d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float)); d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
//PLL commands //PLL commands
d_dump_file.write((char*)&carr_error, sizeof(float)); d_dump_file.write((char*)&carr_error, sizeof(float));
d_dump_file.write((char*)&carr_nco, sizeof(float)); d_dump_file.write((char*)&carr_nco, sizeof(float));
//DLL commands //DLL commands
d_dump_file.write((char*)&code_error, sizeof(float)); d_dump_file.write((char*)&code_error, sizeof(float));
d_dump_file.write((char*)&code_nco, sizeof(float)); d_dump_file.write((char*)&code_nco, sizeof(float));
// CN0 and carrier lock test // CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float)); d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float)); d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float=0; tmp_float=0;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double)); d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
} }
catch (std::ifstream::failure e) { catch (std::ifstream::failure e)
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n"; {
} std::cout << "Exception writing trk dump file " << e.what() << std::endl;
} }
}
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter_seconds = d_sample_counter_seconds + (((double)d_current_prn_length_samples)/(double)d_fs_in); d_sample_counter_seconds = d_sample_counter_seconds + ( ((double)d_current_prn_length_samples) / (double)d_fs_in );
d_sample_counter+=d_current_prn_length_samples; //count for the processed samples d_sample_counter += d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
} }
void gps_l1_ca_dll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
void gps_l1_ca_dll_pll_tracking_cc::set_acq_code_phase(float code_phase)
{
d_acq_code_phase_samples = code_phase; d_acq_code_phase_samples = code_phase;
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples; LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
} }
void gps_l1_ca_dll_pll_tracking_cc::set_acq_doppler(float doppler) {
void gps_l1_ca_dll_pll_tracking_cc::set_acq_doppler(float doppler)
{
d_acq_carrier_doppler_hz = doppler; d_acq_carrier_doppler_hz = doppler;
LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz; LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
} }
void gps_l1_ca_dll_pll_tracking_cc::set_satellite(unsigned int satellite) {
void gps_l1_ca_dll_pll_tracking_cc::set_satellite(unsigned int satellite)
{
d_satellite = satellite; d_satellite = satellite;
LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite; LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite;
} }
void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel) { void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel)
d_channel = channel; {
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel; d_channel = channel;
// ############# ENABLE DATA FILE LOG ################# LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
if (d_dump==true) // ############# ENABLE DATA FILE LOG #################
{ if (d_dump==true)
if (d_dump_file.is_open()==false) {
{ if (d_dump_file.is_open()==false)
try { {
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel)); try
d_dump_filename.append(".dat"); {
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); d_dump_filename.append(".dat");
std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl; 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);
catch (std::ifstream::failure e) { std::cout << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n"; }
} catch (std::ifstream::failure e)
} {
} std::cout << "channel "<< d_channel << " Exception opening trk dump file " << e.what() << std::endl;
}
}
}
} }
void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp) void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
{ {
d_acq_sample_stamp = sample_stamp; d_acq_sample_stamp = sample_stamp;
} }
void gps_l1_ca_dll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue) void gps_l1_ca_dll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{ {
d_channel_internal_queue = channel_internal_queue; d_channel_internal_queue = channel_internal_queue;

51
src/algorithms/tracking/libs/CN_estimators.cc
View File

@ -70,22 +70,22 @@ float gps_l1_ca_CN0_SNV(gr_complex* Prompt_buffer, int length, long fs_in)
//SNR_SNV(count)=Psig/(Ptot-Psig); //SNR_SNV(count)=Psig/(Ptot-Psig);
//CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length); //CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length);
float SNR, SNR_dB_Hz; float SNR, SNR_dB_Hz;
float tmp_abs_I,tmp_abs_Q; float tmp_abs_I, tmp_abs_Q;
float Psig,Ptot; float Psig, Ptot;
//float M2,M4; //float M2,M4;
Psig=0; Psig = 0;
Ptot=0; Ptot = 0;
for (int i=0;i<length;i++) for (int i=0; i<length; i++)
{ {
tmp_abs_I=std::abs(Prompt_buffer[i].imag()); tmp_abs_I = std::abs(Prompt_buffer[i].imag());
tmp_abs_Q=std::abs(Prompt_buffer[i].real()); tmp_abs_Q = std::abs(Prompt_buffer[i].real());
Psig+=tmp_abs_I; Psig += tmp_abs_I;
Ptot+=Prompt_buffer[i].imag()*Prompt_buffer[i].imag()+Prompt_buffer[i].real()*Prompt_buffer[i].real(); Ptot += Prompt_buffer[i].imag()*Prompt_buffer[i].imag() + Prompt_buffer[i].real()*Prompt_buffer[i].real();
} }
Psig=Psig/(float)length; Psig = Psig / (float)length;
Psig=Psig*Psig; Psig = Psig*Psig;
SNR=Psig/(Ptot/(float)length-Psig); SNR = Psig / (Ptot / (float)length - Psig);
SNR_dB_Hz=10*log10(SNR)+10*log10(fs_in/2)-10*log10(GPS_L1_CA_CODE_LENGTH_CHIPS); SNR_dB_Hz = 10*log10(SNR) + 10*log10(fs_in/2) - 10*log10(GPS_L1_CA_CODE_LENGTH_CHIPS);
return SNR_dB_Hz; return SNR_dB_Hz;
} }
@ -109,26 +109,25 @@ float carrier_lock_detector(gr_complex* Prompt_buffer, int length)
//NBD=sum(abs(imag(x((n-N+1):n))))^2 + sum(abs(real(x((n-N+1):n))))^2; //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); //NBP=sum(imag(x((n-N+1):n)).^2) - sum(real(x((n-N+1):n)).^2);
//LOCK(count)=NBD/NBP; //LOCK(count)=NBD/NBP;
float tmp_abs_I,tmp_abs_Q; float tmp_abs_I, tmp_abs_Q;
float tmp_sum_abs_I,tmp_sum_abs_Q; float tmp_sum_abs_I, tmp_sum_abs_Q;
float tmp_sum_sqr_I,tmp_sum_sqr_Q; float tmp_sum_sqr_I, tmp_sum_sqr_Q;
tmp_sum_abs_I=0; tmp_sum_abs_I=0;
tmp_sum_abs_Q=0; tmp_sum_abs_Q=0;
tmp_sum_sqr_I=0; tmp_sum_sqr_I=0;
tmp_sum_sqr_Q=0; tmp_sum_sqr_Q=0;
float NBD,NBP; float NBD,NBP;
for (int i=0;i<length;i++) for (int i=0; i<length; i++)
{ {
tmp_abs_I=std::abs(Prompt_buffer[i].imag()); tmp_abs_I = std::abs(Prompt_buffer[i].imag());
tmp_abs_Q=std::abs(Prompt_buffer[i].real()); tmp_abs_Q = std::abs(Prompt_buffer[i].real());
tmp_sum_abs_I+=tmp_abs_I; tmp_sum_abs_I += tmp_abs_I;
tmp_sum_abs_Q+=tmp_abs_Q; tmp_sum_abs_Q += tmp_abs_Q;
tmp_sum_sqr_I+=(Prompt_buffer[i].imag()*Prompt_buffer[i].imag()); tmp_sum_sqr_I += (Prompt_buffer[i].imag()*Prompt_buffer[i].imag());
tmp_sum_sqr_Q+=(Prompt_buffer[i].real()*Prompt_buffer[i].real()); tmp_sum_sqr_Q += (Prompt_buffer[i].real()*Prompt_buffer[i].real());
} }
NBD=tmp_sum_abs_I*tmp_sum_abs_I+tmp_sum_abs_Q*tmp_sum_abs_Q; 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; NBP = tmp_sum_sqr_I - tmp_sum_sqr_Q;
return NBD/NBP; return NBD/NBP;
} }

22
src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc
View File

@ -38,7 +38,8 @@
#include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_DLL_filter.h"
void tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){ void tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k)
{
// Solve natural frequency // Solve natural frequency
float Wn; float Wn;
Wn = lbw*8*zeta / (4*zeta*zeta + 1); Wn = lbw*8*zeta / (4*zeta*zeta + 1);
@ -47,20 +48,26 @@ void tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float
*tau2 = (2.0 * zeta) / Wn; *tau2 = (2.0 * zeta) / Wn;
} }
void tracking_2nd_DLL_filter::set_DLL_BW(float dll_bw_hz) void tracking_2nd_DLL_filter::set_DLL_BW(float dll_bw_hz)
{ {
//Calculate filter coefficient values //Calculate filter coefficient values
d_dllnoisebandwidth=dll_bw_hz; d_dllnoisebandwidth =dll_bw_hz;
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio, 1.0);// Calculate filter coefficient values
} }
void tracking_2nd_DLL_filter::initialize(float d_acq_code_phase_samples) void tracking_2nd_DLL_filter::initialize(float d_acq_code_phase_samples)
{ {
// code tracking loop parameters // code tracking loop parameters
d_old_code_nco = 0.0; d_old_code_nco = 0.0;
d_old_code_error = 0.0; d_old_code_error = 0.0;
} }
float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator) float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
{ {
float code_nco; float code_nco;
@ -70,13 +77,14 @@ float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
return code_nco; return code_nco;
} }
tracking_2nd_DLL_filter::tracking_2nd_DLL_filter () tracking_2nd_DLL_filter::tracking_2nd_DLL_filter ()
{ {
d_pdi_code = 0.001;// Summation interval for code d_pdi_code = 0.001;// Summation interval for code
d_dlldampingratio=0.7; d_dlldampingratio = 0.7;
} }
tracking_2nd_DLL_filter::~tracking_2nd_DLL_filter () tracking_2nd_DLL_filter::~tracking_2nd_DLL_filter ()
{ {}
}

24
src/algorithms/tracking/libs/tracking_discriminators.cc
View File

@ -47,9 +47,9 @@
float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1, float t2) float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1, float t2)
{ {
float cross,dot; float cross,dot;
dot=prompt_s1.imag()*prompt_s2.imag()+prompt_s1.real()*prompt_s2.real(); dot = prompt_s1.imag()*prompt_s2.imag() + prompt_s1.real()*prompt_s2.real();
cross=prompt_s1.imag()*prompt_s2.real()-prompt_s2.imag()*prompt_s1.real(); cross = prompt_s1.imag()*prompt_s2.real() - prompt_s2.imag()*prompt_s1.real();
return atan2(cross,dot)/(t2-t1); return atan2(cross, dot) / (t2-t1);
} }
@ -62,7 +62,7 @@ float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1
*/ */
float pll_four_quadrant_atan(gr_complex prompt_s1) float pll_four_quadrant_atan(gr_complex prompt_s1)
{ {
return atan2(prompt_s1.real(),prompt_s1.imag()); return atan2(prompt_s1.real(), prompt_s1.imag());
} }
@ -75,11 +75,13 @@ float pll_four_quadrant_atan(gr_complex prompt_s1)
*/ */
float pll_cloop_two_quadrant_atan(gr_complex prompt_s1) float pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
{ {
if (prompt_s1.imag()!=0.0) if (prompt_s1.imag() != 0.0)
{ {
return atan(prompt_s1.real()/prompt_s1.imag()); return atan(prompt_s1.real() / prompt_s1.imag());
}else{ }
return 0; else
{
return 0;
} }
} }
@ -95,7 +97,7 @@ float pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1) float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
{ {
float P_early, P_late; float P_early, P_late;
P_early=std::abs(early_s1); P_early = std::abs(early_s1);
P_late=std::abs(late_s1); P_late = std::abs(late_s1);
return (P_early-P_late)/((P_early+P_late)); return (P_early - P_late) / ((P_early + P_late));
} }

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

@ -39,59 +39,59 @@
void gps_navigation_message::reset() void gps_navigation_message::reset()
{ {
d_TOW=0; d_TOW = 0;
d_IODE_SF2=0; d_IODE_SF2 = 0;
d_IODE_SF3=0; d_IODE_SF3 = 0;
d_Crs=0; d_Crs = 0;
d_Delta_n=0; d_Delta_n = 0;
d_M_0=0; d_M_0 = 0;
d_Cuc=0; d_Cuc = 0;
d_e_eccentricity=0; d_e_eccentricity = 0;
d_Cus=0; d_Cus = 0;
d_sqrt_A=0; d_sqrt_A = 0;
d_Toe=0; d_Toe = 0;
d_Toc=0; d_Toc = 0;
d_Cic=0; d_Cic = 0;
d_OMEGA0=0; d_OMEGA0 = 0;
d_Cis=0; d_Cis = 0;
d_i_0=0; d_i_0 = 0;
d_Crc=0; d_Crc = 0;
d_OMEGA=0; d_OMEGA = 0;
d_OMEGA_DOT=0; d_OMEGA_DOT = 0;
d_IDOT=0; d_IDOT = 0;
i_code_on_L2=0; i_code_on_L2 = 0;
i_GPS_week=0; i_GPS_week = 0;
b_L2_P_data_flag=false; b_L2_P_data_flag = false;
i_SV_accuracy=0; i_SV_accuracy = 0;
i_SV_health=0; i_SV_health = 0;
d_TGD=0; d_TGD = 0;
d_IODC=-1; d_IODC = -1;
i_AODO=0; i_AODO = 0;
b_fit_interval_flag=false; b_fit_interval_flag = false;
d_spare1=0; d_spare1 = 0;
d_spare2=0; d_spare2 = 0;
d_A_f0=0; d_A_f0 = 0;
d_A_f1=0; d_A_f1 = 0;
d_A_f2=0; d_A_f2 = 0;
//clock terms //clock terms
//d_master_clock=0; //d_master_clock=0;
d_dtr=0; d_dtr = 0;
d_satClkCorr=0; d_satClkCorr = 0;
// satellite positions // satellite positions
d_satpos_X=0; d_satpos_X = 0;
d_satpos_Y=0; d_satpos_Y = 0;
d_satpos_Z=0; d_satpos_Z = 0;
// info // info
i_channel_ID=0; i_channel_ID = 0;
i_satellite_PRN=0; i_satellite_PRN = 0;
// time synchro // time synchro
d_subframe1_timestamp_ms=0; d_subframe1_timestamp_ms = 0;
// flags // flags
b_alert_flag = false; b_alert_flag = false;
@ -100,35 +100,35 @@ void gps_navigation_message::reset()
// Ionosphere and UTC // Ionosphere and UTC
d_alpha0=0; d_alpha0 = 0;
d_alpha1=0; d_alpha1 = 0;
d_alpha2=0; d_alpha2 = 0;
d_alpha3=0; d_alpha3 = 0;
d_beta0=0; d_beta0 = 0;
d_beta1=0; d_beta1 = 0;
d_beta2=0; d_beta2 = 0;
d_beta3=0; d_beta3 = 0;
d_A1=0; d_A1 = 0;
d_A0=0; d_A0 = 0;
d_t_OT=0; d_t_OT = 0;
i_WN_T=0; i_WN_T = 0;
d_DeltaT_LS=0; d_DeltaT_LS = 0;
i_WN_LSF=0; i_WN_LSF = 0;
i_DN=0; i_DN = 0;
d_DeltaT_LSF=0; d_DeltaT_LSF= 0;
//Almanac //Almanac
d_Toa = 0; d_Toa = 0;
i_WN_A = 0; i_WN_A = 0;
for (int i=1; i < 32; i++ ) for (int i=1; i < 32; i++ )
{ {
almanacHealth[i]=0; almanacHealth[i] = 0;
} }
// Satellite velocity // Satellite velocity
d_satvel_X=0; d_satvel_X = 0;
d_satvel_Y=0; d_satvel_Y = 0;
d_satvel_Z=0; d_satvel_Z = 0;
//Plane A (info from http://www.navcen.uscg.gov/?Do=constellationStatus) //Plane A (info from http://www.navcen.uscg.gov/?Do=constellationStatus)
satelliteBlock[9] = "IIA"; satelliteBlock[9] = "IIA";
@ -182,8 +182,8 @@ gps_navigation_message::gps_navigation_message()
void gps_navigation_message::print_gps_word_bytes(unsigned int GPS_word) void gps_navigation_message::print_gps_word_bytes(unsigned int GPS_word)
{ {
std::cout << " Word ="; std::cout << " Word =";
std::cout<<std::bitset<32>(GPS_word); std::cout << std::bitset<32>(GPS_word);
std::cout<<std::endl; std::cout << std::endl;
} }
@ -192,13 +192,13 @@ bool gps_navigation_message::read_navigation_bool(std::bitset<GPS_SUBFRAME_BITS>
{ {
bool value; bool value;
if (bits[GPS_SUBFRAME_BITS-slices[0].position]==1) if (bits[GPS_SUBFRAME_BITS - slices[0].position] == 1)
{ {
value=true; value = true;
} }
else else
{ {
value=false; value = false;
} }
return value; return value;
@ -212,15 +212,15 @@ unsigned long int gps_navigation_message::read_navigation_unsigned(std::bitset<G
{ {
unsigned long int value; unsigned long int value;
value=0; value = 0;
for (int i=0;i<num_of_slices;i++) for (int i=0; i<num_of_slices; i++)
{ {
for (int j=0;j<slices[i].length;j++) for (int j=0; j<slices[i].length; j++)
{ {
value<<=1; //shift left value <<= 1; //shift left
if (bits[GPS_SUBFRAME_BITS-slices[i].position-j]==1) if (bits[GPS_SUBFRAME_BITS - slices[i].position - j] == 1)
{ {
value+=1; // insert the bit value += 1; // insert the bit
} }
} }
} }
@ -234,22 +234,25 @@ unsigned long int gps_navigation_message::read_navigation_unsigned(std::bitset<G
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 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; signed long int value = 0;
// read the MSB and perform the sign extension // read the MSB and perform the sign extension
if (bits[GPS_SUBFRAME_BITS-slices[0].position]==1) if (bits[GPS_SUBFRAME_BITS-slices[0].position]==1)
{ {
value^=0xFFFFFFFF; value^=0xFFFFFFFF;
}else{
value&=0;
} }
for (int i=0;i<num_of_slices;i++) else
{ {
for (int j=0;j<slices[i].length;j++) value&=0;
}
for (int i=0; i<num_of_slices; i++)
{
for (int j=0; j<slices[i].length; j++)
{ {
value<<=1; //shift left value<<=1; //shift left
value&=0xFFFFFFFE; //reset the corresponding bit value&=0xFFFFFFFE; //reset the corresponding bit
if (bits[GPS_SUBFRAME_BITS-slices[i].position-j]==1) if (bits[GPS_SUBFRAME_BITS - slices[i].position-j] == 1)
{ {
value+=1; // insert the bit value+=1; // insert the bit
} }
@ -270,10 +273,11 @@ double gps_navigation_message::check_t(double time)
if (time > half_week) if (time > half_week)
{ {
corrTime = time - 2*half_week; corrTime = time - 2*half_week;
}else if (time < -half_week) }
{ else if (time < -half_week)
corrTime = time + 2*half_week; {
} corrTime = time + 2*half_week;
}
return corrTime; return corrTime;
} }
@ -314,25 +318,25 @@ void gps_navigation_message::satellitePosition(double transmitTime)
// Find satellite's position ---------------------------------------------- // Find satellite's position ----------------------------------------------
// Restore semi-major axis // Restore semi-major axis
a = d_sqrt_A*d_sqrt_A; a = d_sqrt_A*d_sqrt_A;
// Time from ephemeris reference epoch // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe); tk = check_t(transmitTime - d_Toe);
// Computed mean motion // Computed mean motion
n0 = sqrt(GM / (a*a*a)); n0 = sqrt(GM / (a*a*a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2*GPS_PI),(2*GPS_PI)); M = fmod((M + 2*GPS_PI),(2*GPS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; E = M;
// --- Iteratively compute eccentric anomaly ---------------------------- // --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1;ii<20;ii++) for (int ii = 1;ii<20;ii++)
@ -351,8 +355,8 @@ void gps_navigation_message::satellitePosition(double transmitTime)
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E); d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
// Compute the true anomaly // Compute the true anomaly
double tmp_Y=sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E); double tmp_Y = sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E);
double tmp_X=cos(E)-d_e_eccentricity; double tmp_X = cos(E) - d_e_eccentricity;
nu = atan2(tmp_Y, tmp_X); nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
@ -362,7 +366,7 @@ void gps_navigation_message::satellitePosition(double transmitTime)
phi = fmod((phi),(2*GPS_PI)); phi = fmod((phi),(2*GPS_PI));
// Correct argument of latitude // Correct argument of latitude
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi); u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi);
// Correct radius // Correct radius
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi); r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi);
@ -403,11 +407,6 @@ void gps_navigation_message::satellitePosition(double transmitTime)
d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i); d_satvel_Z = d_satpos_Y * sin(i);
} }
@ -422,9 +421,9 @@ void gps_navigation_message::satellitePosition(double transmitTime)
int gps_navigation_message::subframe_decoder(char *subframe) int gps_navigation_message::subframe_decoder(char *subframe)
{ {
int subframe_ID=0; int subframe_ID = 0;
int SV_data_ID=0; int SV_data_ID = 0;
int SV_page=0; int SV_page = 0;
//double tmp_TOW; //double tmp_TOW;
unsigned int gps_word; unsigned int gps_word;
@ -432,13 +431,13 @@ int gps_navigation_message::subframe_decoder(char *subframe)
// UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE
std::bitset<GPS_SUBFRAME_BITS> subframe_bits; std::bitset<GPS_SUBFRAME_BITS> subframe_bits;
std::bitset<GPS_WORD_BITS+2> word_bits; std::bitset<GPS_WORD_BITS+2> word_bits;
for (int i=0;i<10;i++) for (int i=0; i<10; i++)
{ {
memcpy(&gps_word,&subframe[i*4],sizeof(char)*4); memcpy(&gps_word,&subframe[i*4], sizeof(char)*4);
word_bits=std::bitset<(GPS_WORD_BITS+2)>(gps_word); word_bits = std::bitset<(GPS_WORD_BITS+2)>(gps_word);
for (int j=0;j<GPS_WORD_BITS;j++) for (int j=0; j<GPS_WORD_BITS; j++)
{ {
subframe_bits[GPS_WORD_BITS*(9-i)+j]=word_bits[j]; subframe_bits[GPS_WORD_BITS*(9-i) + j] = word_bits[j];
} }
} }
@ -451,11 +450,12 @@ int gps_navigation_message::subframe_decoder(char *subframe)
print_gps_word_bytes(gps_word); print_gps_word_bytes(gps_word);
} }
*/ */
subframe_ID=(int)read_navigation_unsigned(subframe_bits,SUBFRAME_ID,num_of_slices(SUBFRAME_ID)); 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 // Decode all 5 sub-frames
switch (subframe_ID){ switch (subframe_ID)
{
//--- Decode the sub-frame id ------------------------------------------ //--- Decode the sub-frame id ------------------------------------------
// ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf // ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf
case 1: case 1:
@ -466,7 +466,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
// subframe and we need the TOW of the first subframe in this data block // subframe and we need the TOW of the first subframe in this data block
// (the variable subframe at this point contains bits of the last subframe). // (the variable subframe at this point contains bits of the last subframe).
//TOW = bin2dec(subframe(31:47)) * 6 - 30; //TOW = bin2dec(subframe(31:47)) * 6 - 30;
d_TOW = (double)read_navigation_unsigned(subframe_bits,TOW,num_of_slices(TOW)); d_TOW = (double)read_navigation_unsigned(subframe_bits, TOW, num_of_slices(TOW));
d_TOW = d_TOW*6-6; //we are in the first subframe (the transmitted TOW is the start time of the next subframe, thus we need to substract one subframe (6 seconds)) ! d_TOW = d_TOW*6-6; //we are in the first subframe (the transmitted TOW is the start time of the next subframe, thus we need to substract one subframe (6 seconds)) !
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
@ -474,22 +474,22 @@ int gps_navigation_message::subframe_decoder(char *subframe)
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
// It contains WN, SV clock corrections, health and accuracy // It contains WN, SV clock corrections, health and accuracy
i_GPS_week = (int)read_navigation_unsigned(subframe_bits,GPS_WEEK,num_of_slices(GPS_WEEK)); i_GPS_week = (int)read_navigation_unsigned(subframe_bits, GPS_WEEK, num_of_slices(GPS_WEEK));
i_SV_accuracy = (int)read_navigation_unsigned(subframe_bits,SV_ACCURACY,num_of_slices(SV_ACCURACY)); // (20.3.3.3.1.3) i_SV_accuracy = (int)read_navigation_unsigned(subframe_bits, SV_ACCURACY, num_of_slices(SV_ACCURACY)); // (20.3.3.3.1.3)
i_SV_health = (int)read_navigation_unsigned(subframe_bits,SV_HEALTH,num_of_slices(SV_HEALTH)); i_SV_health = (int)read_navigation_unsigned(subframe_bits, SV_HEALTH, num_of_slices(SV_HEALTH));
b_L2_P_data_flag = read_navigation_bool(subframe_bits,L2_P_DATA_FLAG); // b_L2_P_data_flag = read_navigation_bool(subframe_bits, L2_P_DATA_FLAG); //
i_code_on_L2 = (int)read_navigation_unsigned(subframe_bits,CA_OR_P_ON_L2,num_of_slices(CA_OR_P_ON_L2)); i_code_on_L2 = (int)read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2, num_of_slices(CA_OR_P_ON_L2));
d_TGD = (double)read_navigation_signed(subframe_bits,T_GD,num_of_slices(T_GD)); d_TGD = (double)read_navigation_signed(subframe_bits, T_GD, num_of_slices(T_GD));
d_TGD = d_TGD*T_GD_LSB; d_TGD = d_TGD*T_GD_LSB;
d_IODC = (double)read_navigation_unsigned(subframe_bits,IODC,num_of_slices(IODC)); d_IODC = (double)read_navigation_unsigned(subframe_bits, IODC, num_of_slices(IODC));
d_Toc = (double)read_navigation_unsigned(subframe_bits,T_OC,num_of_slices(T_OC)); d_Toc = (double)read_navigation_unsigned(subframe_bits, T_OC, num_of_slices(T_OC));
d_Toc = d_Toc*T_OC_LSB; d_Toc = d_Toc*T_OC_LSB;
d_A_f0 = (double)read_navigation_signed(subframe_bits,A_F0,num_of_slices(A_F0)); d_A_f0 = (double)read_navigation_signed(subframe_bits, A_F0, num_of_slices(A_F0));
d_A_f0 = d_A_f0*A_F0_LSB; d_A_f0 = d_A_f0*A_F0_LSB;
d_A_f1 = (double)read_navigation_signed(subframe_bits,A_F1,num_of_slices(A_F1)); d_A_f1 = (double)read_navigation_signed(subframe_bits, A_F1, num_of_slices(A_F1));
d_A_f1 = d_A_f1*A_F1_LSB; d_A_f1 = d_A_f1*A_F1_LSB;
d_A_f2 = (double)read_navigation_signed(subframe_bits,A_F2,num_of_slices(A_F2)); d_A_f2 = (double)read_navigation_signed(subframe_bits, A_F2, num_of_slices(A_F2));
d_A_f2 = d_A_f2*A_F2_LSB; d_A_f2 = d_A_f2*A_F2_LSB;
@ -511,26 +511,26 @@ int gps_navigation_message::subframe_decoder(char *subframe)
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
d_IODE_SF2 = (double)read_navigation_unsigned(subframe_bits,IODE_SF2,num_of_slices(IODE_SF2)); 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 = (double)read_navigation_signed(subframe_bits, C_RS, num_of_slices(C_RS));
d_Crs =d_Crs * C_RS_LSB; d_Crs = d_Crs * C_RS_LSB;
d_Delta_n = (double)read_navigation_signed(subframe_bits,DELTA_N,num_of_slices(DELTA_N)); d_Delta_n = (double)read_navigation_signed(subframe_bits, DELTA_N, num_of_slices(DELTA_N));
d_Delta_n = d_Delta_n * DELTA_N_LSB; d_Delta_n = d_Delta_n * DELTA_N_LSB;
d_M_0 = (double)read_navigation_signed(subframe_bits,M_0,num_of_slices(M_0)); d_M_0 = (double)read_navigation_signed(subframe_bits, M_0, num_of_slices(M_0));
d_M_0 = d_M_0 * M_0_LSB; d_M_0 = d_M_0 * M_0_LSB;
d_Cuc = (double)read_navigation_signed(subframe_bits,C_UC,num_of_slices(C_UC)); d_Cuc = (double)read_navigation_signed(subframe_bits, C_UC, num_of_slices(C_UC));
d_Cuc = d_Cuc * C_UC_LSB; d_Cuc = d_Cuc * C_UC_LSB;
d_e_eccentricity = (double)read_navigation_unsigned(subframe_bits,E,num_of_slices(E)); d_e_eccentricity = (double)read_navigation_unsigned(subframe_bits, E, num_of_slices(E));
d_e_eccentricity = d_e_eccentricity * E_LSB; d_e_eccentricity = d_e_eccentricity * E_LSB;
d_Cus = (double)read_navigation_signed(subframe_bits,C_US,num_of_slices(C_US)); d_Cus = (double)read_navigation_signed(subframe_bits, C_US, num_of_slices(C_US));
d_Cus = d_Cus * C_US_LSB; d_Cus = d_Cus * C_US_LSB;
d_sqrt_A = (double)read_navigation_unsigned(subframe_bits,SQRT_A,num_of_slices(SQRT_A)); d_sqrt_A = (double)read_navigation_unsigned(subframe_bits, SQRT_A, num_of_slices(SQRT_A));
d_sqrt_A = d_sqrt_A * SQRT_A_LSB; d_sqrt_A = d_sqrt_A * SQRT_A_LSB;
d_Toe = (double)read_navigation_unsigned(subframe_bits,T_OE,num_of_slices(T_OE)); d_Toe = (double)read_navigation_unsigned(subframe_bits, T_OE, num_of_slices(T_OE));
d_Toe = d_Toe * T_OE_LSB; d_Toe = d_Toe * T_OE_LSB;
b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG); b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG);
i_AODO = (int)read_navigation_unsigned(subframe_bits,AODO,num_of_slices(AODO)); i_AODO = (int)read_navigation_unsigned(subframe_bits, AODO, num_of_slices(AODO));
i_AODO = i_AODO * AODO_LSB; i_AODO = i_AODO * AODO_LSB;
break; break;
@ -551,22 +551,22 @@ int gps_navigation_message::subframe_decoder(char *subframe)
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
d_Cic = (double)read_navigation_signed(subframe_bits,C_IC,num_of_slices(C_IC)); d_Cic = (double)read_navigation_signed(subframe_bits, C_IC, num_of_slices(C_IC));
d_Cic = d_Cic * C_IC_LSB; d_Cic = d_Cic * C_IC_LSB;
d_OMEGA0 = (double)read_navigation_signed(subframe_bits,OMEGA_0,num_of_slices(OMEGA_0)); d_OMEGA0 = (double)read_navigation_signed(subframe_bits, OMEGA_0, num_of_slices(OMEGA_0));
d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB; d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB;
d_Cis = (double)read_navigation_signed(subframe_bits,C_IS,num_of_slices(C_IS)); d_Cis = (double)read_navigation_signed(subframe_bits, C_IS, num_of_slices(C_IS));
d_Cis = d_Cis * C_IS_LSB; d_Cis = d_Cis * C_IS_LSB;
d_i_0 = (double)read_navigation_signed(subframe_bits,I_0,num_of_slices(I_0)); d_i_0 = (double)read_navigation_signed(subframe_bits, I_0, num_of_slices(I_0));
d_i_0 = d_i_0 * I_0_LSB; d_i_0 = d_i_0 * I_0_LSB;
d_Crc = (double)read_navigation_signed(subframe_bits,C_RC,num_of_slices(C_RC)); d_Crc = (double)read_navigation_signed(subframe_bits, C_RC, num_of_slices(C_RC));
d_Crc = d_Crc * C_RC_LSB; d_Crc = d_Crc * C_RC_LSB;
d_OMEGA = (double)read_navigation_signed(subframe_bits,OMEGA,num_of_slices(OMEGA)); d_OMEGA = (double)read_navigation_signed(subframe_bits, OMEGA, num_of_slices(OMEGA));
d_OMEGA = d_OMEGA * OMEGA_LSB; d_OMEGA = d_OMEGA * OMEGA_LSB;
d_OMEGA_DOT = (double)read_navigation_signed(subframe_bits,OMEGA_DOT,num_of_slices(OMEGA_DOT)); d_OMEGA_DOT = (double)read_navigation_signed(subframe_bits, OMEGA_DOT, num_of_slices(OMEGA_DOT));
d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB; d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB;
d_IODE_SF3 = (double)read_navigation_unsigned(subframe_bits,IODE_SF3,num_of_slices(IODE_SF3)); d_IODE_SF3 = (double)read_navigation_unsigned(subframe_bits, IODE_SF3, num_of_slices(IODE_SF3));
d_IDOT = (double)read_navigation_signed(subframe_bits,I_DOT,num_of_slices(I_DOT)); d_IDOT = (double)read_navigation_signed(subframe_bits, I_DOT, num_of_slices(I_DOT));
d_IDOT = d_IDOT*I_DOT_LSB; d_IDOT = d_IDOT*I_DOT_LSB;
break; break;
@ -587,8 +587,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
SV_data_ID = (int)read_navigation_unsigned(subframe_bits,SV_DATA_ID,num_of_slices(SV_DATA_ID)); SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID, num_of_slices(SV_DATA_ID));
SV_page = (int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE)); SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE, num_of_slices(SV_PAGE));
if (SV_page == 13) if (SV_page == 13)
{ {
@ -598,51 +598,47 @@ int gps_navigation_message::subframe_decoder(char *subframe)
if (SV_page == 18) if (SV_page == 18)
{ {
// Page 18 - Ionospheric and UTC data // Page 18 - Ionospheric and UTC data
d_alpha0 = (double)read_navigation_signed(subframe_bits, ALPHA_0, num_of_slices(ALPHA_0));
d_alpha0 = (double)read_navigation_signed(subframe_bits, ALPHA_0,num_of_slices(ALPHA_0));
d_alpha0 = d_alpha0 * ALPHA_0_LSB; d_alpha0 = d_alpha0 * ALPHA_0_LSB;
d_alpha1 = (double)read_navigation_signed(subframe_bits, ALPHA_1,num_of_slices(ALPHA_1)); d_alpha1 = (double)read_navigation_signed(subframe_bits, ALPHA_1, num_of_slices(ALPHA_1));
d_alpha1 = d_alpha1 * ALPHA_1_LSB; d_alpha1 = d_alpha1 * ALPHA_1_LSB;
d_alpha2 = (double)read_navigation_signed(subframe_bits, ALPHA_2,num_of_slices(ALPHA_2)); d_alpha2 = (double)read_navigation_signed(subframe_bits, ALPHA_2, num_of_slices(ALPHA_2));
d_alpha2 = d_alpha2 * ALPHA_2_LSB; d_alpha2 = d_alpha2 * ALPHA_2_LSB;
d_alpha3 = (double)read_navigation_signed(subframe_bits, ALPHA_3,num_of_slices(ALPHA_3)); d_alpha3 = (double)read_navigation_signed(subframe_bits, ALPHA_3, num_of_slices(ALPHA_3));
d_alpha3 = d_alpha3 * ALPHA_3_LSB; d_alpha3 = d_alpha3 * ALPHA_3_LSB;
d_beta0 = (double)read_navigation_signed(subframe_bits, BETA_0,num_of_slices(BETA_0)); d_beta0 = (double)read_navigation_signed(subframe_bits, BETA_0, num_of_slices(BETA_0));
d_beta0 = d_beta0 * BETA_0_LSB; d_beta0 = d_beta0 * BETA_0_LSB;
d_beta1 = (double)read_navigation_signed(subframe_bits, BETA_1,num_of_slices(BETA_1)); d_beta1 = (double)read_navigation_signed(subframe_bits, BETA_1, num_of_slices(BETA_1));
d_beta1 = d_beta1 * BETA_1_LSB; d_beta1 = d_beta1 * BETA_1_LSB;
d_beta2 = (double)read_navigation_signed(subframe_bits, BETA_2,num_of_slices(BETA_2)); d_beta2 = (double)read_navigation_signed(subframe_bits, BETA_2, num_of_slices(BETA_2));
d_beta2 = d_beta2 * BETA_2_LSB; d_beta2 = d_beta2 * BETA_2_LSB;
d_beta3 = (double)read_navigation_signed(subframe_bits, BETA_3,num_of_slices(BETA_3)); d_beta3 = (double)read_navigation_signed(subframe_bits, BETA_3, num_of_slices(BETA_3));
d_beta3 = d_beta3 * BETA_3_LSB; d_beta3 = d_beta3 * BETA_3_LSB;
d_A1 = (double)read_navigation_signed(subframe_bits, A_1,num_of_slices(A_1)); d_A1 = (double)read_navigation_signed(subframe_bits, A_1, num_of_slices(A_1));
d_A1 = d_A1 * A_1_LSB; d_A1 = d_A1 * A_1_LSB;
d_A0 = (double)read_navigation_signed(subframe_bits, A_0,num_of_slices(A_0)); d_A0 = (double)read_navigation_signed(subframe_bits, A_0, num_of_slices(A_0));
d_A0 = d_A0 * A_0_LSB; d_A0 = d_A0 * A_0_LSB;
d_t_OT = (double)read_navigation_unsigned(subframe_bits, T_OT,num_of_slices(T_OT)); d_t_OT = (double)read_navigation_unsigned(subframe_bits, T_OT,num_of_slices(T_OT));
d_t_OT = d_t_OT * T_OT_LSB; d_t_OT = d_t_OT * T_OT_LSB;
i_WN_T = (int)read_navigation_unsigned(subframe_bits, WN_T,num_of_slices(WN_T)); i_WN_T = (int)read_navigation_unsigned(subframe_bits, WN_T, num_of_slices(WN_T));
d_DeltaT_LS = (double)read_navigation_signed(subframe_bits, DELTAT_LS,num_of_slices(DELTAT_LS)); d_DeltaT_LS = (double)read_navigation_signed(subframe_bits, DELTAT_LS, num_of_slices(DELTAT_LS));
i_WN_LSF = (int)read_navigation_unsigned(subframe_bits, WN_LSF,num_of_slices(WN_LSF)); i_WN_LSF = (int)read_navigation_unsigned(subframe_bits, WN_LSF, num_of_slices(WN_LSF));
i_DN = (int)read_navigation_unsigned(subframe_bits, DN,num_of_slices(DN));; // Right-justified ? i_DN = (int)read_navigation_unsigned(subframe_bits, DN, num_of_slices(DN));; // Right-justified ?
d_DeltaT_LSF = (double)read_navigation_signed(subframe_bits, DELTAT_LSF,num_of_slices(DELTAT_LSF)); d_DeltaT_LSF = (double)read_navigation_signed(subframe_bits, DELTAT_LSF, num_of_slices(DELTAT_LSF));
} }
if (SV_page == 25) if (SV_page == 25)
{ {
// Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32) // Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32)
//! \TODO Read Anti-Spoofing, SV config //! \TODO Read Anti-Spoofing, SV config
almanacHealth[25] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV25,num_of_slices(HEALTH_SV25)); almanacHealth[25] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV25, num_of_slices(HEALTH_SV25));
almanacHealth[26] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV26,num_of_slices(HEALTH_SV26)); almanacHealth[26] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV26, num_of_slices(HEALTH_SV26));
almanacHealth[27] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV27,num_of_slices(HEALTH_SV27)); almanacHealth[27] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV27, num_of_slices(HEALTH_SV27));
almanacHealth[28] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV28,num_of_slices(HEALTH_SV28)); almanacHealth[28] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV28, num_of_slices(HEALTH_SV28));
almanacHealth[29] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV29,num_of_slices(HEALTH_SV29)); almanacHealth[29] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV29, num_of_slices(HEALTH_SV29));
almanacHealth[30] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV30,num_of_slices(HEALTH_SV30)); almanacHealth[30] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV30, num_of_slices(HEALTH_SV30));
almanacHealth[31] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV31,num_of_slices(HEALTH_SV31)); almanacHealth[31] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV31, num_of_slices(HEALTH_SV31));
almanacHealth[32] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV32,num_of_slices(HEALTH_SV32)); almanacHealth[32] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV32, num_of_slices(HEALTH_SV32));
} }
break; break;
@ -651,8 +647,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
SV_data_ID = (int)read_navigation_unsigned(subframe_bits,SV_DATA_ID,num_of_slices(SV_DATA_ID)); SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID, num_of_slices(SV_DATA_ID));
SV_page = (int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE)); SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE, num_of_slices(SV_PAGE));
if (SV_page < 25) if (SV_page < 25)
{ {
@ -664,30 +660,30 @@ int gps_navigation_message::subframe_decoder(char *subframe)
d_Toa = d_Toa * T_OA_LSB; d_Toa = d_Toa * T_OA_LSB;
i_WN_A = (int)read_navigation_unsigned(subframe_bits,WN_A,num_of_slices(WN_A)); i_WN_A = (int)read_navigation_unsigned(subframe_bits,WN_A,num_of_slices(WN_A));
almanacHealth[1] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV1,num_of_slices(HEALTH_SV1)); almanacHealth[1] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV1, num_of_slices(HEALTH_SV1));
almanacHealth[2] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV2,num_of_slices(HEALTH_SV2)); almanacHealth[2] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV2, num_of_slices(HEALTH_SV2));
almanacHealth[3] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV3,num_of_slices(HEALTH_SV3)); almanacHealth[3] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV3, num_of_slices(HEALTH_SV3));
almanacHealth[4] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV4,num_of_slices(HEALTH_SV4)); almanacHealth[4] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV4, num_of_slices(HEALTH_SV4));
almanacHealth[5] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV5,num_of_slices(HEALTH_SV5)); almanacHealth[5] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV5, num_of_slices(HEALTH_SV5));
almanacHealth[6] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV6,num_of_slices(HEALTH_SV6)); almanacHealth[6] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV6, num_of_slices(HEALTH_SV6));
almanacHealth[7] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV7,num_of_slices(HEALTH_SV7)); almanacHealth[7] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV7, num_of_slices(HEALTH_SV7));
almanacHealth[8] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV8,num_of_slices(HEALTH_SV8)); almanacHealth[8] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV8, num_of_slices(HEALTH_SV8));
almanacHealth[9] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV9,num_of_slices(HEALTH_SV9)); almanacHealth[9] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV9, num_of_slices(HEALTH_SV9));
almanacHealth[10] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV10,num_of_slices(HEALTH_SV10)); almanacHealth[10] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV10, num_of_slices(HEALTH_SV10));
almanacHealth[11] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV11,num_of_slices(HEALTH_SV11)); almanacHealth[11] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV11, num_of_slices(HEALTH_SV11));
almanacHealth[12] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV12,num_of_slices(HEALTH_SV12)); almanacHealth[12] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV12, num_of_slices(HEALTH_SV12));
almanacHealth[13] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV13,num_of_slices(HEALTH_SV13)); almanacHealth[13] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV13, num_of_slices(HEALTH_SV13));
almanacHealth[14] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV14,num_of_slices(HEALTH_SV14)); almanacHealth[14] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV14, num_of_slices(HEALTH_SV14));
almanacHealth[15] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV15,num_of_slices(HEALTH_SV15)); almanacHealth[15] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV15, num_of_slices(HEALTH_SV15));
almanacHealth[16] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV16,num_of_slices(HEALTH_SV16)); almanacHealth[16] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV16, num_of_slices(HEALTH_SV16));
almanacHealth[17] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV17,num_of_slices(HEALTH_SV17)); almanacHealth[17] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV17, num_of_slices(HEALTH_SV17));
almanacHealth[18] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV18,num_of_slices(HEALTH_SV18)); almanacHealth[18] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV18, num_of_slices(HEALTH_SV18));
almanacHealth[19] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV19,num_of_slices(HEALTH_SV19)); almanacHealth[19] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV19, num_of_slices(HEALTH_SV19));
almanacHealth[20] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV20,num_of_slices(HEALTH_SV20)); almanacHealth[20] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV20, num_of_slices(HEALTH_SV20));
almanacHealth[21] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV21,num_of_slices(HEALTH_SV21)); almanacHealth[21] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV21, num_of_slices(HEALTH_SV21));
almanacHealth[22] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV22,num_of_slices(HEALTH_SV22)); almanacHealth[22] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV22, num_of_slices(HEALTH_SV22));
almanacHealth[23] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV23,num_of_slices(HEALTH_SV23)); almanacHealth[23] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV23, num_of_slices(HEALTH_SV23));
almanacHealth[24] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV24,num_of_slices(HEALTH_SV24)); almanacHealth[24] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV24, num_of_slices(HEALTH_SV24));
} }
break; break;
@ -706,7 +702,7 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T)); double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
// Determine if the effectivity time of the leap second event is in the past // Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF-i_GPS_week; int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week;
if ((weeksToLeapSecondEvent) >= 0) // is not in the past if ((weeksToLeapSecondEvent) >= 0) // is not in the past
{ {
@ -715,13 +711,13 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
if (weeksToLeapSecondEvent > 0) if (weeksToLeapSecondEvent > 0)
{ {
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400); t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
} }
else //we are in the same week than the leap second event else //we are in the same week than the leap second event
{ {
if (abs(gpstime_corrected-secondOfLeapSecondEvent) > 21600) if (abs(gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{ {
/* 20.3.3.5.2.4a /* 20.3.3.5.2.4a
* Whenever the effectivity time indicated by the WN_LSF and the DN values * Whenever the effectivity time indicated by the WN_LSF and the DN values
@ -731,7 +727,7 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
* the UTC/GPS-time relationship is given by * the UTC/GPS-time relationship is given by
*/ */
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400); t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
} }
else else
{ {
@ -742,15 +738,15 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
* transition is provided by the following expression for UTC: * transition is provided by the following expression for UTC:
*/ */
int W = fmod(gpstime_corrected-Delta_t_UTC-43200,86400)+43200; int W = fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400) + 43200;
t_utc_daytime =fmod(W,86400+d_DeltaT_LSF-d_DeltaT_LS); t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS);
//implement something to handle a leap second event! //implement something to handle a leap second event!
} }
if ( (gpstime_corrected - secondOfLeapSecondEvent ) > 21600) if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{ {
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T)); Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800*(double)(i_GPS_week - i_WN_T));
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400); t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
} }
} }
} }
@ -762,13 +758,12 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
* and the userÕs current time does not fall in the time span as given above * and the userÕs current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/ * in 20.3.3.5.2.4b,*/
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T)); Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400); t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
} }
double secondsOfWeekBeforeToday= 43200*floor(gpstime_corrected/43200); double secondsOfWeekBeforeToday= 43200 * floor(gpstime_corrected / 43200);
t_utc = secondsOfWeekBeforeToday+t_utc_daytime; t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc; return t_utc;
} }
@ -785,7 +780,7 @@ bool gps_navigation_message::satellite_validation()
// and check if the data have been filled (!=0) // and check if the data have been filled (!=0)
if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!=-1) if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!=-1)
{ {
flag_data_valid=true; flag_data_valid = true;
} }
return flag_data_valid; return flag_data_valid;
} }

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

@ -180,9 +180,9 @@ public:
double d_DeltaT_LSF; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s] double d_DeltaT_LSF; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s]
// Satellite velocity // Satellite velocity
double d_satvel_X; double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y; double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z; double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m]
// public functions // public functions
void reset(); void reset();