mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-01-12 10:20:32 +00:00
Some code cleaning
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
parent
dab517aff0
commit
bc62d8d5be
@ -58,10 +58,10 @@ GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration,
|
||||
DLOG(INFO) << "role " << role;
|
||||
|
||||
int averaging_depth;
|
||||
averaging_depth=configuration->property(role + ".averaging_depth", 10);
|
||||
averaging_depth = configuration->property(role + ".averaging_depth", 10);
|
||||
|
||||
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_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);
|
||||
|
||||
DLOG(INFO) << "global navigation message queue assigned to pvt ("<< pvt_->unique_id() << ")";
|
||||
DLOG(INFO) << "global navigation message queue assigned to pvt (" << pvt_->unique_id() << ")";
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -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,
|
||||
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++;
|
||||
|
||||
std::map<int,gnss_pseudorange> gnss_pseudoranges_map;
|
||||
|
||||
@ -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)
|
||||
{
|
||||
// init empty ephemeris for all the available GNSS channels
|
||||
d_nchannels=nchannels;
|
||||
d_ephemeris=new gps_navigation_message[nchannels];
|
||||
d_dump_filename=dump_filename;
|
||||
d_flag_dump_enabled=flag_dump_to_file;
|
||||
d_averaging_depth=0;
|
||||
d_nchannels = nchannels;
|
||||
d_ephemeris = new gps_navigation_message[nchannels];
|
||||
d_dump_filename = dump_filename;
|
||||
d_flag_dump_enabled = flag_dump_to_file;
|
||||
d_averaging_depth = 0;
|
||||
|
||||
// ############# 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
|
||||
{
|
||||
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);
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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;
|
||||
|
||||
//--- Make a rotation matrix -----------------------------------------------
|
||||
arma::mat R3=arma::zeros(3,3);
|
||||
R3(0, 0)= cos(omegatau);
|
||||
R3(0, 1)= sin(omegatau);
|
||||
R3(0, 2)= 0.0;
|
||||
R3(1, 0)=-sin(omegatau);
|
||||
R3(1, 1)= cos(omegatau);
|
||||
R3(1, 2)=0.0;
|
||||
R3(2, 0)= 0.0;
|
||||
R3(2, 1)= 0.0;
|
||||
R3(2, 2)= 1;
|
||||
arma::mat R3 = arma::zeros(3,3);
|
||||
R3(0, 0) = cos(omegatau);
|
||||
R3(0, 1) = sin(omegatau);
|
||||
R3(0, 2) = 0.0;
|
||||
R3(1, 0) = -sin(omegatau);
|
||||
R3(1, 1) = cos(omegatau);
|
||||
R3(1, 2) = 0.0;
|
||||
R3(2, 0) = 0.0;
|
||||
R3(2, 1) = 0.0;
|
||||
R3(2, 2) = 1;
|
||||
//--- Do the rotation ------------------------------------------------------
|
||||
arma::vec X_sat_rot;
|
||||
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 az;
|
||||
arma::mat el;
|
||||
A=arma::zeros(nmbOfSatellites,4);
|
||||
omc=arma::zeros(nmbOfSatellites,1);
|
||||
az=arma::zeros(1,nmbOfSatellites);
|
||||
el=arma::zeros(1,nmbOfSatellites);
|
||||
A=arma::zeros(nmbOfSatellites, 4);
|
||||
omc=arma::zeros(nmbOfSatellites, 1);
|
||||
az=arma::zeros(1, nmbOfSatellites);
|
||||
el=arma::zeros(1, nmbOfSatellites);
|
||||
for (int i = 0; i < nmbOfSatellites; i++)
|
||||
{
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
A(i,j)=0.0; //Armadillo
|
||||
A(i, j) = 0.0; //Armadillo
|
||||
}
|
||||
omc(i, 0)=0.0;
|
||||
az(0, i)=0.0;
|
||||
omc(i, 0) = 0.0;
|
||||
az(0, i)= 0.0;
|
||||
}
|
||||
el = az;
|
||||
|
||||
@ -174,7 +174,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
||||
if (iter == 0)
|
||||
{
|
||||
//--- Initialize variables at the first iteration --------------
|
||||
Rot_X=X.col(i); //Armadillo
|
||||
Rot_X = X.col(i); //Armadillo
|
||||
trop = 0.0;
|
||||
}
|
||||
else
|
||||
@ -194,10 +194,10 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
||||
|
||||
//--- Construct the A matrix ---------------------------------------
|
||||
//Armadillo
|
||||
A(i,0)=(-(Rot_X(0) - pos(0))) / obs(i);
|
||||
A(i,1)=(-(Rot_X(1) - pos(1))) / obs(i);
|
||||
A(i,2)=(-(Rot_X(2) - pos(2))) / obs(i);
|
||||
A(i,3)=1.0;
|
||||
A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i);
|
||||
A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i);
|
||||
A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i);
|
||||
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 ---------------------------------------------
|
||||
x = arma::solve(w*A,w*omc); // Armadillo
|
||||
x = arma::solve(w*A, w*omc); // Armadillo
|
||||
|
||||
//--- Apply position update --------------------------------------------
|
||||
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;
|
||||
|
||||
arma::mat W=arma::eye(d_nchannels,d_nchannels); //channels weights matrix
|
||||
arma::vec obs=arma::zeros(d_nchannels); // pseudoranges observation vector
|
||||
arma::mat satpos=arma::zeros(3,d_nchannels); //satellite positions matrix
|
||||
arma::mat W = arma::eye(d_nchannels,d_nchannels); //channels weights matrix
|
||||
arma::vec obs = arma::zeros(d_nchannels); // pseudoranges observation vector
|
||||
arma::mat satpos = arma::zeros(3,d_nchannels); //satellite positions matrix
|
||||
|
||||
int GPS_week = 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
|
||||
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);
|
||||
if (gnss_pseudoranges_iter!=gnss_pseudoranges_map.end())
|
||||
gnss_pseudoranges_iter = gnss_pseudoranges_map.find(d_ephemeris[i].i_satellite_PRN);
|
||||
if (gnss_pseudoranges_iter != gnss_pseudoranges_map.end())
|
||||
{
|
||||
/*!
|
||||
* \todo Place here the satellite CN0 (power level, or weight factor)
|
||||
*/
|
||||
W(i,i)=1;
|
||||
W(i,i) = 1;
|
||||
// compute the GPS master clock
|
||||
// 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_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
|
||||
d_ephemeris[i].satellitePosition(GPS_corrected_time);
|
||||
|
||||
satpos(0,i)=d_ephemeris[i].d_satpos_X;
|
||||
satpos(1,i)=d_ephemeris[i].d_satpos_Y;
|
||||
satpos(2,i)=d_ephemeris[i].d_satpos_Z;
|
||||
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";
|
||||
obs(i)=gnss_pseudoranges_iter->second.pseudorange_m+d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
|
||||
satpos(0,i) = d_ephemeris[i].d_satpos_X;
|
||||
satpos(1,i) = d_ephemeris[i].d_satpos_Y;
|
||||
satpos(2,i) = d_ephemeris[i].d_satpos_Z;
|
||||
LOG_AT_LEVEL(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X
|
||||
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
|
||||
obs(i) = gnss_pseudoranges_iter->second.pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
|
||||
valid_obs++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// no valid pseudorange for the current channel
|
||||
W(i,i)=0; // channel de-activated
|
||||
obs(i)=1; // to avoid algorithm problems (divide by zero)
|
||||
W(i,i) = 0; // channel de-activated
|
||||
obs(i) = 1; // to avoid algorithm problems (divide by zero)
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// no valid ephemeris for the current channel
|
||||
W(i,i)=0; // channel de-activated
|
||||
obs(i)=1; // to avoid algorithm problems (divide by zero)
|
||||
W(i,i) = 0; // channel de-activated
|
||||
obs(i) = 1; // to avoid algorithm problems (divide by zero)
|
||||
}
|
||||
}
|
||||
std::cout<<"PVT: valid observations="<<valid_obs<<std::endl;
|
||||
@ -280,43 +280,43 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_
|
||||
{
|
||||
arma::vec mypos;
|
||||
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);
|
||||
|
||||
// Compute UTC time and print PVT solution
|
||||
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800*(double)GPS_week);
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999,8,22),t);
|
||||
std::cout << "Position at "<<boost::posix_time::to_simple_string(p_time)<<" is Lat = " << d_latitude_d << " [deg] Long = "<< d_longitude_d <<" [deg] Height= "<<d_height_m<<" [m]" <<std::endl;
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
std::cout << "Position at " << boost::posix_time::to_simple_string(p_time) <<" is Lat = " << d_latitude_d << " [deg] Long = " << d_longitude_d << " [deg] Height= " << d_height_m << " [m]" << std::endl;
|
||||
// ######## LOG FILE #########
|
||||
if(d_flag_dump_enabled==true)
|
||||
if(d_flag_dump_enabled == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
// PVT GPS time
|
||||
tmp_double=GPS_current_time;
|
||||
tmp_double = GPS_current_time;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position East [m]
|
||||
tmp_double=mypos(0);
|
||||
tmp_double = mypos(0);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position North [m]
|
||||
tmp_double=mypos(1);
|
||||
tmp_double = mypos(1);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// ECEF User Position Up [m]
|
||||
tmp_double=mypos(2);
|
||||
tmp_double = mypos(2);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// User clock offset [s]
|
||||
tmp_double=mypos(3);
|
||||
tmp_double = mypos(3);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Latitude [deg]
|
||||
tmp_double=d_latitude_d;
|
||||
tmp_double = d_latitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Longitude [deg]
|
||||
tmp_double=d_longitude_d;
|
||||
tmp_double = d_longitude_d;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
// GEO user position Height [m]
|
||||
tmp_double=d_height_m;
|
||||
tmp_double = d_height_m;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
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
|
||||
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
|
||||
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_height_m.push_front(d_height_m);
|
||||
|
||||
d_avg_latitude_d=0;
|
||||
d_avg_longitude_d=0;
|
||||
d_avg_height_m=0;
|
||||
for (unsigned int i=0;i<d_hist_longitude_d.size();i++)
|
||||
d_avg_latitude_d = 0;
|
||||
d_avg_longitude_d = 0;
|
||||
d_avg_height_m = 0;
|
||||
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_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_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_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_longitude_d=d_avg_longitude_d/(double)d_averaging_depth;
|
||||
d_avg_height_m=d_avg_height_m/(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_height_m = d_avg_height_m / (double)d_averaging_depth;
|
||||
return true; //indicates that the returned position is valid
|
||||
}
|
||||
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_height_m.push_front(d_height_m);
|
||||
|
||||
d_avg_latitude_d=d_latitude_d;
|
||||
d_avg_longitude_d=d_longitude_d;
|
||||
d_avg_height_m=d_height_m;
|
||||
d_avg_latitude_d = d_latitude_d;
|
||||
d_avg_longitude_d = d_longitude_d;
|
||||
d_avg_height_m = d_height_m;
|
||||
return false;//indicates that the returned position is not valid yet
|
||||
// output the average, although it will not have the full historic available
|
||||
// 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;
|
||||
lambda = atan2(Y,X);
|
||||
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;
|
||||
c = a[elipsoid_selection]*sqrt(1+ex2);
|
||||
c = a[elipsoid_selection] * sqrt(1+ex2);
|
||||
double phi;
|
||||
phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection]))*f[elipsoid_selection])));
|
||||
phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
|
||||
|
||||
double h = 0.1;
|
||||
double oldh = 0;
|
||||
@ -424,19 +424,19 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
|
||||
do
|
||||
{
|
||||
oldh = h;
|
||||
N = c/sqrt(1+ex2*(cos(phi)*cos(phi)));
|
||||
phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection])*f[elipsoid_selection]*N/(N+h)))));
|
||||
h = sqrt(X*X+Y*Y)/cos(phi)-N;
|
||||
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
|
||||
phi = atan(Z / ((sqrt(X*X + Y*Y) * (1 - (2 -f[elipsoid_selection]) * f[elipsoid_selection] *N / (N + h) ))));
|
||||
h = sqrt(X*X + Y*Y) / cos(phi) - N;
|
||||
iterations = iterations + 1;
|
||||
if (iterations > 100)
|
||||
{
|
||||
std::cout<<"Failed to approximate h with desired precision. h-oldh= "<<h-oldh<<std::endl;
|
||||
std::cout << "Failed to approximate h with desired precision. h-oldh= " << h-oldh << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
while (abs(h-oldh) > 1.0e-12);
|
||||
d_latitude_d = phi*180.0/GPS_PI;
|
||||
d_longitude_d = lambda*180/GPS_PI;
|
||||
while (abs(h - oldh) > 1.0e-12);
|
||||
d_latitude_d = phi * 180.0 / GPS_PI;
|
||||
d_longitude_d = lambda * 180 / GPS_PI;
|
||||
d_height_m = h;
|
||||
}
|
||||
|
||||
|
||||
@ -45,31 +45,31 @@ bool kml_printer::set_headers(std::string filename)
|
||||
kml_file.open(filename.c_str());
|
||||
if (kml_file.is_open())
|
||||
{
|
||||
DLOG(INFO)<<"KML printer writing on "<<filename.c_str();
|
||||
kml_file<<"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\r\n"
|
||||
<<"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\r\n"
|
||||
<<" <Document>\r\n"
|
||||
<<" <name>GNSS Track</name>\r\n"
|
||||
<<" <description>GNSS-SDR Receiver position log file created at "<<asctime (timeinfo)
|
||||
<<" </description>\r\n"
|
||||
<<"<Style id=\"yellowLineGreenPoly\">\r\n"
|
||||
<<" <LineStyle>\r\n"
|
||||
<<" <color>7f00ffff</color>\r\n"
|
||||
<<" <width>1</width>\r\n"
|
||||
<<" </LineStyle>\r\n"
|
||||
<<"<PolyStyle>\r\n"
|
||||
<<" <color>7f00ff00</color>\r\n"
|
||||
<<"</PolyStyle>\r\n"
|
||||
<<"</Style>\r\n"
|
||||
<<"<Placemark>\r\n"
|
||||
<<"<name>GNSS-SDR PVT</name>\r\n"
|
||||
<<"<description>GNSS-SDR position log</description>\r\n"
|
||||
<<"<styleUrl>#yellowLineGreenPoly</styleUrl>\r\n"
|
||||
<<"<LineString>\r\n"
|
||||
<<"<extrude>0</extrude>\r\n"
|
||||
<<"<tessellate>1</tessellate>\r\n"
|
||||
<<"<altitudeMode>absolute</altitudeMode>\r\n"
|
||||
<<"<coordinates>\r\n";
|
||||
DLOG(INFO) << "KML printer writing on " << filename.c_str();
|
||||
kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
|
||||
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
|
||||
<< " <Document>" << std::endl
|
||||
<< " <name>GNSS Track</name>" << std::endl
|
||||
<< " <description>GNSS-SDR Receiver position log file created at " << asctime (timeinfo)
|
||||
<< " </description>" << std::endl
|
||||
<< "<Style id=\"yellowLineGreenPoly\">" << std::endl
|
||||
<< " <LineStyle>" << std::endl
|
||||
<< " <color>7f00ffff</color>" << std::endl
|
||||
<< " <width>1</width>" << std::endl
|
||||
<< " </LineStyle>" << std::endl
|
||||
<< "<PolyStyle>" << std::endl
|
||||
<< " <color>7f00ff00</color>" << std::endl
|
||||
<< "</PolyStyle>" << std::endl
|
||||
<< "</Style>" << std::endl
|
||||
<< "<Placemark>" << std::endl
|
||||
<< "<name>GNSS-SDR PVT</name>" << std::endl
|
||||
<< "<description>GNSS-SDR position log</description>" << std::endl
|
||||
<< "<styleUrl>#yellowLineGreenPoly</styleUrl>" << std::endl
|
||||
<< "<LineString>" << std::endl
|
||||
<< "<extrude>0</extrude>" << std::endl
|
||||
<< "<tessellate>1</tessellate>" << std::endl
|
||||
<< "<altitudeMode>absolute</altitudeMode>" << std::endl
|
||||
<< "<coordinates>" << std::endl;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
@ -84,7 +84,7 @@ bool kml_printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v
|
||||
double latitude;
|
||||
double longitude;
|
||||
double height;
|
||||
if (print_average_values==false)
|
||||
if (print_average_values == false)
|
||||
{
|
||||
latitude=position->d_latitude_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;
|
||||
height=position->d_avg_height_m;
|
||||
}
|
||||
|
||||
if (kml_file.is_open())
|
||||
{
|
||||
kml_file<<longitude<<","<<latitude<<","<<height<<"\r\n";
|
||||
kml_file << longitude << "," << latitude << "," << height << std::endl;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
@ -111,10 +112,10 @@ bool kml_printer::close_file()
|
||||
{
|
||||
if (kml_file.is_open())
|
||||
{
|
||||
kml_file<<"</coordinates>\r\n"
|
||||
<<"</LineString>\r\n"
|
||||
<<"</Placemark>\r\n"
|
||||
<<"</Document>\r\n"
|
||||
kml_file<<"</coordinates>" << std::endl
|
||||
<<"</LineString>" << std::endl
|
||||
<<"</Placemark>" << std::endl
|
||||
<<"</Document>" << std::endl
|
||||
<<"</kml>";
|
||||
kml_file.close();
|
||||
return true;
|
||||
|
||||
@ -264,12 +264,12 @@ std::string Rinex_Printer::getLocalTime()
|
||||
|
||||
std::stringstream strmHour;
|
||||
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;
|
||||
|
||||
std::stringstream strmMin;
|
||||
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;
|
||||
|
||||
if (version == 2)
|
||||
@ -371,7 +371,7 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, gps_navigation_message
|
||||
// -------- Line COMMENT
|
||||
line.clear();
|
||||
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);
|
||||
out << line << std::endl;
|
||||
|
||||
@ -547,7 +547,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, gps_navigation_message nav
|
||||
line += satelliteSystem["GPS"];
|
||||
if (nav_msg.i_satellite_PRN < 10) line += std::string("0");
|
||||
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 += year;
|
||||
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 minutes (timestring, 11, 2);
|
||||
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(month, 6);
|
||||
line += Rinex_Printer::rightJustify(day, 6);
|
||||
line += Rinex_Printer::rightJustify(hour, 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 += std::string(9, ' ');
|
||||
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)
|
||||
{
|
||||
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);
|
||||
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, ' ');
|
||||
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, ' ');
|
||||
// Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event
|
||||
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);
|
||||
}
|
||||
if (lineObs.size()<80) lineObs += std::string(80 - lineObs.size(), ' ');
|
||||
if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' ');
|
||||
out << lineObs << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
@ -72,6 +72,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
|
||||
|
||||
repeat_ = configuration->property("Acquisition" + boost::lexical_cast<
|
||||
std::string>(channel_) + ".repeat_satellite", false);
|
||||
|
||||
DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_
|
||||
<< std::endl;
|
||||
|
||||
@ -139,8 +140,8 @@ void Channel::disconnect(gr_top_block_sptr top_block)
|
||||
return;
|
||||
}
|
||||
|
||||
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(acq_->get_right_block(), 0, trk_->get_left_block(), 0);
|
||||
top_block->disconnect(trk_->get_right_block(), 0, nav_->get_left_block(), 0);
|
||||
|
||||
acq_->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()
|
||||
{
|
||||
|
||||
return nav_->get_right_block();
|
||||
}
|
||||
|
||||
|
||||
@ -34,33 +34,26 @@
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
struct Ev_gps_channel_start_acquisition: sc::event<
|
||||
Ev_gps_channel_start_acquisition>
|
||||
struct Ev_gps_channel_start_acquisition: sc::event<Ev_gps_channel_start_acquisition>
|
||||
{};
|
||||
|
||||
struct Ev_gps_channel_valid_acquisition: sc::event<
|
||||
Ev_gps_channel_valid_acquisition>
|
||||
struct Ev_gps_channel_valid_acquisition: sc::event<Ev_gps_channel_valid_acquisition>
|
||||
{};
|
||||
|
||||
struct Ev_gps_channel_failed_acquisition_repeat: sc::event<
|
||||
Ev_gps_channel_failed_acquisition_repeat>
|
||||
struct Ev_gps_channel_failed_acquisition_repeat: sc::event<Ev_gps_channel_failed_acquisition_repeat>
|
||||
{};
|
||||
|
||||
struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event<
|
||||
Ev_gps_channel_failed_acquisition_no_repeat>
|
||||
struct Ev_gps_channel_failed_acquisition_no_repeat: sc::event<Ev_gps_channel_failed_acquisition_no_repeat>
|
||||
{};
|
||||
|
||||
struct Ev_gps_channel_failed_tracking: sc::event<
|
||||
Ev_gps_channel_failed_tracking>
|
||||
struct Ev_gps_channel_failed_tracking: sc::event<Ev_gps_channel_failed_tracking>
|
||||
{};
|
||||
|
||||
struct gps_channel_idle_fsm_S0: public sc::state<gps_channel_idle_fsm_S0,
|
||||
GpsL1CaChannelFsm>
|
||||
struct gps_channel_idle_fsm_S0: public sc::state<gps_channel_idle_fsm_S0,GpsL1CaChannelFsm>
|
||||
{
|
||||
public:
|
||||
// sc::transition(event, next state)
|
||||
typedef sc::transition<Ev_gps_channel_start_acquisition,
|
||||
gps_channel_acquiring_fsm_S1> reactions;
|
||||
typedef sc::transition<Ev_gps_channel_start_acquisition, gps_channel_acquiring_fsm_S1> reactions;
|
||||
gps_channel_idle_fsm_S0(my_context 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:
|
||||
typedef mpl::list<sc::transition<
|
||||
Ev_gps_channel_failed_acquisition_no_repeat,
|
||||
gps_channel_waiting_fsm_S3>, sc::transition<
|
||||
Ev_gps_channel_failed_acquisition_repeat,
|
||||
gps_channel_acquiring_fsm_S1>, sc::transition<
|
||||
Ev_gps_channel_valid_acquisition, gps_channel_tracking_fsm_S2> >
|
||||
reactions;
|
||||
typedef mpl::list<sc::transition<Ev_gps_channel_failed_acquisition_no_repeat, gps_channel_waiting_fsm_S3>,
|
||||
sc::transition<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) :
|
||||
my_base(ctx)
|
||||
gps_channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout << "Enter Channel_Acq_S1 " << std::endl;
|
||||
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:
|
||||
typedef sc::transition<Ev_gps_channel_failed_tracking,
|
||||
gps_channel_acquiring_fsm_S1> reactions;
|
||||
typedef sc::transition<Ev_gps_channel_failed_tracking, gps_channel_acquiring_fsm_S1> reactions;
|
||||
|
||||
gps_channel_tracking_fsm_S2(my_context ctx) :
|
||||
my_base(ctx)
|
||||
gps_channel_tracking_fsm_S2(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout << "Enter Channel_tracking_S2 " << std::endl;
|
||||
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:
|
||||
typedef sc::transition<Ev_gps_channel_start_acquisition,
|
||||
gps_channel_acquiring_fsm_S1> reactions;
|
||||
typedef sc::transition<Ev_gps_channel_start_acquisition, gps_channel_acquiring_fsm_S1> reactions;
|
||||
|
||||
gps_channel_waiting_fsm_S3(my_context ctx) :
|
||||
my_base(ctx)
|
||||
gps_channel_waiting_fsm_S3(my_context ctx) : my_base(ctx)
|
||||
{
|
||||
//std::cout << "Enter Channel_waiting_S3 " << std::endl;
|
||||
context<GpsL1CaChannelFsm> ().request_satellite();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
GpsL1CaChannelFsm::GpsL1CaChannelFsm()
|
||||
{
|
||||
initiate(); //start the FSM
|
||||
}
|
||||
|
||||
GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) :
|
||||
acq_(acquisition)
|
||||
|
||||
|
||||
|
||||
GpsL1CaChannelFsm::GpsL1CaChannelFsm(AcquisitionInterface *acquisition) : acq_(acquisition)
|
||||
{
|
||||
initiate(); //start the FSM
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::Event_gps_start_acquisition()
|
||||
{
|
||||
this->process_event(Ev_gps_channel_start_acquisition());
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::Event_gps_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());
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::Event_gps_failed_acquisition_no_repeat()
|
||||
{
|
||||
this->process_event(Ev_gps_channel_failed_acquisition_no_repeat());
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::Event_gps_failed_tracking()
|
||||
{
|
||||
this->process_event(Ev_gps_channel_failed_tracking());
|
||||
}
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::set_acquisition(AcquisitionInterface *acquisition)
|
||||
{
|
||||
acq_ = acquisition;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::set_tracking(TrackingInterface *tracking)
|
||||
{
|
||||
trk_ = tracking;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::set_queue(gr_msg_queue_sptr queue)
|
||||
{
|
||||
queue_ = queue;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::set_channel(unsigned int channel)
|
||||
{
|
||||
channel_ = channel;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::start_acquisition()
|
||||
{
|
||||
acq_->reset();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::start_tracking()
|
||||
{
|
||||
LOG_AT_LEVEL(INFO) << "Channel " << channel_
|
||||
@ -193,6 +204,8 @@ void GpsL1CaChannelFsm::start_tracking()
|
||||
trk_->start_tracking();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void GpsL1CaChannelFsm::request_satellite()
|
||||
{
|
||||
ControlMessageFactory* cmf = new ControlMessageFactory();
|
||||
@ -201,6 +214,5 @@ void GpsL1CaChannelFsm::request_satellite()
|
||||
queue_->handle(cmf->GetQueueMessage(channel_, 0));
|
||||
}
|
||||
delete cmf;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -55,14 +55,14 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
|
||||
{
|
||||
|
||||
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";
|
||||
|
||||
DLOG(INFO) << "role " << role;
|
||||
|
||||
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_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
|
||||
|
||||
@ -61,23 +61,23 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
|
||||
d_queue = queue;
|
||||
d_dump = dump;
|
||||
d_nchannels = nchannels;
|
||||
d_output_rate_ms=output_rate_ms;
|
||||
d_history_prn_delay_ms= new std::deque<double>[d_nchannels];
|
||||
d_dump_filename=dump_filename;
|
||||
d_flag_averaging=flag_averaging;
|
||||
d_output_rate_ms = output_rate_ms;
|
||||
d_history_prn_delay_ms = new std::deque<double>[d_nchannels];
|
||||
d_dump_filename = dump_filename;
|
||||
d_flag_averaging = flag_averaging;
|
||||
|
||||
// ############# 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 {
|
||||
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);
|
||||
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) {
|
||||
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 )
|
||||
{
|
||||
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 traveltime_ms;
|
||||
double pseudorange_m;
|
||||
int history_shift=0;
|
||||
int history_shift = 0;
|
||||
double delta_timestamp_ms;
|
||||
double min_delta_timestamp_ms;
|
||||
double actual_min_prn_delay_ms;
|
||||
double current_prn_delay_ms;
|
||||
|
||||
int pseudoranges_reference_sat_ID=0;
|
||||
unsigned int pseudoranges_reference_sat_channel_ID=0;
|
||||
int pseudoranges_reference_sat_ID = 0;
|
||||
unsigned int pseudoranges_reference_sat_channel_ID = 0;
|
||||
|
||||
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
|
||||
*/
|
||||
@ -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
|
||||
{
|
||||
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
|
||||
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);
|
||||
flag_history_ok=false; // at least one channel need more samples
|
||||
flag_history_ok = false; // at least one channel need more samples
|
||||
}
|
||||
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++)
|
||||
{
|
||||
current_gnss_pseudorange.valid=false;
|
||||
current_gnss_pseudorange.SV_ID=0;
|
||||
current_gnss_pseudorange.pseudorange_m=0;
|
||||
current_gnss_pseudorange.timestamp_ms=0;
|
||||
*out[i]=current_gnss_pseudorange;
|
||||
current_gnss_pseudorange.valid = false;
|
||||
current_gnss_pseudorange.SV_ID = 0;
|
||||
current_gnss_pseudorange.pseudorange_m = 0;
|
||||
current_gnss_pseudorange.timestamp_ms = 0;
|
||||
*out[i] = current_gnss_pseudorange;
|
||||
}
|
||||
/*
|
||||
* 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)
|
||||
*/
|
||||
// The nearest satellite, first preamble to arrive
|
||||
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]
|
||||
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]
|
||||
|
||||
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_ID = gps_words_iter->second.satellite_PRN; // it is the reference!
|
||||
pseudoranges_reference_sat_channel_ID = gps_words_iter->second.channel_ID;
|
||||
|
||||
// The farthest satellite, last preamble to arrive
|
||||
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;
|
||||
min_delta_timestamp_ms=gps_words_iter->second.prn_delay_ms-max_preamble_delay_ms; //[ms]
|
||||
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;
|
||||
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
|
||||
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
|
||||
/*!
|
||||
@ -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
|
||||
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;
|
||||
history_shift=round(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);
|
||||
//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
|
||||
//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="<<
|
||||
@ -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";
|
||||
//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
|
||||
*/
|
||||
@ -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 ###
|
||||
|
||||
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]
|
||||
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]
|
||||
//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
|
||||
current_gnss_pseudorange.pseudorange_m=pseudorange_m;
|
||||
current_gnss_pseudorange.timestamp_ms=pseudoranges_timestamp_ms;
|
||||
current_gnss_pseudorange.SV_ID=gps_words_iter->second.satellite_PRN;
|
||||
current_gnss_pseudorange.valid=true;
|
||||
current_gnss_pseudorange.pseudorange_m = pseudorange_m;
|
||||
current_gnss_pseudorange.timestamp_ms = pseudoranges_timestamp_ms;
|
||||
current_gnss_pseudorange.SV_ID = gps_words_iter->second.satellite_PRN;
|
||||
current_gnss_pseudorange.valid = true;
|
||||
// #### 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
|
||||
try {
|
||||
double tmp_double;
|
||||
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));
|
||||
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));
|
||||
tmp_double=out[i][0].pseudorange_m;
|
||||
tmp_double = out[i][0].pseudorange_m;
|
||||
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));
|
||||
tmp_double=out[i][0].SV_ID;
|
||||
tmp_double = out[i][0].SV_ID;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
}
|
||||
catch (std::ifstream::failure e) {
|
||||
std::cout << "Exception writing observables dump file "<<e.what()<<"\r\n";
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
std::cout << "Exception writing observables dump file "<< e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
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
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0;//hold on
|
||||
return 0; //hold on
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
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++)
|
||||
{
|
||||
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
|
||||
int vector_length, gr_msg_queue_sptr queue, bool dump) :
|
||||
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_satellite = satellite;
|
||||
d_vector_length = vector_length;
|
||||
d_samples_per_bit=20; // it is exactly 1000*(1/50)=20
|
||||
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_samples_per_bit = 20; // it is exactly 1000*(1/50)=20
|
||||
d_fs_in = fs_in;
|
||||
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";
|
||||
// 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
|
||||
d_preambles_symbols=(signed int*)malloc(sizeof(signed int)*8*d_samples_per_bit);
|
||||
int n=0;
|
||||
for (int i=0;i<8;i++)
|
||||
d_preambles_symbols = (signed int*)malloc(sizeof(signed int)*8*d_samples_per_bit);
|
||||
int n = 0;
|
||||
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
|
||||
{
|
||||
d_preambles_symbols[n]=-1;
|
||||
d_preambles_symbols[n] = -1;
|
||||
}
|
||||
n++;
|
||||
}
|
||||
}
|
||||
d_sample_counter=0;
|
||||
d_preamble_code_phase_seconds=0;
|
||||
d_stat=0;
|
||||
d_preamble_index=0;
|
||||
d_symbol_accumulator_counter=0;
|
||||
d_frame_bit_index=0;
|
||||
d_sample_counter = 0;
|
||||
d_preamble_code_phase_seconds = 0;
|
||||
d_stat = 0;
|
||||
d_preamble_index = 0;
|
||||
d_symbol_accumulator_counter = 0;
|
||||
d_frame_bit_index = 0;
|
||||
|
||||
d_flag_frame_sync=false;
|
||||
d_GPS_frame_4bytes=0;
|
||||
d_prev_GPS_frame_4bytes=0;
|
||||
d_flag_parity=false;
|
||||
d_flag_frame_sync = false;
|
||||
d_GPS_frame_4bytes = 0;
|
||||
d_prev_GPS_frame_4bytes = 0;
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
|
||||
gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc()
|
||||
{
|
||||
delete d_preambles_symbols;
|
||||
d_dump_file.close();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
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
|
||||
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;
|
||||
|
||||
// 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 = parity & 0x3F;
|
||||
if (parity == (gpsword&0x3F))
|
||||
return(true);
|
||||
else
|
||||
return(false);
|
||||
|
||||
if (parity == (gpsword&0x3F)) return(true);
|
||||
else return(false);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
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) {
|
||||
int corr_value=0;
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||
{
|
||||
int corr_value = 0;
|
||||
int preamble_diff;
|
||||
|
||||
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!
|
||||
//******* 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
|
||||
{
|
||||
corr_value-=d_preambles_symbols[i];
|
||||
corr_value -= d_preambles_symbols[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_value+=d_preambles_symbols[i];
|
||||
corr_value += d_preambles_symbols[i];
|
||||
}
|
||||
}
|
||||
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)
|
||||
{
|
||||
//TODO: Rewrite with state machine
|
||||
if (d_stat==0)
|
||||
if (d_stat == 0)
|
||||
{
|
||||
d_GPS_FSM.Event_gps_word_preamble();
|
||||
d_preamble_index=d_sample_counter;//record the preamble sample stamp
|
||||
std::cout<<"Preamble detection for SAT "<<d_satellite<<std::endl;
|
||||
d_symbol_accumulator=0; //sync the symbol to bits integrator
|
||||
d_symbol_accumulator_counter=0;
|
||||
d_frame_bit_index=8;
|
||||
d_stat=1; // enter into frame pre-detection status
|
||||
d_preamble_index = d_sample_counter;//record the preamble sample stamp
|
||||
std::cout << "Preamble detection for SAT " << d_satellite<< std::endl;
|
||||
d_symbol_accumulator = 0; //sync the symbol to bits integrator
|
||||
d_symbol_accumulator_counter = 0;
|
||||
d_frame_bit_index = 8;
|
||||
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);
|
||||
if (abs(preamble_diff-6000)<1)
|
||||
if (abs(preamble_diff - 6000) < 1)
|
||||
{
|
||||
d_GPS_FSM.Event_gps_word_preamble();
|
||||
d_flag_preamble=true;
|
||||
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_code_phase_seconds=in[4][0];
|
||||
d_flag_preamble = true;
|
||||
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_code_phase_seconds = in[4][0];
|
||||
|
||||
if (!d_flag_frame_sync)
|
||||
{
|
||||
d_flag_frame_sync=true;
|
||||
std::cout<<" Frame sync SAT "<<d_satellite<<" with preamble start at "<<d_preamble_time_seconds<<" [s]"<<std::endl;
|
||||
d_flag_frame_sync = true;
|
||||
std::cout <<" Frame sync SAT " << d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d_stat==1)
|
||||
if (d_stat == 1)
|
||||
{
|
||||
preamble_diff=d_sample_counter-d_preamble_index;
|
||||
if (preamble_diff>6001)
|
||||
preamble_diff = d_sample_counter - d_preamble_index;
|
||||
if (preamble_diff > 6001)
|
||||
{
|
||||
std::cout<<"Lost of frame sync SAT "<<this->d_satellite<<" preamble_diff= "<<preamble_diff<<std::endl;
|
||||
d_stat=0; //lost of frame sync
|
||||
d_flag_frame_sync=false;
|
||||
std::cout << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff << std::endl;
|
||||
d_stat = 0; //lost of frame sync
|
||||
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];
|
||||
//******* 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++;
|
||||
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
|
||||
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_counter=0;
|
||||
d_symbol_accumulator = 0;
|
||||
d_symbol_accumulator_counter = 0;
|
||||
|
||||
//******* bits to words ******
|
||||
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
|
||||
//Each word in wordbuff is composed of:
|
||||
// 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]
|
||||
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)
|
||||
{
|
||||
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
|
||||
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))
|
||||
{
|
||||
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_flag_parity=true;
|
||||
d_flag_parity = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
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_GPS_frame_4bytes=d_GPS_frame_4bytes & 0;
|
||||
d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame
|
||||
d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -320,15 +321,15 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
|
||||
// output the frame
|
||||
consume_each(1); //one by one
|
||||
|
||||
gps_synchro.valid_word=(d_flag_frame_sync==true and d_flag_parity==true);
|
||||
gps_synchro.flag_preamble=d_flag_preamble;
|
||||
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.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.satellite_PRN=d_satellite;
|
||||
gps_synchro.channel_ID=d_channel;
|
||||
*out[0]=gps_synchro;
|
||||
gps_synchro.valid_word = (d_flag_frame_sync == true and d_flag_parity == true);
|
||||
gps_synchro.flag_preamble = d_flag_preamble;
|
||||
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.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.satellite_PRN = d_satellite;
|
||||
gps_synchro.channel_ID = d_channel;
|
||||
*out[0] = gps_synchro;
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
void gps_l1_ca_telemetry_decoder_cc::set_channel(int 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;
|
||||
}
|
||||
|
||||
|
||||
@ -64,205 +64,221 @@ using google::LogMessage;
|
||||
|
||||
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
|
||||
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,
|
||||
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,
|
||||
gr_vector_int &ninput_items_required){
|
||||
ninput_items_required[0] =(int)d_vector_length*2; //set the required available samples in each call
|
||||
gr_vector_int &ninput_items_required)
|
||||
{
|
||||
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
|
||||
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_make_io_signature(5, 5, sizeof(double))) {
|
||||
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_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_make_io_signature(3, 3, sizeof(float)),vector_length) {
|
||||
// initialize internal vars
|
||||
d_queue = queue;
|
||||
d_dump = dump;
|
||||
d_satellite = satellite;
|
||||
d_if_freq = if_freq;
|
||||
d_fs_in = fs_in;
|
||||
d_vector_length = vector_length;
|
||||
d_dump_filename =dump_filename;
|
||||
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
|
||||
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
|
||||
// initialize internal vars
|
||||
d_queue = queue;
|
||||
d_dump = dump;
|
||||
d_satellite = satellite;
|
||||
d_if_freq = if_freq;
|
||||
d_fs_in = fs_in;
|
||||
d_vector_length = vector_length;
|
||||
d_dump_filename = dump_filename;
|
||||
|
||||
// Initialize tracking ==========================================
|
||||
// Initialize tracking ==========================================
|
||||
|
||||
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
|
||||
d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
|
||||
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
|
||||
d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
|
||||
|
||||
//--- DLL variables --------------------------------------------------------
|
||||
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
|
||||
//--- DLL variables --------------------------------------------------------
|
||||
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
|
||||
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
|
||||
d_early_code= new gr_complex[d_vector_length*2];
|
||||
d_prompt_code=new gr_complex[d_vector_length*2];
|
||||
d_late_code=new gr_complex[d_vector_length*2];
|
||||
d_early_code = new gr_complex[d_vector_length*2];
|
||||
d_prompt_code = new gr_complex[d_vector_length*2];
|
||||
d_late_code = new gr_complex[d_vector_length*2];
|
||||
|
||||
// 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 ------------------------------
|
||||
// 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)
|
||||
d_rem_code_phase_samples = 0.0;
|
||||
d_rem_code_phase_samples = 0.0;
|
||||
// define residual carrier phase
|
||||
d_rem_carr_phase_rad = 0.0;
|
||||
d_rem_carr_phase_rad = 0.0;
|
||||
|
||||
// sample synchronization
|
||||
d_sample_counter=0;
|
||||
d_sample_counter_seconds=0;
|
||||
d_acq_sample_stamp=0;
|
||||
d_sample_counter = 0;
|
||||
d_sample_counter_seconds = 0;
|
||||
d_acq_sample_stamp = 0;
|
||||
|
||||
d_enable_tracking=false;
|
||||
d_pull_in=false;
|
||||
d_last_seg=0;
|
||||
d_enable_tracking = false;
|
||||
d_pull_in = false;
|
||||
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
|
||||
d_cn0_estimation_counter=0;
|
||||
d_Prompt_buffer=new gr_complex[CN0_ESTIMATION_SAMPLES];
|
||||
d_carrier_lock_test=1;
|
||||
d_CN0_SNV_dB_Hz=0;
|
||||
d_carrier_lock_fail_counter=0;
|
||||
d_carrier_lock_threshold=5;
|
||||
d_cn0_estimation_counter = 0;
|
||||
d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES];
|
||||
d_carrier_lock_test = 1;
|
||||
d_CN0_SNV_dB_Hz = 0;
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_carrier_lock_threshold = 5;
|
||||
}
|
||||
|
||||
void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
|
||||
/*
|
||||
* correct the code phase according to the delay between acq and trk
|
||||
*/
|
||||
unsigned long int acq_trk_diff_samples;
|
||||
float acq_trk_diff_seconds;
|
||||
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp;//-d_vector_length;
|
||||
std::cout<<"acq_trk_diff_samples="<<acq_trk_diff_samples<<"\r\n";
|
||||
acq_trk_diff_seconds=(float)acq_trk_diff_samples/(float)d_fs_in;
|
||||
//doppler effect
|
||||
// Fd=(C/(C+Vr))*F
|
||||
float radial_velocity;
|
||||
radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
|
||||
// new chip and prn sequence periods based on acq Doppler
|
||||
float T_chip_mod_seconds;
|
||||
float T_prn_mod_seconds;
|
||||
float T_prn_mod_samples;
|
||||
d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ;
|
||||
T_chip_mod_seconds=1/d_code_freq_hz;
|
||||
T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
|
||||
T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
|
||||
void gps_l1_ca_dll_pll_tracking_cc::start_tracking()
|
||||
{
|
||||
/*
|
||||
* correct the code phase according to the delay between acq and trk
|
||||
*/
|
||||
unsigned long int acq_trk_diff_samples;
|
||||
float acq_trk_diff_seconds;
|
||||
acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
|
||||
std::cout << "acq_trk_diff_samples=" << acq_trk_diff_samples << std::endl;
|
||||
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in;
|
||||
//doppler effect
|
||||
// Fd=(C/(C+Vr))*F
|
||||
float radial_velocity;
|
||||
radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
|
||||
// new chip and prn sequence periods based on acq Doppler
|
||||
float T_chip_mod_seconds;
|
||||
float T_prn_mod_seconds;
|
||||
float T_prn_mod_samples;
|
||||
d_code_freq_hz = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
|
||||
T_chip_mod_seconds = 1/d_code_freq_hz;
|
||||
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
|
||||
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
|
||||
|
||||
|
||||
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
||||
d_next_prn_length_samples=round(T_prn_mod_samples);
|
||||
#else
|
||||
d_next_prn_length_samples=std::round(T_prn_mod_samples);
|
||||
#endif
|
||||
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
||||
d_next_prn_length_samples = round(T_prn_mod_samples);
|
||||
#else
|
||||
d_next_prn_length_samples = std::round(T_prn_mod_samples);
|
||||
#endif
|
||||
|
||||
|
||||
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_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_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;
|
||||
N_prn_diff=acq_trk_diff_seconds/T_prn_true_seconds;
|
||||
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);
|
||||
if (corrected_acq_phase_samples<0)
|
||||
{
|
||||
corrected_acq_phase_samples=T_prn_mod_samples+corrected_acq_phase_samples;
|
||||
}
|
||||
delay_correction_samples=d_acq_code_phase_samples-corrected_acq_phase_samples;
|
||||
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
|
||||
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);
|
||||
if (corrected_acq_phase_samples < 0)
|
||||
{
|
||||
corrected_acq_phase_samples = T_prn_mod_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;
|
||||
// DLL/PLL filter initialization
|
||||
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_carrier_doppler_hz = d_acq_carrier_doppler_hz;
|
||||
// DLL/PLL filter initialization
|
||||
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
|
||||
|
||||
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
|
||||
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[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1];
|
||||
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
|
||||
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[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
|
||||
|
||||
d_carrier_lock_fail_counter=0;
|
||||
d_rem_code_phase_samples=0;
|
||||
d_rem_carr_phase_rad=0;
|
||||
d_rem_code_phase_samples=0;
|
||||
d_next_rem_code_phase_samples=0;
|
||||
d_acc_carrier_phase_rad=0;
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_rem_code_phase_samples = 0;
|
||||
d_rem_carr_phase_rad = 0;
|
||||
d_rem_code_phase_samples = 0;
|
||||
d_next_rem_code_phase_samples = 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
|
||||
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 ";
|
||||
// DEBUG OUTPUT
|
||||
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" << std::endl;
|
||||
|
||||
// enable tracking
|
||||
d_pull_in=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";
|
||||
// enable tracking
|
||||
d_pull_in = 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 << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
|
||||
{
|
||||
float tcode_chips;
|
||||
float rem_code_phase_chips;
|
||||
int associated_chip_index;
|
||||
int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS;
|
||||
// unified loop for E, P, L code vectors
|
||||
rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in);
|
||||
tcode_chips=-rem_code_phase_chips;
|
||||
for (int i=0;i<d_current_prn_length_samples;i++)
|
||||
{
|
||||
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
||||
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];
|
||||
associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips));
|
||||
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));
|
||||
d_late_code[i] = d_ca_code[associated_chip_index];
|
||||
tcode_chips=tcode_chips+d_code_phase_step_chips;
|
||||
#else
|
||||
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];
|
||||
associated_chip_index = 1+std::round(fmod(tcode_chips, code_length_chips));
|
||||
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));
|
||||
d_late_code[i] = d_ca_code[associated_chip_index];
|
||||
tcode_chips=tcode_chips+d_code_phase_step_chips;
|
||||
#endif
|
||||
}
|
||||
float tcode_chips;
|
||||
float rem_code_phase_chips;
|
||||
int associated_chip_index;
|
||||
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
|
||||
// unified loop for E, P, L code vectors
|
||||
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
|
||||
tcode_chips = -rem_code_phase_chips;
|
||||
for (int i=0; i<d_current_prn_length_samples; i++)
|
||||
{
|
||||
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
||||
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];
|
||||
associated_chip_index = 1 + round(fmod(tcode_chips, code_length_chips));
|
||||
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));
|
||||
d_late_code[i] = d_ca_code[associated_chip_index];
|
||||
tcode_chips = tcode_chips + d_code_phase_step_chips;
|
||||
#else
|
||||
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];
|
||||
associated_chip_index = 1 + std::round(fmod(tcode_chips, code_length_chips));
|
||||
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));
|
||||
d_late_code[i] = d_ca_code[associated_chip_index];
|
||||
tcode_chips = tcode_chips + d_code_phase_step_chips;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
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_rad=d_rem_carr_phase_rad;
|
||||
for(int i = 0; i < d_current_prn_length_samples; i++) {
|
||||
d_carr_sign[i] = gr_complex(cos(phase_rad),sin(phase_rad));
|
||||
phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz / (float)d_fs_in;
|
||||
phase_rad = d_rem_carr_phase_rad;
|
||||
for(int i = 0; i < d_current_prn_length_samples; i++)
|
||||
{
|
||||
d_carr_sign[i] = gr_complex(cos(phase_rad), sin(phase_rad));
|
||||
phase_rad += phase_step_rad;
|
||||
}
|
||||
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_rem_carr_phase_rad = fmod(phase_rad, TWO_PI);
|
||||
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_early_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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* Tracking signal processing
|
||||
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
|
||||
*/
|
||||
|
||||
int gps_l1_ca_dll_pll_tracking_cc::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))
|
||||
// {
|
||||
// std::cout<<"End of signal detected\r\n";
|
||||
// const int samples_available = ninput_items[0];
|
||||
// consume_each(samples_available);
|
||||
// return 0;
|
||||
// }
|
||||
// if ((unsigned int)ninput_items[0]<(d_vector_length*2))
|
||||
// {
|
||||
// std::cout<<"End of signal detected\r\n";
|
||||
// const int samples_available = ninput_items[0];
|
||||
// consume_each(samples_available);
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// process vars
|
||||
float carr_error;
|
||||
float carr_nco;
|
||||
float code_error;
|
||||
float code_nco;
|
||||
d_Early=gr_complex(0,0);
|
||||
d_Prompt=gr_complex(0,0);
|
||||
d_Late=gr_complex(0,0);
|
||||
// process vars
|
||||
float carr_error;
|
||||
float carr_nco;
|
||||
float code_error;
|
||||
float code_nco;
|
||||
d_Early = gr_complex(0,0);
|
||||
d_Prompt = gr_complex(0,0);
|
||||
d_Late = gr_complex(0,0);
|
||||
|
||||
if (d_enable_tracking==true){
|
||||
/*
|
||||
* Receiver signal alignment
|
||||
*/
|
||||
if (d_pull_in==true)
|
||||
{
|
||||
int samples_offset;
|
||||
if (d_enable_tracking==true){
|
||||
/*
|
||||
* Receiver signal alignment
|
||||
*/
|
||||
if (d_pull_in==true)
|
||||
{
|
||||
int samples_offset;
|
||||
|
||||
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
|
||||
float acq_trk_shif_correction_samples;
|
||||
int acq_to_trk_delay_samples;
|
||||
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);
|
||||
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
|
||||
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
||||
samples_offset=round(d_acq_code_phase_samples+acq_trk_shif_correction_samples);
|
||||
#else
|
||||
samples_offset=std::round(d_acq_code_phase_samples+acq_trk_shif_correction_samples);
|
||||
#endif
|
||||
// /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=d_sample_counter+samples_offset; //count for the processed samples
|
||||
d_pull_in=false;
|
||||
//std::cout<<" samples_offset="<<samples_offset<<"\r\n";
|
||||
consume_each(samples_offset); //shift input to perform alignement with local replica
|
||||
return 1;
|
||||
}
|
||||
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
|
||||
float acq_trk_shif_correction_samples;
|
||||
int acq_to_trk_delay_samples;
|
||||
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);
|
||||
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
|
||||
#ifdef GNSS_SDR_USE_BOOST_ROUND
|
||||
samples_offset=round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
|
||||
#else
|
||||
samples_offset=std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
|
||||
#endif
|
||||
// /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 =d_sample_counter + samples_offset; //count for the processed samples
|
||||
d_pull_in = false;
|
||||
//std::cout<<" samples_offset="<<samples_offset<<"\r\n";
|
||||
consume_each(samples_offset); //shift input to perform alignement with local replica
|
||||
return 1;
|
||||
}
|
||||
|
||||
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
|
||||
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;
|
||||
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
|
||||
double **out = (double **) &output_items[0];
|
||||
|
||||
update_local_code();
|
||||
update_local_carrier();
|
||||
// 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 << 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
|
||||
/*!
|
||||
* \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;
|
||||
gr_complex bb_signal_sample(0,0);
|
||||
|
||||
// Compute DLL error and update code NCO
|
||||
code_error=dll_nc_e_minus_l_normalized(d_Early,d_Late);
|
||||
// Implement code loop filter and generate NCO command
|
||||
code_nco=d_code_loop_filter.get_code_nco(code_error);
|
||||
// Modify code freq based on NCO command
|
||||
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - code_nco;
|
||||
// perform Early, Prompt and Late correlation
|
||||
/*!
|
||||
* \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];
|
||||
}
|
||||
|
||||
// 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;
|
||||
// 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;
|
||||
|
||||
// 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;
|
||||
}
|
||||
// Compute DLL error and update code NCO
|
||||
code_error = dll_nc_e_minus_l_normalized(d_Early, d_Late);
|
||||
// Implement code loop filter and generate NCO command
|
||||
code_nco = d_code_loop_filter.get_code_nco(code_error);
|
||||
// Modify code freq based on NCO command
|
||||
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - code_nco;
|
||||
|
||||
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
|
||||
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
|
||||
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
|
||||
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!
|
||||
*/
|
||||
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
|
||||
if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
|
||||
{
|
||||
// fill buffer with prompt correlator output values
|
||||
d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt;
|
||||
d_cn0_estimation_counter++;
|
||||
}else{
|
||||
d_cn0_estimation_counter=0;
|
||||
d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in);
|
||||
d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
|
||||
// ###### TRACKING UNLOCK NOTIFICATION #####
|
||||
int tracking_message;
|
||||
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>MINIMUM_VALID_CN0)
|
||||
{
|
||||
d_carrier_lock_fail_counter++;
|
||||
}else{
|
||||
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
|
||||
}
|
||||
if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER)
|
||||
{
|
||||
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
|
||||
tracking_message=3; //loss of lock
|
||||
d_channel_internal_queue->push(tracking_message);
|
||||
d_carrier_lock_fail_counter=0;
|
||||
d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
|
||||
/*!
|
||||
* \todo Improve the lock detection algorithm!
|
||||
*/
|
||||
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
|
||||
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
|
||||
{
|
||||
// fill buffer with prompt correlator output values
|
||||
d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
|
||||
d_cn0_estimation_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_cn0_estimation_counter = 0;
|
||||
d_CN0_SNV_dB_Hz = gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in);
|
||||
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
|
||||
// ###### TRACKING UNLOCK NOTIFICATION #####
|
||||
int tracking_message;
|
||||
if (d_carrier_lock_test < d_carrier_lock_threshold or d_carrier_lock_test > MINIMUM_VALID_CN0)
|
||||
{
|
||||
d_carrier_lock_fail_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
|
||||
}
|
||||
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
|
||||
{
|
||||
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
|
||||
*/
|
||||
// ########### Output the tracking data to navigation and PVT ##########
|
||||
// Output channel 0: Prompt correlator output Q
|
||||
*out[0]=(double)d_Prompt.real();
|
||||
// Output channel 1: Prompt correlator output I
|
||||
*out[1]=(double)d_Prompt.imag();
|
||||
// Output channel 2: PRN absolute delay [s]
|
||||
*out[2]=d_sample_counter_seconds;
|
||||
// Output channel 3: d_acc_carrier_phase_rad [rad]
|
||||
*out[3]=(double)d_acc_carrier_phase_rad;
|
||||
// Output channel 4: PRN code phase [s]
|
||||
*out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in);
|
||||
/*!
|
||||
* \todo Output the CN0
|
||||
*/
|
||||
// ########### Output the tracking data to navigation and PVT ##########
|
||||
// Output channel 0: Prompt correlator output Q
|
||||
*out[0] = (double)d_Prompt.real();
|
||||
// Output channel 1: Prompt correlator output I
|
||||
*out[1] = (double)d_Prompt.imag();
|
||||
// Output channel 2: PRN absolute delay [s]
|
||||
*out[2] = d_sample_counter_seconds;
|
||||
// Output channel 3: d_acc_carrier_phase_rad [rad]
|
||||
*out[3] = (double)d_acc_carrier_phase_rad;
|
||||
// Output channel 4: PRN code phase [s]
|
||||
*out[4] = (double)d_code_phase_samples * (1 / (float)d_fs_in);
|
||||
|
||||
// ########## DEBUG OUTPUT
|
||||
/*!
|
||||
* \todo The stop timer has to be moved to the signal source!
|
||||
*/
|
||||
// debug: Second counter in channel 0
|
||||
if (d_channel==0)
|
||||
{
|
||||
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
|
||||
{
|
||||
d_last_seg=floor(d_sample_counter/d_fs_in);
|
||||
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<<"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!
|
||||
}
|
||||
}else
|
||||
{
|
||||
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;
|
||||
//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;
|
||||
*out[1]=0;
|
||||
*out[2]=0;
|
||||
*out[3]=0;
|
||||
*out[4]=0;
|
||||
}
|
||||
// ########## DEBUG OUTPUT
|
||||
/*!
|
||||
* \todo The stop timer has to be moved to the signal source!
|
||||
*/
|
||||
// debug: Second counter in channel 0
|
||||
if (d_channel == 0)
|
||||
{
|
||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter / d_fs_in);
|
||||
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<<"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!
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
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;
|
||||
//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;
|
||||
*out[1] = 0;
|
||||
*out[2] = 0;
|
||||
*out[3] = 0;
|
||||
*out[4] = 0;
|
||||
}
|
||||
|
||||
if(d_dump) {
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
float prompt_I;
|
||||
float prompt_Q;
|
||||
float tmp_E,tmp_P,tmp_L;
|
||||
float tmp_float;
|
||||
prompt_I=d_Prompt.imag();
|
||||
prompt_Q=d_Prompt.real();
|
||||
tmp_E=std::abs<float>(d_Early);
|
||||
tmp_P=std::abs<float>(d_Prompt);
|
||||
tmp_L=std::abs<float>(d_Late);
|
||||
try {
|
||||
// EPR
|
||||
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_L, sizeof(float));
|
||||
// PROMPT I and Q (to analyze navigation symbols)
|
||||
d_dump_file.write((char*)&prompt_I, sizeof(float));
|
||||
d_dump_file.write((char*)&prompt_Q, sizeof(float));
|
||||
// PRN start sample stamp
|
||||
//tmp_float=(float)d_sample_counter;
|
||||
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
|
||||
// accumulated carrier phase
|
||||
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
|
||||
if(d_dump) {
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
float prompt_I;
|
||||
float prompt_Q;
|
||||
float tmp_E, tmp_P, tmp_L;
|
||||
float tmp_float;
|
||||
prompt_I = d_Prompt.imag();
|
||||
prompt_Q = d_Prompt.real();
|
||||
tmp_E = std::abs<float>(d_Early);
|
||||
tmp_P = std::abs<float>(d_Prompt);
|
||||
tmp_L = std::abs<float>(d_Late);
|
||||
try
|
||||
{
|
||||
// EPR
|
||||
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_L, sizeof(float));
|
||||
// PROMPT I and Q (to analyze navigation symbols)
|
||||
d_dump_file.write((char*)&prompt_I, sizeof(float));
|
||||
d_dump_file.write((char*)&prompt_Q, sizeof(float));
|
||||
// PRN start sample stamp
|
||||
//tmp_float=(float)d_sample_counter;
|
||||
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
|
||||
// accumulated carrier phase
|
||||
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
|
||||
|
||||
// carrier and code frequency
|
||||
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
|
||||
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
|
||||
// carrier and code frequency
|
||||
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
|
||||
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
|
||||
|
||||
//PLL commands
|
||||
d_dump_file.write((char*)&carr_error, sizeof(float));
|
||||
d_dump_file.write((char*)&carr_nco, sizeof(float));
|
||||
//PLL commands
|
||||
d_dump_file.write((char*)&carr_error, sizeof(float));
|
||||
d_dump_file.write((char*)&carr_nco, sizeof(float));
|
||||
|
||||
//DLL commands
|
||||
d_dump_file.write((char*)&code_error, sizeof(float));
|
||||
d_dump_file.write((char*)&code_nco, sizeof(float));
|
||||
//DLL commands
|
||||
d_dump_file.write((char*)&code_error, sizeof(float));
|
||||
d_dump_file.write((char*)&code_nco, sizeof(float));
|
||||
|
||||
// CN0 and carrier lock test
|
||||
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
|
||||
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
|
||||
// CN0 and carrier lock test
|
||||
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
|
||||
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
|
||||
|
||||
// AUX vars (for debug purposes)
|
||||
tmp_float=0;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
|
||||
}
|
||||
catch (std::ifstream::failure e) {
|
||||
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
|
||||
}
|
||||
}
|
||||
// AUX vars (for debug purposes)
|
||||
tmp_float=0;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
|
||||
}
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
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
|
||||
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
|
||||
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
|
||||
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 += 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
|
||||
}
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite;
|
||||
}
|
||||
|
||||
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;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump==true)
|
||||
{
|
||||
if (d_dump_file.is_open()==false)
|
||||
{
|
||||
try {
|
||||
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
||||
d_dump_filename.append(".dat");
|
||||
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
|
||||
}
|
||||
catch (std::ifstream::failure e) {
|
||||
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
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;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump==true)
|
||||
{
|
||||
if (d_dump_file.is_open()==false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
||||
d_dump_filename.append(".dat");
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit);
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
std::cout << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
|
||||
}
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
std::cout << "channel "<< d_channel << " Exception opening trk dump file " << e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int 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)
|
||||
{
|
||||
d_channel_internal_queue = channel_internal_queue;
|
||||
|
||||
@ -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);
|
||||
//CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length);
|
||||
float SNR, SNR_dB_Hz;
|
||||
float tmp_abs_I,tmp_abs_Q;
|
||||
float Psig,Ptot;
|
||||
float tmp_abs_I, tmp_abs_Q;
|
||||
float Psig, Ptot;
|
||||
//float M2,M4;
|
||||
Psig=0;
|
||||
Ptot=0;
|
||||
for (int i=0;i<length;i++)
|
||||
Psig = 0;
|
||||
Ptot = 0;
|
||||
for (int i=0; i<length; i++)
|
||||
{
|
||||
tmp_abs_I=std::abs(Prompt_buffer[i].imag());
|
||||
tmp_abs_Q=std::abs(Prompt_buffer[i].real());
|
||||
Psig+=tmp_abs_I;
|
||||
Ptot+=Prompt_buffer[i].imag()*Prompt_buffer[i].imag()+Prompt_buffer[i].real()*Prompt_buffer[i].real();
|
||||
tmp_abs_I = std::abs(Prompt_buffer[i].imag());
|
||||
tmp_abs_Q = std::abs(Prompt_buffer[i].real());
|
||||
Psig += tmp_abs_I;
|
||||
Ptot += Prompt_buffer[i].imag()*Prompt_buffer[i].imag() + Prompt_buffer[i].real()*Prompt_buffer[i].real();
|
||||
}
|
||||
Psig=Psig/(float)length;
|
||||
Psig=Psig*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);
|
||||
Psig = Psig / (float)length;
|
||||
Psig = Psig*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);
|
||||
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;
|
||||
//NBP=sum(imag(x((n-N+1):n)).^2) - sum(real(x((n-N+1):n)).^2);
|
||||
//LOCK(count)=NBD/NBP;
|
||||
float tmp_abs_I,tmp_abs_Q;
|
||||
float tmp_sum_abs_I,tmp_sum_abs_Q;
|
||||
float tmp_sum_sqr_I,tmp_sum_sqr_Q;
|
||||
float tmp_abs_I, tmp_abs_Q;
|
||||
float tmp_sum_abs_I, tmp_sum_abs_Q;
|
||||
float tmp_sum_sqr_I, tmp_sum_sqr_Q;
|
||||
tmp_sum_abs_I=0;
|
||||
tmp_sum_abs_Q=0;
|
||||
tmp_sum_sqr_I=0;
|
||||
tmp_sum_sqr_Q=0;
|
||||
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_Q=std::abs(Prompt_buffer[i].real());
|
||||
tmp_sum_abs_I+=tmp_abs_I;
|
||||
tmp_sum_abs_Q+=tmp_abs_Q;
|
||||
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_abs_I = std::abs(Prompt_buffer[i].imag());
|
||||
tmp_abs_Q = std::abs(Prompt_buffer[i].real());
|
||||
tmp_sum_abs_I += tmp_abs_I;
|
||||
tmp_sum_abs_Q += tmp_abs_Q;
|
||||
tmp_sum_sqr_I += (Prompt_buffer[i].imag()*Prompt_buffer[i].imag());
|
||||
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;
|
||||
NBP=tmp_sum_sqr_I-tmp_sum_sqr_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;
|
||||
return NBD/NBP;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -38,7 +38,8 @@
|
||||
#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
|
||||
float Wn;
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void tracking_2nd_DLL_filter::set_DLL_BW(float dll_bw_hz)
|
||||
{
|
||||
//Calculate filter coefficient values
|
||||
d_dllnoisebandwidth=dll_bw_hz;
|
||||
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values
|
||||
d_dllnoisebandwidth =dll_bw_hz;
|
||||
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)
|
||||
{
|
||||
// code tracking loop parameters
|
||||
d_old_code_nco = 0.0;
|
||||
d_old_code_error = 0.0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
|
||||
{
|
||||
float code_nco;
|
||||
@ -70,13 +77,14 @@ float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
|
||||
return code_nco;
|
||||
}
|
||||
|
||||
|
||||
|
||||
tracking_2nd_DLL_filter::tracking_2nd_DLL_filter ()
|
||||
{
|
||||
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 ()
|
||||
{
|
||||
{}
|
||||
|
||||
}
|
||||
|
||||
@ -47,9 +47,9 @@
|
||||
float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1, float t2)
|
||||
{
|
||||
float cross,dot;
|
||||
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();
|
||||
return atan2(cross,dot)/(t2-t1);
|
||||
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();
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
if (prompt_s1.imag()!=0.0)
|
||||
if (prompt_s1.imag() != 0.0)
|
||||
{
|
||||
return atan(prompt_s1.real()/prompt_s1.imag());
|
||||
}else{
|
||||
return 0;
|
||||
return atan(prompt_s1.real() / prompt_s1.imag());
|
||||
}
|
||||
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 P_early, P_late;
|
||||
P_early=std::abs(early_s1);
|
||||
P_late=std::abs(late_s1);
|
||||
return (P_early-P_late)/((P_early+P_late));
|
||||
P_early = std::abs(early_s1);
|
||||
P_late = std::abs(late_s1);
|
||||
return (P_early - P_late) / ((P_early + P_late));
|
||||
}
|
||||
|
||||
@ -39,59 +39,59 @@
|
||||
|
||||
void gps_navigation_message::reset()
|
||||
{
|
||||
d_TOW=0;
|
||||
d_IODE_SF2=0;
|
||||
d_IODE_SF3=0;
|
||||
d_Crs=0;
|
||||
d_Delta_n=0;
|
||||
d_M_0=0;
|
||||
d_Cuc=0;
|
||||
d_e_eccentricity=0;
|
||||
d_Cus=0;
|
||||
d_sqrt_A=0;
|
||||
d_Toe=0;
|
||||
d_Toc=0;
|
||||
d_Cic=0;
|
||||
d_OMEGA0=0;
|
||||
d_Cis=0;
|
||||
d_i_0=0;
|
||||
d_Crc=0;
|
||||
d_OMEGA=0;
|
||||
d_OMEGA_DOT=0;
|
||||
d_IDOT=0;
|
||||
i_code_on_L2=0;
|
||||
i_GPS_week=0;
|
||||
b_L2_P_data_flag=false;
|
||||
i_SV_accuracy=0;
|
||||
i_SV_health=0;
|
||||
d_TGD=0;
|
||||
d_IODC=-1;
|
||||
i_AODO=0;
|
||||
d_TOW = 0;
|
||||
d_IODE_SF2 = 0;
|
||||
d_IODE_SF3 = 0;
|
||||
d_Crs = 0;
|
||||
d_Delta_n = 0;
|
||||
d_M_0 = 0;
|
||||
d_Cuc = 0;
|
||||
d_e_eccentricity = 0;
|
||||
d_Cus = 0;
|
||||
d_sqrt_A = 0;
|
||||
d_Toe = 0;
|
||||
d_Toc = 0;
|
||||
d_Cic = 0;
|
||||
d_OMEGA0 = 0;
|
||||
d_Cis = 0;
|
||||
d_i_0 = 0;
|
||||
d_Crc = 0;
|
||||
d_OMEGA = 0;
|
||||
d_OMEGA_DOT = 0;
|
||||
d_IDOT = 0;
|
||||
i_code_on_L2 = 0;
|
||||
i_GPS_week = 0;
|
||||
b_L2_P_data_flag = false;
|
||||
i_SV_accuracy = 0;
|
||||
i_SV_health = 0;
|
||||
d_TGD = 0;
|
||||
d_IODC = -1;
|
||||
i_AODO = 0;
|
||||
|
||||
b_fit_interval_flag=false;
|
||||
d_spare1=0;
|
||||
d_spare2=0;
|
||||
b_fit_interval_flag = false;
|
||||
d_spare1 = 0;
|
||||
d_spare2 = 0;
|
||||
|
||||
d_A_f0=0;
|
||||
d_A_f1=0;
|
||||
d_A_f2=0;
|
||||
d_A_f0 = 0;
|
||||
d_A_f1 = 0;
|
||||
d_A_f2 = 0;
|
||||
|
||||
//clock terms
|
||||
//d_master_clock=0;
|
||||
d_dtr=0;
|
||||
d_satClkCorr=0;
|
||||
d_dtr = 0;
|
||||
d_satClkCorr = 0;
|
||||
|
||||
// satellite positions
|
||||
d_satpos_X=0;
|
||||
d_satpos_Y=0;
|
||||
d_satpos_Z=0;
|
||||
d_satpos_X = 0;
|
||||
d_satpos_Y = 0;
|
||||
d_satpos_Z = 0;
|
||||
|
||||
// info
|
||||
i_channel_ID=0;
|
||||
i_satellite_PRN=0;
|
||||
i_channel_ID = 0;
|
||||
i_satellite_PRN = 0;
|
||||
|
||||
// time synchro
|
||||
d_subframe1_timestamp_ms=0;
|
||||
d_subframe1_timestamp_ms = 0;
|
||||
|
||||
// flags
|
||||
b_alert_flag = false;
|
||||
@ -100,35 +100,35 @@ void gps_navigation_message::reset()
|
||||
|
||||
|
||||
// Ionosphere and UTC
|
||||
d_alpha0=0;
|
||||
d_alpha1=0;
|
||||
d_alpha2=0;
|
||||
d_alpha3=0;
|
||||
d_beta0=0;
|
||||
d_beta1=0;
|
||||
d_beta2=0;
|
||||
d_beta3=0;
|
||||
d_A1=0;
|
||||
d_A0=0;
|
||||
d_t_OT=0;
|
||||
i_WN_T=0;
|
||||
d_DeltaT_LS=0;
|
||||
i_WN_LSF=0;
|
||||
i_DN=0;
|
||||
d_DeltaT_LSF=0;
|
||||
d_alpha0 = 0;
|
||||
d_alpha1 = 0;
|
||||
d_alpha2 = 0;
|
||||
d_alpha3 = 0;
|
||||
d_beta0 = 0;
|
||||
d_beta1 = 0;
|
||||
d_beta2 = 0;
|
||||
d_beta3 = 0;
|
||||
d_A1 = 0;
|
||||
d_A0 = 0;
|
||||
d_t_OT = 0;
|
||||
i_WN_T = 0;
|
||||
d_DeltaT_LS = 0;
|
||||
i_WN_LSF = 0;
|
||||
i_DN = 0;
|
||||
d_DeltaT_LSF= 0;
|
||||
|
||||
//Almanac
|
||||
d_Toa = 0;
|
||||
i_WN_A = 0;
|
||||
for (int i=1; i < 32; i++ )
|
||||
{
|
||||
almanacHealth[i]=0;
|
||||
almanacHealth[i] = 0;
|
||||
}
|
||||
|
||||
// Satellite velocity
|
||||
d_satvel_X=0;
|
||||
d_satvel_Y=0;
|
||||
d_satvel_Z=0;
|
||||
d_satvel_X = 0;
|
||||
d_satvel_Y = 0;
|
||||
d_satvel_Z = 0;
|
||||
|
||||
//Plane A (info from http://www.navcen.uscg.gov/?Do=constellationStatus)
|
||||
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)
|
||||
{
|
||||
std::cout << " Word =";
|
||||
std::cout<<std::bitset<32>(GPS_word);
|
||||
std::cout<<std::endl;
|
||||
std::cout << std::bitset<32>(GPS_word);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
|
||||
@ -192,13 +192,13 @@ bool gps_navigation_message::read_navigation_bool(std::bitset<GPS_SUBFRAME_BITS>
|
||||
{
|
||||
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
|
||||
{
|
||||
value=false;
|
||||
value = false;
|
||||
}
|
||||
|
||||
return value;
|
||||
@ -212,15 +212,15 @@ unsigned long int gps_navigation_message::read_navigation_unsigned(std::bitset<G
|
||||
{
|
||||
unsigned long int value;
|
||||
|
||||
value=0;
|
||||
for (int i=0;i<num_of_slices;i++)
|
||||
value = 0;
|
||||
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
|
||||
if (bits[GPS_SUBFRAME_BITS-slices[i].position-j]==1)
|
||||
value <<= 1; //shift left
|
||||
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 value=0;
|
||||
signed long int value = 0;
|
||||
|
||||
// read the MSB and perform the sign extension
|
||||
if (bits[GPS_SUBFRAME_BITS-slices[0].position]==1)
|
||||
{
|
||||
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&=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
|
||||
}
|
||||
@ -270,10 +273,11 @@ double gps_navigation_message::check_t(double time)
|
||||
if (time > half_week)
|
||||
{
|
||||
corrTime = time - 2*half_week;
|
||||
}else if (time < -half_week)
|
||||
{
|
||||
corrTime = time + 2*half_week;
|
||||
}
|
||||
}
|
||||
else if (time < -half_week)
|
||||
{
|
||||
corrTime = time + 2*half_week;
|
||||
}
|
||||
return corrTime;
|
||||
}
|
||||
|
||||
@ -314,25 +318,25 @@ void gps_navigation_message::satellitePosition(double transmitTime)
|
||||
// Find satellite's position ----------------------------------------------
|
||||
|
||||
// Restore semi-major axis
|
||||
a = d_sqrt_A*d_sqrt_A;
|
||||
a = d_sqrt_A*d_sqrt_A;
|
||||
|
||||
// Time from ephemeris reference epoch
|
||||
tk = check_t(transmitTime - d_Toe);
|
||||
tk = check_t(transmitTime - d_Toe);
|
||||
|
||||
// Computed mean motion
|
||||
n0 = sqrt(GM / (a*a*a));
|
||||
n0 = sqrt(GM / (a*a*a));
|
||||
|
||||
// Corrected mean motion
|
||||
n = n0 + d_Delta_n;
|
||||
n = n0 + d_Delta_n;
|
||||
|
||||
// Mean anomaly
|
||||
M = d_M_0 + n * tk;
|
||||
M = d_M_0 + n * tk;
|
||||
|
||||
// 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
|
||||
E = M;
|
||||
E = M;
|
||||
|
||||
// --- Iteratively compute eccentric anomaly ----------------------------
|
||||
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);
|
||||
|
||||
// Compute the true anomaly
|
||||
double tmp_Y=sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E);
|
||||
double tmp_X=cos(E)-d_e_eccentricity;
|
||||
double tmp_Y = sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E);
|
||||
double tmp_X = cos(E) - d_e_eccentricity;
|
||||
nu = atan2(tmp_Y, tmp_X);
|
||||
|
||||
// Compute angle phi (argument of Latitude)
|
||||
@ -362,7 +366,7 @@ void gps_navigation_message::satellitePosition(double transmitTime)
|
||||
phi = fmod((phi),(2*GPS_PI));
|
||||
|
||||
// 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
|
||||
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_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);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -422,9 +421,9 @@ void gps_navigation_message::satellitePosition(double transmitTime)
|
||||
|
||||
int gps_navigation_message::subframe_decoder(char *subframe)
|
||||
{
|
||||
int subframe_ID=0;
|
||||
int SV_data_ID=0;
|
||||
int SV_page=0;
|
||||
int subframe_ID = 0;
|
||||
int SV_data_ID = 0;
|
||||
int SV_page = 0;
|
||||
//double tmp_TOW;
|
||||
|
||||
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
|
||||
std::bitset<GPS_SUBFRAME_BITS> subframe_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);
|
||||
word_bits=std::bitset<(GPS_WORD_BITS+2)>(gps_word);
|
||||
for (int j=0;j<GPS_WORD_BITS;j++)
|
||||
memcpy(&gps_word,&subframe[i*4], sizeof(char)*4);
|
||||
word_bits = std::bitset<(GPS_WORD_BITS+2)>(gps_word);
|
||||
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);
|
||||
}
|
||||
*/
|
||||
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;
|
||||
|
||||
// Decode all 5 sub-frames
|
||||
switch (subframe_ID){
|
||||
switch (subframe_ID)
|
||||
{
|
||||
//--- Decode the sub-frame id ------------------------------------------
|
||||
// ICD (IS-GPS-200E Appendix II). http://www.losangeles.af.mil/shared/media/document/AFD-100813-045.pdf
|
||||
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
|
||||
// (the variable subframe at this point contains bits of the last subframe).
|
||||
//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)) !
|
||||
|
||||
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);
|
||||
|
||||
// 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_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_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_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); //
|
||||
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));
|
||||
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));
|
||||
d_TGD = (double)read_navigation_signed(subframe_bits, T_GD, num_of_slices(T_GD));
|
||||
d_TGD = d_TGD*T_GD_LSB;
|
||||
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_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 = 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_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_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;
|
||||
|
||||
|
||||
@ -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_alert_flag = read_navigation_bool(subframe_bits, ALERT_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_Crs = (double)read_navigation_signed(subframe_bits,C_RS,num_of_slices(C_RS));
|
||||
d_Crs =d_Crs * C_RS_LSB;
|
||||
d_Delta_n = (double)read_navigation_signed(subframe_bits,DELTA_N,num_of_slices(DELTA_N));
|
||||
d_IODE_SF2 = (double)read_navigation_unsigned(subframe_bits, IODE_SF2, num_of_slices(IODE_SF2));
|
||||
d_Crs = (double)read_navigation_signed(subframe_bits, C_RS, num_of_slices(C_RS));
|
||||
d_Crs = d_Crs * C_RS_LSB;
|
||||
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_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_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_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_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_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_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;
|
||||
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;
|
||||
|
||||
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_alert_flag = read_navigation_bool(subframe_bits, ALERT_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_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_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_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_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_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_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_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_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 = d_IDOT*I_DOT_LSB;
|
||||
|
||||
break;
|
||||
@ -587,8 +587,8 @@ int gps_navigation_message::subframe_decoder(char *subframe)
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_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_page = (int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE));
|
||||
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));
|
||||
|
||||
if (SV_page == 13)
|
||||
{
|
||||
@ -598,51 +598,47 @@ int gps_navigation_message::subframe_decoder(char *subframe)
|
||||
if (SV_page == 18)
|
||||
{
|
||||
// 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_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_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_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_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_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_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_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_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_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_t_OT = (double)read_navigation_unsigned(subframe_bits, T_OT,num_of_slices(T_OT));
|
||||
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));
|
||||
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_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));
|
||||
|
||||
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));
|
||||
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 ?
|
||||
d_DeltaT_LSF = (double)read_navigation_signed(subframe_bits, DELTAT_LSF, num_of_slices(DELTAT_LSF));
|
||||
}
|
||||
|
||||
if (SV_page == 25)
|
||||
{
|
||||
// Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32)
|
||||
//! \TODO Read Anti-Spoofing, SV config
|
||||
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[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[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[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[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[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[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[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));
|
||||
}
|
||||
|
||||
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_alert_flag = read_navigation_bool(subframe_bits, ALERT_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_page = (int)read_navigation_unsigned(subframe_bits,SV_PAGE,num_of_slices(SV_PAGE));
|
||||
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));
|
||||
|
||||
if (SV_page < 25)
|
||||
{
|
||||
@ -664,30 +660,30 @@ int gps_navigation_message::subframe_decoder(char *subframe)
|
||||
d_Toa = d_Toa * T_OA_LSB;
|
||||
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[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[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[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[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[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[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[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[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[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[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[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[24] = (int)read_navigation_unsigned(subframe_bits,HEALTH_SV24,num_of_slices(HEALTH_SV24));
|
||||
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[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[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[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[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[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[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[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[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[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[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[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));
|
||||
|
||||
}
|
||||
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));
|
||||
|
||||
// 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
|
||||
{
|
||||
@ -715,13 +711,13 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
|
||||
|
||||
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
|
||||
{
|
||||
|
||||
|
||||
if (abs(gpstime_corrected-secondOfLeapSecondEvent) > 21600)
|
||||
if (abs(gpstime_corrected - secondOfLeapSecondEvent) > 21600)
|
||||
{
|
||||
/* 20.3.3.5.2.4a
|
||||
* Whenever the effectivity time indicated by the WN_LSF and the DN values
|
||||
@ -731,7 +727,7 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
|
||||
* 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
|
||||
{
|
||||
@ -742,15 +738,15 @@ double gps_navigation_message::utc_time(double gpstime_corrected)
|
||||
* transition is provided by the following expression for UTC:
|
||||
*/
|
||||
|
||||
int W = fmod(gpstime_corrected-Delta_t_UTC-43200,86400)+43200;
|
||||
t_utc_daytime =fmod(W,86400+d_DeltaT_LSF-d_DeltaT_LS);
|
||||
int W = fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400) + 43200;
|
||||
t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS);
|
||||
|
||||
//implement something to handle a leap second event!
|
||||
}
|
||||
if ( (gpstime_corrected - secondOfLeapSecondEvent ) > 21600)
|
||||
if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
|
||||
{
|
||||
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
|
||||
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -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
|
||||
* in 20.3.3.5.2.4b,*/
|
||||
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 *(double)(i_GPS_week - i_WN_T));
|
||||
t_utc_daytime=fmod(gpstime_corrected-Delta_t_UTC,86400);
|
||||
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400);
|
||||
}
|
||||
|
||||
double secondsOfWeekBeforeToday= 43200*floor(gpstime_corrected/43200);
|
||||
t_utc = secondsOfWeekBeforeToday+t_utc_daytime;
|
||||
double secondsOfWeekBeforeToday= 43200 * floor(gpstime_corrected / 43200);
|
||||
t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
|
||||
return t_utc;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -785,7 +780,7 @@ bool gps_navigation_message::satellite_validation()
|
||||
// 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)
|
||||
{
|
||||
flag_data_valid=true;
|
||||
flag_data_valid = true;
|
||||
}
|
||||
return flag_data_valid;
|
||||
}
|
||||
|
||||
@ -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]
|
||||
|
||||
// Satellite velocity
|
||||
double d_satvel_X;
|
||||
double d_satvel_Y;
|
||||
double d_satvel_Z;
|
||||
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m]
|
||||
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m]
|
||||
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m]
|
||||
|
||||
// public functions
|
||||
void reset();
|
||||
|
||||
Loading…
Reference in New Issue
Block a user