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

Merge with next

Browse Source
This commit is contained in:
Anthony Arnold
2014-09-18 02:10:56 +10:00
parent 2478a43b0a df2d8a88aa
commit 77b635e8c0
67 changed files with 1950 additions and 2390 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
CMakeLists.txt
View File

@@ -44,13 +44,32 @@ option(ENABLE_GPERFTOOLS "Enable linking to Gperftools libraries (tcmalloc and p
option(ENABLE_GENERIC_ARCH "Builds a portable binary" OFF) option(ENABLE_GENERIC_ARCH "Builds a portable binary" OFF)
# Set the version information here ###############################
# GNSS-SDR version information
###############################
# Get the current working branch
execute_process(
COMMAND git rev-parse --abbrev-ref HEAD
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
OUTPUT_VARIABLE GIT_BRANCH
OUTPUT_STRIP_TRAILING_WHITESPACE
)
# Get the latest abbreviated commit hash of the working branch
execute_process(
COMMAND git log -1 --format=%h
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
OUTPUT_VARIABLE GIT_COMMIT_HASH
OUTPUT_STRIP_TRAILING_WHITESPACE
)
set(VERSION_INFO_MAJOR_VERSION 0) set(VERSION_INFO_MAJOR_VERSION 0)
set(VERSION_INFO_API_COMPAT 0) set(VERSION_INFO_API_COMPAT 0)
set(VERSION_INFO_MINOR_VERSION 4) set(VERSION_INFO_MINOR_VERSION 4.git-${GIT_BRANCH}-${GIT_COMMIT_HASH})
set(VERSION ${VERSION_INFO_MAJOR_VERSION}.${VERSION_INFO_API_COMPAT}.${VERSION_INFO_MINOR_VERSION}) set(VERSION ${VERSION_INFO_MAJOR_VERSION}.${VERSION_INFO_API_COMPAT}.${VERSION_INFO_MINOR_VERSION})
######################################################################## ########################################################################
# Environment setup # Environment setup
######################################################################## ########################################################################
@@ -73,7 +92,6 @@ if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
else(ARCH_64BITS) else(ARCH_64BITS)
set(ARCH_ "(32 bits)") set(ARCH_ "(32 bits)")
endif(ARCH_64BITS) endif(ARCH_64BITS)
if(EXISTS "/etc/lsb-release") if(EXISTS "/etc/lsb-release")
execute_process(COMMAND cat /etc/lsb-release execute_process(COMMAND cat /etc/lsb-release
COMMAND grep DISTRIB_ID COMMAND grep DISTRIB_ID

64
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
View File

@@ -347,7 +347,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
d_position_UTC_time = p_time; d_position_UTC_time = p_time;
LOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos; LOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos;
cart2geo((double)mypos(0), (double)mypos(1), (double)mypos(2), 4); cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
//ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km)
if (d_height_m > 50000) if (d_height_m > 50000)
{ {
@@ -366,17 +366,17 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
// 1- Rotation matrix from ECEF coordinates to ENU coordinates // 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F = arma::zeros(3,3); arma::mat F = arma::zeros(3,3);
F(0,0) = -sin(GPS_TWO_PI*(d_longitude_d/360.0)); F(0,0) = -sin(GPS_TWO_PI * (d_longitude_d/360.0));
F(0,1) = -sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0)); F(0,1) = -sin(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0));
F(0,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0)); F(0,2) = cos(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0));
F(1,0) = cos((GPS_TWO_PI*d_longitude_d)/360.0); F(1,0) = cos((GPS_TWO_PI * d_longitude_d)/360.0);
F(1,1) = -sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0); F(1,1) = -sin((GPS_TWO_PI * d_latitude_d)/360.0) * sin((GPS_TWO_PI * d_longitude_d)/360.0);
F(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0); F(1,2) = cos((GPS_TWO_PI * d_latitude_d/360.0)) * sin((GPS_TWO_PI * d_longitude_d)/360.0);
F(2,0) = 0; F(2,0) = 0;
F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0); F(2,1) = cos((GPS_TWO_PI * d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0)); F(2,2) = sin((GPS_TWO_PI * d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS) // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2); arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
@@ -384,7 +384,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
try try
{ {
DOP_ENU = arma::htrans(F)*Q_ECEF*F; DOP_ENU = arma::htrans(F) * Q_ECEF * F;
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
@@ -461,9 +461,9 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
} }
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth; d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth; d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth; d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
b_valid_position = true; b_valid_position = true;
return true; //indicates that the returned position is valid return true; //indicates that the returned position is valid
} }
@@ -514,9 +514,9 @@ void galileo_e1_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_sele
const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563}; const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda = atan2(Y, X); double lambda = atan2(Y, X);
double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 - f[elipsoid_selection])); double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection]) * (1 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1+ex2); double c = a[elipsoid_selection] * sqrt(1+ex2);
double phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection]))); double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1; double h = 0.1;
double oldh = 0; double oldh = 0;
@@ -526,8 +526,8 @@ void galileo_e1_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_sele
{ {
oldh = h; oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X*X + Y*Y) * (1 - (2 - f[elipsoid_selection]) * f[elipsoid_selection] *N / (N + h) )))); phi = atan(Z / ((sqrt(X * X + Y * Y) * (1 - (2 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) ))));
h = sqrt(X*X + Y*Y) / cos(phi) - N; h = sqrt(X * X + Y * Y) / cos(phi) - N;
iterations = iterations + 1; iterations = iterations + 1;
if (iterations > 100) if (iterations > 100)
{ {
@@ -582,7 +582,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
} }
// first guess // first guess
double P = sqrt(X*X + Y*Y); // P is distance from spin axis double P = sqrt(X * X + Y * Y); // P is distance from spin axis
//direct calculation of longitude //direct calculation of longitude
if (P > 1.0E-20) if (P > 1.0E-20)
@@ -600,7 +600,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
*dlambda = *dlambda + 360.0; *dlambda = *dlambda + 360.0;
} }
double r = sqrt(P*P + Z*Z); // r is distance from origin (0,0,0) double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
double sinphi; double sinphi;
if (r > 1.0E-20) if (r > 1.0E-20)
@@ -621,7 +621,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
return; return;
} }
*h = r - a*(1 - sinphi*sinphi/finv); *h = r - a * (1 - sinphi * sinphi/finv);
// iterate // iterate
double cosphi; double cosphi;
@@ -636,18 +636,18 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
cosphi = cos(*dphi); cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction // compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq*sinphi*sinphi); N_phi = a / sqrt(1 - esq * sinphi * sinphi);
// compute residuals in P and Z // compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi; dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi; dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
// update height and latitude // update height and latitude
*h = *h + (sinphi*dZ + cosphi*dP); *h = *h + (sinphi * dZ + cosphi * dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h)); *dphi = *dphi + (cosphi * dZ - sinphi * dP)/(N_phi + (*h));
// test for convergence // test for convergence
if ((dP*dP + dZ*dZ) < tolsq) if ((dP * dP + dZ * dZ) < tolsq)
{ {
break; break;
} }
@@ -693,12 +693,12 @@ void galileo_e1_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
arma::mat F = arma::zeros(3,3); arma::mat F = arma::zeros(3,3);
F(0,0) = -sl; F(0,0) = -sl;
F(0,1) = -sb*cl; F(0,1) = -sb * cl;
F(0,2) = cb*cl; F(0,2) = cb * cl;
F(1,0) = cl; F(1,0) = cl;
F(1,1) = -sb*sl; F(1,1) = -sb * sl;
F(1,2) = cb*sl; F(1,2) = cb * sl;
F(2,0) = 0; F(2,0) = 0;
F(2,1) = cb; F(2,1) = cb;
@@ -713,7 +713,7 @@ void galileo_e1_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
double U = local_vector(2); double U = local_vector(2);
double hor_dis; double hor_dis;
hor_dis = sqrt(E*E + N*N); hor_dis = sqrt(E * E + N * N);
if (hor_dis < 1.0E-20) if (hor_dis < 1.0E-20)
{ {
@@ -731,7 +731,7 @@ void galileo_e1_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
*Az = *Az + 360.0; *Az = *Az + 360.0;
} }
*D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2)); *D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
} }
void galileo_e1_ls_pvt::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km) void galileo_e1_ls_pvt::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km)
@@ -764,7 +764,7 @@ void galileo_e1_ls_pvt::tropo(double *ddr_m, double sinel, double hsta_km, doubl
const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5); const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5);
double tkhum = t_kel + tlapse * (hhum_km - htkel_km); double tkhum = t_kel + tlapse * (hhum_km - htkel_km);
double atkel = 7.5*(tkhum - 273.15) / (237.3 + tkhum - 273.15); double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15);
double e0 = 0.0611 * hum * pow(10, atkel); double e0 = 0.0611 * hum * pow(10, atkel);
double tksea = t_kel - tlapse * htkel_km; double tksea = t_kel - tlapse * htkel_km;
double tkelh = tksea + tlapse * hhum_km; double tkelh = tksea + tlapse * hhum_km;

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

@@ -332,7 +332,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
} }
// Compute UTC time and print PVT solution // Compute UTC time and print PVT solution
double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60) double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek*(double)GPS_week); boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast<double>(GPS_week));
// 22 August 1999 last GPS time roll over // 22 August 1999 last GPS time roll over
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time; d_position_UTC_time = p_time;
@@ -441,9 +441,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
} }
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth; d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth; d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth; d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
b_valid_position = true; b_valid_position = true;
return true; //indicates that the returned position is valid return true; //indicates that the returned position is valid
} }

8
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@@ -416,7 +416,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
d_position_UTC_time = p_time; d_position_UTC_time = p_time;
LOG(INFO) << "HYBRID Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z) = " << mypos; LOG(INFO) << "HYBRID Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z) = " << mypos;
cart2geo((double)mypos(0), (double)mypos(1), (double)mypos(2), 4); cart2geo(static_cast<double>(mypos(0)), static_cast<double>(mypos(1)), static_cast<double>(mypos(2)), 4);
//ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km)
if (d_height_m > 50000) if (d_height_m > 50000)
{ {
@@ -535,9 +535,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i);
} }
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth; d_avg_latitude_d = d_avg_latitude_d / static_cast<double>(d_averaging_depth);
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth; d_avg_longitude_d = d_avg_longitude_d / static_cast<double>(d_averaging_depth);
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth; d_avg_height_m = d_avg_height_m / static_cast<double>(d_averaging_depth);
b_valid_position = true; b_valid_position = true;
return true; //indicates that the returned position is valid return true; //indicates that the returned position is valid
} }

28
src/algorithms/PVT/libs/nmea_printer.cc
View File

@@ -222,8 +222,8 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
north = true; north = true;
} }
int deg = (int)lat; int deg = static_cast<int>(lat);
double mins = lat - (double)deg; double mins = lat - static_cast<double>(deg);
mins *= 60.0 ; mins *= 60.0 ;
std::ostringstream out_string; std::ostringstream out_string;
out_string.setf(std::ios::fixed, std::ios::floatfield); out_string.setf(std::ios::fixed, std::ios::floatfield);
@@ -259,8 +259,8 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
{ {
east = true; east = true;
} }
int deg = (int)longitude; int deg = static_cast<int>(longitude);
double mins = longitude - (double)deg; double mins = longitude - static_cast<double>(deg);
mins *= 60.0 ; mins *= 60.0 ;
std::ostringstream out_string; std::ostringstream out_string;
out_string.setf(std::ios::fixed, std::ios::floatfield); out_string.setf(std::ios::fixed, std::ios::floatfield);
@@ -271,7 +271,7 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
out_string.precision(4); out_string.precision(4);
out_string << mins; out_string << mins;
if (east==true) if (east == true)
{ {
out_string << ",E"; out_string << ",E";
} }
@@ -361,7 +361,7 @@ std::string Nmea_Printer::get_GPRMC()
sentence_str << ",V"; sentence_str << ",V";
}; };
if (d_PVT_data->d_flag_averaging==true) if (d_PVT_data->d_flag_averaging == true)
{ {
// Latitude ddmm.mmmm,(N or S) // Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->d_avg_latitude_d); sentence_str << "," << latitude_to_hm(d_PVT_data->d_avg_latitude_d);
@@ -422,7 +422,7 @@ std::string Nmea_Printer::get_GPRMC()
sentence_str << "*"; sentence_str << "*";
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str << std::hex << (int)checksum; sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence // end NMEA sentence
sentence_str << "\r\n"; sentence_str << "\r\n";
@@ -507,7 +507,7 @@ std::string Nmea_Printer::get_GPGSA()
sentence_str << "*"; sentence_str << "*";
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str << std::hex << (int)checksum; sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence // end NMEA sentence
sentence_str << "\r\n"; sentence_str << "\r\n";
@@ -532,7 +532,7 @@ std::string Nmea_Printer::get_GPGSV()
// 1st step: How many GPGSV frames we need? (up to 3) // 1st step: How many GPGSV frames we need? (up to 3)
// Each frame contains up to 4 satellites // Each frame contains up to 4 satellites
int n_frames; int n_frames;
n_frames = std::ceil(((double)n_sats_used) / 4.0); n_frames = std::ceil((static_cast<double>(n_sats_used)) / 4.0);
// generate the frames // generate the frames
int current_satellite = 0; int current_satellite = 0;
@@ -566,17 +566,17 @@ std::string Nmea_Printer::get_GPGSV()
frame_str << ","; frame_str << ",";
frame_str.width(2); frame_str.width(2);
frame_str.fill('0'); frame_str.fill('0');
frame_str << std::dec << (int)d_PVT_data->d_visible_satellites_El[current_satellite]; frame_str << std::dec << static_cast<int>(d_PVT_data->d_visible_satellites_El[current_satellite]);
frame_str << ","; frame_str << ",";
frame_str.width(3); frame_str.width(3);
frame_str.fill('0'); frame_str.fill('0');
frame_str << std::dec << (int)d_PVT_data->d_visible_satellites_Az[current_satellite]; frame_str << std::dec << static_cast<int>(d_PVT_data->d_visible_satellites_Az[current_satellite]);
frame_str << ","; frame_str << ",";
frame_str.width(2); frame_str.width(2);
frame_str.fill('0'); frame_str.fill('0');
frame_str << std::dec << (int)d_PVT_data->d_visible_satellites_CN0_dB[current_satellite]; frame_str << std::dec << static_cast<int>(d_PVT_data->d_visible_satellites_CN0_dB[current_satellite]);
current_satellite++; current_satellite++;
@@ -592,7 +592,7 @@ std::string Nmea_Printer::get_GPGSV()
frame_str << "*"; frame_str << "*";
frame_str.width(2); frame_str.width(2);
frame_str.fill('0'); frame_str.fill('0');
frame_str << std::hex << (int)checksum; frame_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence // end NMEA sentence
frame_str << "\r\n"; frame_str << "\r\n";
@@ -712,7 +712,7 @@ std::string Nmea_Printer::get_GPGGA()
sentence_str << "*"; sentence_str << "*";
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str << std::hex <<(int)checksum; sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence // end NMEA sentence
sentence_str << "\r\n"; sentence_str << "\r\n";

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

@@ -143,7 +143,7 @@ Rinex_Printer::Rinex_Printer()
if ( FLAGS_RINEX_version.compare("3.01") == 0 ) if ( FLAGS_RINEX_version.compare("3.01") == 0 )
{ {
version = 3; version = 3;
stringVersion = "3.02"; stringVersion = "3.01";
} }
else if ( FLAGS_RINEX_version.compare("3.02") == 0 ) else if ( FLAGS_RINEX_version.compare("3.02") == 0 )
{ {
@@ -163,7 +163,7 @@ Rinex_Printer::Rinex_Printer()
else if ( FLAGS_RINEX_version.compare("2.10") == 0 ) else if ( FLAGS_RINEX_version.compare("2.10") == 0 )
{ {
version = 2; version = 2;
stringVersion = "2.11"; stringVersion = "2.10";
} }
else if ( FLAGS_RINEX_version.compare("2") == 0 ) else if ( FLAGS_RINEX_version.compare("2") == 0 )
{ {
@@ -219,7 +219,7 @@ Rinex_Printer::~Rinex_Printer()
void Rinex_Printer::lengthCheck(std::string line) void Rinex_Printer::lengthCheck(const std::string& line)
{ {
if (line.length() != 80) if (line.length() != 80)
{ {
@@ -391,7 +391,7 @@ std::string Rinex_Printer::getLocalTime()
} }
void Rinex_Printer::rinex_nav_header(std::ofstream& out, Galileo_Iono iono, Galileo_Utc_Model utc_model, Galileo_Almanac galileo_almanac) void Rinex_Printer::rinex_nav_header(std::ofstream& out, const Galileo_Iono& iono, const Galileo_Utc_Model& utc_model, const Galileo_Almanac& galileo_almanac)
{ {
std::string line; std::string line;
stringVersion = "3.02"; stringVersion = "3.02";
@@ -501,7 +501,7 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Galileo_Iono iono, Gal
out << line << std::endl; out << line << std::endl;
} }
void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Iono iono, Gps_Utc_Model utc_model) void Rinex_Printer::rinex_nav_header(std::ofstream& out, const Gps_Iono& iono, const Gps_Utc_Model& utc_model)
{ {
std::string line; std::string line;
@@ -680,7 +680,7 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Iono iono, Gps_Utc_
out << line << std::endl; out << line << std::endl;
} }
void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Iono gps_iono, Gps_Utc_Model gps_utc_model, Galileo_Iono galileo_iono, Galileo_Utc_Model galileo_utc_model, Galileo_Almanac galileo_almanac) void Rinex_Printer::rinex_nav_header(std::ofstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac)
{ {
std::string line; std::string line;
stringVersion = "3.02"; stringVersion = "3.02";
@@ -922,12 +922,10 @@ void Rinex_Printer::rinex_sbs_header(std::ofstream& out)
} }
void Rinex_Printer::log_rinex_nav(std::ofstream& out, const std::map<int,Gps_Ephemeris>& eph_map)
void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris> eph_map)
{ {
std::string line; std::string line;
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter; std::map<int,Gps_Ephemeris>::const_iterator gps_ephemeris_iter;
for(gps_ephemeris_iter = eph_map.begin(); for(gps_ephemeris_iter = eph_map.begin();
gps_ephemeris_iter != eph_map.end(); gps_ephemeris_iter != eph_map.end();
@@ -1129,12 +1127,12 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris
} }
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_IDOT, 18, 2); line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_IDOT, 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_code_on_L2), 18, 2); line += Rinex_Printer::doub2for(static_cast<double>(gps_ephemeris_iter->second.i_code_on_L2), 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
double GPS_week_continuous_number = (double)(gps_ephemeris_iter->second.i_GPS_week + 1024); // valid until April 7, 2019 (check http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm) double GPS_week_continuous_number = static_cast<double>(gps_ephemeris_iter->second.i_GPS_week + 1024); // valid until April 7, 2019 (check http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm)
line += Rinex_Printer::doub2for(GPS_week_continuous_number, 18, 2); line += Rinex_Printer::doub2for(GPS_week_continuous_number, 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_code_on_L2), 18, 2); line += Rinex_Printer::doub2for(static_cast<double>(gps_ephemeris_iter->second.i_code_on_L2), 18, 2);
if (version == 2) if (version == 2)
{ {
line += std::string(1, ' '); line += std::string(1, ' ');
@@ -1153,9 +1151,9 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris
{ {
line += std::string(5, ' '); line += std::string(5, ' ');
} }
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_SV_accuracy), 18, 2); line += Rinex_Printer::doub2for(static_cast<double>(gps_ephemeris_iter->second.i_SV_accuracy), 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
line += Rinex_Printer::doub2for((double)(gps_ephemeris_iter->second.i_SV_health), 18, 2); line += Rinex_Printer::doub2for(static_cast<double>(gps_ephemeris_iter->second.i_SV_health), 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_TGD, 18, 2); line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_TGD, 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
@@ -1182,7 +1180,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris
line += std::string(1, ' '); line += std::string(1, ' ');
double curve_fit_interval = 4; double curve_fit_interval = 4;
if (gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIA")) if (gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN).compare("IIA"))
{ {
// Block II/IIA (Table 20-XI IS-GPS-200E ) // Block II/IIA (Table 20-XI IS-GPS-200E )
if ( (gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248) ) curve_fit_interval = 8; if ( (gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248) ) curve_fit_interval = 8;
@@ -1193,10 +1191,10 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris
if ( gps_ephemeris_iter->second.d_IODC == 757 ) curve_fit_interval = 98; if ( gps_ephemeris_iter->second.d_IODC == 757 ) curve_fit_interval = 98;
} }
if ((gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIR") == 0) || if ((gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN).compare("IIR") == 0) ||
(gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIR-M") == 0) || (gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN).compare("IIR-M") == 0) ||
(gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIF") == 0) || (gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN).compare("IIF") == 0) ||
(gps_ephemeris_iter->second.satelliteBlock[gps_ephemeris_iter->second.i_satellite_PRN].compare("IIIA") == 0) ) (gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN).compare("IIIA") == 0) )
{ {
// Block IIR/IIR-M/IIF/IIIA (Table 20-XII IS-GPS-200E ) // Block IIR/IIR-M/IIF/IIIA (Table 20-XII IS-GPS-200E )
if ( (gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248)) curve_fit_interval = 8; if ( (gps_ephemeris_iter->second.d_IODC > 239) && (gps_ephemeris_iter->second.d_IODC < 248)) curve_fit_interval = 8;
@@ -1219,11 +1217,10 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris
} }
void Rinex_Printer::log_rinex_nav(std::ofstream& out, const std::map<int, Galileo_Ephemeris>& eph_map)
void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int, Galileo_Ephemeris> eph_map)
{ {
std::string line; std::string line;
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter; std::map<int,Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
line.clear(); line.clear();
for(galileo_ephemeris_iter = eph_map.begin(); for(galileo_ephemeris_iter = eph_map.begin();
galileo_ephemeris_iter != eph_map.end(); galileo_ephemeris_iter != eph_map.end();
@@ -1331,7 +1328,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int, Galileo_Ephe
int data_source_INAV = Rinex_Printer::toInt(iNAVE1B, 10); int data_source_INAV = Rinex_Printer::toInt(iNAVE1B, 10);
line += Rinex_Printer::doub2for(static_cast<double>(data_source_INAV), 18, 2); line += Rinex_Printer::doub2for(static_cast<double>(data_source_INAV), 18, 2);
line += std::string(1, ' '); line += std::string(1, ' ');
double GST_week = (double)(galileo_ephemeris_iter->second.WN_5); double GST_week = static_cast<double>(galileo_ephemeris_iter->second.WN_5);
double num_GST_rollovers = floor((GST_week + 1024.0) / 4096.0 ); double num_GST_rollovers = floor((GST_week + 1024.0) / 4096.0 );
double Galileo_week_continuous_number = GST_week + 1024.0 + num_GST_rollovers * 4096.0; double Galileo_week_continuous_number = GST_week + 1024.0 + num_GST_rollovers * 4096.0;
line += Rinex_Printer::doub2for(Galileo_week_continuous_number, 18, 2); line += Rinex_Printer::doub2for(Galileo_week_continuous_number, 18, 2);
@@ -1399,7 +1396,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int, Galileo_Ephe
} }
void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int, Gps_Ephemeris> gps_eph_map, std::map<int, Galileo_Ephemeris> galileo_eph_map) void Rinex_Printer::log_rinex_nav(std::ofstream& out, const std::map<int, Gps_Ephemeris>& gps_eph_map, const std::map<int, Galileo_Ephemeris>& galileo_eph_map)
{ {
version = 3; version = 3;
stringVersion = "3.02"; stringVersion = "3.02";
@@ -1408,7 +1405,7 @@ void Rinex_Printer::log_rinex_nav(std::ofstream& out, std::map<int, Gps_Ephemeri
} }
void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, double d_TOW_first_observation) void Rinex_Printer::rinex_obs_header(std::ofstream& out, const Gps_Ephemeris& eph, const double d_TOW_first_observation)
{ {
std::string line; std::string line;
@@ -1639,7 +1636,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, doub
// -------- TIME OF FIRST OBS // -------- TIME OF FIRST OBS
line.clear(); line.clear();
boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph,d_TOW_first_observation); boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph,d_TOW_first_observation);
std::string timestring=boost::posix_time::to_iso_string(p_gps_time); std::string timestring = boost::posix_time::to_iso_string(p_gps_time);
std::string year (timestring, 0, 4); std::string year (timestring, 0, 4);
std::string month (timestring, 4, 2); std::string month (timestring, 4, 2);
std::string day (timestring, 6, 2); std::string day (timestring, 6, 2);
@@ -1671,7 +1668,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, doub
void Rinex_Printer::rinex_obs_header(std::ofstream& out, Galileo_Ephemeris eph, double d_TOW_first_observation) void Rinex_Printer::rinex_obs_header(std::ofstream& out, const Galileo_Ephemeris& eph, const double d_TOW_first_observation)
{ {
std::string line; std::string line;
version = 3; version = 3;
@@ -1873,7 +1870,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Galileo_Ephemeris eph,
} }
void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double d_TOW_first_observation) void Rinex_Printer::rinex_obs_header(std::ofstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation)
{ {
std::string line; std::string line;
version = 3; version = 3;
@@ -2101,7 +2098,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris gps_eph,
out << line << std::endl; out << line << std::endl;
} }
void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double obs_time, std::map<int,Gnss_Synchro> pseudoranges) void Rinex_Printer::log_rinex_obs(std::ofstream& out, const Gps_Ephemeris& eph, const double obs_time, const std::map<int,Gnss_Synchro>& pseudoranges)
{ {
// RINEX observations timestamps are GPS timestamps. // RINEX observations timestamps are GPS timestamps.
std::string line; std::string line;
@@ -2154,7 +2151,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
line += std::string(1, '0'); line += std::string(1, '0');
//Number of satellites observed in current epoch //Number of satellites observed in current epoch
int numSatellitesObserved = 0; int numSatellitesObserved = 0;
std::map<int,Gnss_Synchro>::iterator pseudoranges_iter; std::map<int, Gnss_Synchro>::const_iterator pseudoranges_iter;
for(pseudoranges_iter = pseudoranges.begin(); for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end(); pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++) pseudoranges_iter++)
@@ -2167,8 +2164,8 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
pseudoranges_iter++) pseudoranges_iter++)
{ {
line += satelliteSystem["GPS"]; line += satelliteSystem["GPS"];
if ((int)pseudoranges_iter->first < 10) line += std::string(1, '0'); if (static_cast<int>(pseudoranges_iter->first) < 10) line += std::string(1, '0');
line += boost::lexical_cast<std::string>((int)pseudoranges_iter->first); line += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
} }
// Receiver clock offset (optional) // Receiver clock offset (optional)
//line += rightJustify(asString(clockOffset, 12), 15); //line += rightJustify(asString(clockOffset, 12), 15);
@@ -2239,7 +2236,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
//Number of satellites observed in current epoch //Number of satellites observed in current epoch
int numSatellitesObserved = 0; int numSatellitesObserved = 0;
std::map<int,Gnss_Synchro>::iterator pseudoranges_iter; std::map<int, Gnss_Synchro>::const_iterator pseudoranges_iter;
for(pseudoranges_iter = pseudoranges.begin(); for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end(); pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++) pseudoranges_iter++)
@@ -2263,8 +2260,8 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
std::string lineObs; std::string lineObs;
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["GPS"]; lineObs += satelliteSystem["GPS"];
if ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0'); if (static_cast<int>(pseudoranges_iter->first) < 10) lineObs += std::string(1, '0');
lineObs += boost::lexical_cast<std::string>((int)pseudoranges_iter->first); lineObs += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
//lineObs += std::string(2, ' '); //lineObs += std::string(2, ' ');
lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14);
@@ -2296,7 +2293,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
void Rinex_Printer::log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, double obs_time, std::map<int,Gnss_Synchro> pseudoranges) void Rinex_Printer::log_rinex_obs(std::ofstream& out, const Galileo_Ephemeris& eph, double obs_time, const std::map<int,Gnss_Synchro>& pseudoranges)
{ {
// RINEX observations timestamps are Galileo timestamps. // RINEX observations timestamps are Galileo timestamps.
// See http://gage14.upc.es/gLAB/HTML/Observation_Rinex_v3.01.html // See http://gage14.upc.es/gLAB/HTML/Observation_Rinex_v3.01.html
@@ -2341,7 +2338,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, dou
//Number of satellites observed in current epoch //Number of satellites observed in current epoch
int numSatellitesObserved = 0; int numSatellitesObserved = 0;
std::map<int,Gnss_Synchro>::iterator pseudoranges_iter; std::map<int, Gnss_Synchro>::const_iterator pseudoranges_iter;
for(pseudoranges_iter = pseudoranges.begin(); for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end(); pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++) pseudoranges_iter++)
@@ -2364,8 +2361,8 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, dou
std::string lineObs; std::string lineObs;
lineObs.clear(); lineObs.clear();
lineObs += satelliteSystem["Galileo"]; lineObs += satelliteSystem["Galileo"];
if ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0'); if (static_cast<int>(pseudoranges_iter->first) < 10) lineObs += std::string(1, '0');
lineObs += boost::lexical_cast<std::string>((int)pseudoranges_iter->first); lineObs += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
//lineObs += std::string(2, ' '); //lineObs += std::string(2, ' ');
lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14);
@@ -2391,7 +2388,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, dou
} }
void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double gps_obs_time, std::map<int,Gnss_Synchro> pseudoranges) void Rinex_Printer::log_rinex_obs(std::ofstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, double gps_obs_time, const std::map<int,Gnss_Synchro>& pseudoranges)
{ {
std::string line; std::string line;
@@ -2433,7 +2430,7 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris gps_eph, Gal
//Number of satellites observed in current epoch //Number of satellites observed in current epoch
int numSatellitesObserved = 0; int numSatellitesObserved = 0;
std::map<int,Gnss_Synchro>::iterator pseudoranges_iter; std::map<int,Gnss_Synchro>::const_iterator pseudoranges_iter;
for(pseudoranges_iter = pseudoranges.begin(); for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end(); pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++) pseudoranges_iter++)
@@ -2460,8 +2457,8 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris gps_eph, Gal
s.assign(1, pseudoranges_iter->second.System); s.assign(1, pseudoranges_iter->second.System);
if(s.compare("G") == 0) lineObs += satelliteSystem["GPS"]; if(s.compare("G") == 0) lineObs += satelliteSystem["GPS"];
if(s.compare("E") == 0) lineObs += satelliteSystem["Galileo"]; if(s.compare("E") == 0) lineObs += satelliteSystem["Galileo"];
if ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0'); if (static_cast<int>(pseudoranges_iter->first) < 10) lineObs += std::string(1, '0');
lineObs += boost::lexical_cast<std::string>((int)pseudoranges_iter->first); lineObs += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14); lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14);
//Loss of lock indicator (LLI) //Loss of lock indicator (LLI)
@@ -2562,7 +2559,7 @@ void Rinex_Printer::to_date_time(int gps_week, int gps_tow, int &year, int &mont
void Rinex_Printer::log_rinex_sbs(std::ofstream& out, Sbas_Raw_Msg sbs_message) void Rinex_Printer::log_rinex_sbs(std::ofstream& out, const Sbas_Raw_Msg& sbs_message)
{ {
// line 1: PRN / EPOCH / RCVR // line 1: PRN / EPOCH / RCVR
std::stringstream line1; std::stringstream line1;
@@ -2645,7 +2642,7 @@ void Rinex_Printer::log_rinex_sbs(std::ofstream& out, Sbas_Raw_Msg sbs_message)
} }
int Rinex_Printer::signalStrength(double snr) int Rinex_Printer::signalStrength(const double snr)
{ {
int ss; int ss;
ss = int ( std::min( std::max( int (floor(snr/6)) , 1), 9) ); ss = int ( std::min( std::max( int (floor(snr/6)) , 1), 9) );
@@ -2653,37 +2650,37 @@ int Rinex_Printer::signalStrength(double snr)
} }
boost::posix_time::ptime Rinex_Printer::compute_UTC_time(Gps_Navigation_Message nav_msg) boost::posix_time::ptime Rinex_Printer::compute_UTC_time(const Gps_Navigation_Message& nav_msg)
{ {
// if we are processing a file -> wait to leap second to resolve the ambiguity else take the week from the local system time // if we are processing a file -> wait to leap second to resolve the ambiguity else take the week from the local system time
//: idea resolve the ambiguity with the leap second http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm //: idea resolve the ambiguity with the leap second http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm
double utc_t = nav_msg.utc_time(nav_msg.d_TOW); const double utc_t = nav_msg.utc_time(nav_msg.d_TOW);
boost::posix_time::time_duration t = boost::posix_time::millisec((utc_t + 604800*(double)(nav_msg.i_GPS_week))*1000); boost::posix_time::time_duration t = boost::posix_time::millisec((utc_t + 604800 * static_cast<double>(nav_msg.i_GPS_week)) * 1000);
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
return p_time; return p_time;
} }
boost::posix_time::ptime Rinex_Printer::compute_GPS_time(Gps_Ephemeris eph, double obs_time) boost::posix_time::ptime Rinex_Printer::compute_GPS_time(const Gps_Ephemeris& eph, const double obs_time)
{ {
// The RINEX v2.11 v3.00 format uses GPS time for the observations epoch, not UTC time, thus, no leap seconds needed here. // The RINEX v2.11 v3.00 format uses GPS time for the observations epoch, not UTC time, thus, no leap seconds needed here.
// (see Section 3 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex211.txt) // (see Section 3 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex211.txt)
// (see Pag. 17 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex300.pdf) // (see Pag. 17 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex300.pdf)
// --??? No time correction here, since it will be done in the RINEX processor // --??? No time correction here, since it will be done in the RINEX processor
double gps_t = obs_time; const double gps_t = obs_time;
boost::posix_time::time_duration t = boost::posix_time::millisec((gps_t + 604800*(double)(eph.i_GPS_week % 1024))*1000); boost::posix_time::time_duration t = boost::posix_time::millisec((gps_t + 604800 * static_cast<double>(eph.i_GPS_week % 1024)) * 1000);
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
return p_time; return p_time;
} }
boost::posix_time::ptime Rinex_Printer::compute_Galileo_time(Galileo_Ephemeris eph, double obs_time) boost::posix_time::ptime Rinex_Printer::compute_Galileo_time(const Galileo_Ephemeris& eph, const double obs_time)
{ {
// The RINEX v2.11 v3.00 format uses Galileo time for the observations epoch, not UTC time, thus, no leap seconds needed here. // The RINEX v2.11 v3.00 format uses Galileo time for the observations epoch, not UTC time, thus, no leap seconds needed here.
// (see Pag. 17 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf) // (see Pag. 17 in http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf)
// --??? No time correction here, since it will be done in the RINEX processor // --??? No time correction here, since it will be done in the RINEX processor
double galileo_t = obs_time; double galileo_t = obs_time;
boost::posix_time::time_duration t = boost::posix_time::millisec((galileo_t + 604800*(double)(eph.WN_5))*1000); // boost::posix_time::time_duration t = boost::posix_time::millisec((galileo_t + 604800 * static_cast<double>(eph.WN_5)) * 1000); //
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
return p_time; return p_time;
} }

38
src/algorithms/PVT/libs/rinex_printer.h
View File

@@ -93,32 +93,32 @@ public:
/*! /*!
* \brief Generates the GPS Navigation Data header * \brief Generates the GPS Navigation Data header
*/ */
void rinex_nav_header(std::ofstream& out, Gps_Iono iono, Gps_Utc_Model utc_model); void rinex_nav_header(std::ofstream& out, const Gps_Iono& iono, const Gps_Utc_Model& utc_model);
/*! /*!
* \brief Generates the Galileo Navigation Data header * \brief Generates the Galileo Navigation Data header
*/ */
void rinex_nav_header(std::ofstream& out, Galileo_Iono iono, Galileo_Utc_Model utc_model, Galileo_Almanac galileo_almanac); void rinex_nav_header(std::ofstream& out, const Galileo_Iono& iono, const Galileo_Utc_Model& utc_model, const Galileo_Almanac& galileo_almanac);
/*! /*!
* \brief Generates the Mixed (GPS/Galileo) Navigation Data header * \brief Generates the Mixed (GPS/Galileo) Navigation Data header
*/ */
void rinex_nav_header(std::ofstream& out, Gps_Iono gps_iono, Gps_Utc_Model gps_utc_model, Galileo_Iono galileo_iono, Galileo_Utc_Model galileo_utc_model, Galileo_Almanac galileo_almanac); void rinex_nav_header(std::ofstream& out, const Gps_Iono& gps_iono, const Gps_Utc_Model& gps_utc_model, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac);
/*! /*!
* \brief Generates the GPS Observation data header * \brief Generates the GPS Observation data header
*/ */
void rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, double d_TOW_first_observation); void rinex_obs_header(std::ofstream& out, const Gps_Ephemeris& eph, const double d_TOW_first_observation);
/*! /*!
* \brief Generates the Galileo Observation data header * \brief Generates the Galileo Observation data header
*/ */
void rinex_obs_header(std::ofstream& out, Galileo_Ephemeris eph, double d_TOW_first_observation); void rinex_obs_header(std::ofstream& out, const Galileo_Ephemeris& eph, const double d_TOW_first_observation);
/*! /*!
* \brief Generates the Mixed (GPS/Galileo) Observation data header * \brief Generates the Mixed (GPS/Galileo) Observation data header
*/ */
void rinex_obs_header(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double d_TOW_first_observation); void rinex_obs_header(std::ofstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation);
/*! /*!
* \brief Generates the SBAS raw data header * \brief Generates the SBAS raw data header
@@ -128,47 +128,47 @@ public:
/*! /*!
* \brief Computes the UTC time and returns a boost::posix_time::ptime object * \brief Computes the UTC time and returns a boost::posix_time::ptime object
*/ */
boost::posix_time::ptime compute_UTC_time(Gps_Navigation_Message nav_msg); boost::posix_time::ptime compute_UTC_time(const Gps_Navigation_Message& nav_msg);
/*! /*!
* \brief Computes the GPS time and returns a boost::posix_time::ptime object * \brief Computes the GPS time and returns a boost::posix_time::ptime object
*/ */
boost::posix_time::ptime compute_GPS_time(Gps_Ephemeris eph, double obs_time); boost::posix_time::ptime compute_GPS_time(const Gps_Ephemeris& eph, const double obs_time);
/*! /*!
* \brief Computes the Galileo time and returns a boost::posix_time::ptime object * \brief Computes the Galileo time and returns a boost::posix_time::ptime object
*/ */
boost::posix_time::ptime compute_Galileo_time(Galileo_Ephemeris eph, double obs_time); boost::posix_time::ptime compute_Galileo_time(const Galileo_Ephemeris& eph, const double obs_time);
/*! /*!
* \brief Writes data from the GPS navigation message into the RINEX file * \brief Writes data from the GPS navigation message into the RINEX file
*/ */
void log_rinex_nav(std::ofstream& out, std::map<int,Gps_Ephemeris> eph_map); void log_rinex_nav(std::ofstream& out, const std::map<int, Gps_Ephemeris>& eph_map);
/*! /*!
* \brief Writes data from the Galileo navigation message into the RINEX file * \brief Writes data from the Galileo navigation message into the RINEX file
*/ */
void log_rinex_nav(std::ofstream& out, std::map<int, Galileo_Ephemeris> eph_map); void log_rinex_nav(std::ofstream& out, const std::map<int, Galileo_Ephemeris>& eph_map);
/*! /*!
* \brief Writes data from the Mixed (GPS/Galileo) navigation message into the RINEX file * \brief Writes data from the Mixed (GPS/Galileo) navigation message into the RINEX file
*/ */
void log_rinex_nav(std::ofstream& out, std::map<int, Gps_Ephemeris> gps_eph_map, std::map<int, Galileo_Ephemeris> galileo_eph_map); void log_rinex_nav(std::ofstream& out, const std::map<int, Gps_Ephemeris>& gps_eph_map, const std::map<int, Galileo_Ephemeris>& galileo_eph_map);
/*! /*!
* \brief Writes GPS observables into the RINEX file * \brief Writes GPS observables into the RINEX file
*/ */
void log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double obs_time, std::map<int,Gnss_Synchro> pseudoranges); void log_rinex_obs(std::ofstream& out, const Gps_Ephemeris& eph, double obs_time, const std::map<int, Gnss_Synchro>& pseudoranges);
/*! /*!
* \brief Writes Galileo observables into the RINEX file * \brief Writes Galileo observables into the RINEX file
*/ */
void log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, double obs_time, std::map<int,Gnss_Synchro> pseudoranges); void log_rinex_obs(std::ofstream& out, const Galileo_Ephemeris& eph, double obs_time, const std::map<int, Gnss_Synchro>& pseudoranges);
/*! /*!
* \brief Writes Galileo observables into the RINEX file * \brief Writes Galileo observables into the RINEX file
*/ */
void log_rinex_obs(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double gps_obs_time, std::map<int,Gnss_Synchro> pseudoranges); void log_rinex_obs(std::ofstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double gps_obs_time, const std::map<int, Gnss_Synchro>& pseudoranges);
/*! /*!
* \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not. * \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not.
@@ -178,12 +178,12 @@ public:
/*! /*!
* \brief Writes raw SBAS messages into the RINEX file * \brief Writes raw SBAS messages into the RINEX file
*/ */
void log_rinex_sbs(std::ofstream& out, Sbas_Raw_Msg sbs_message); void log_rinex_sbs(std::ofstream& out, const Sbas_Raw_Msg& sbs_message);
std::map<std::string,std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass std::map<std::string,std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass
std::map<std::string,std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH std::map<std::string,std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH
std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors
std::string stringVersion; //<! RINEX version (2.10/2.11 or 3.01) std::string stringVersion; //<! RINEX version (2.10/2.11 or 3.01/3.02)
private: private:
int version ; // RINEX version (2 for 2.10/2.11 and 3 for 3.01) int version ; // RINEX version (2 for 2.10/2.11 and 3 for 3.01)
@@ -191,7 +191,7 @@ private:
/* /*
* Generation of RINEX signal strength indicators * Generation of RINEX signal strength indicators
*/ */
int signalStrength(double snr); int signalStrength(const double snr);
/* Creates RINEX file names according to the naming convention /* Creates RINEX file names according to the naming convention
* *
@@ -225,7 +225,7 @@ private:
/* /*
* Checks that the line is 80 characters length * Checks that the line is 80 characters length
*/ */
void lengthCheck(std::string line); void lengthCheck(const std::string& line);
/* /*
* If the string is bigger than length, truncate it from the right. * If the string is bigger than length, truncate it from the right.

6
src/algorithms/PVT/libs/rtcm_printer.cc
View File

@@ -277,7 +277,7 @@ std::string Rtcm_Printer::print_M1005_test ()
{ {
std::bitset<152> m1005 = get_M1005_test(); std::bitset<152> m1005 = get_M1005_test();
unsigned int msg_length_bits = m1005.to_string().length(); unsigned int msg_length_bits = m1005.to_string().length();
unsigned int msg_length_bytes = std::ceil((float)msg_length_bits / 8.0); unsigned int msg_length_bytes = std::ceil(static_cast<float>(msg_length_bits) / 8.0);
message_length = std::bitset<10>(msg_length_bytes); message_length = std::bitset<10>(msg_length_bytes);
unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits; unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits;
std::string b(zeros_to_fill, '0'); std::string b(zeros_to_fill, '0');
@@ -313,11 +313,11 @@ void Rtcm_Printer::print_M1001 ()
{ {
std::bitset<122> m1001 = get_M1001(); std::bitset<122> m1001 = get_M1001();
unsigned int msg_length_bits = m1001.to_string().length(); unsigned int msg_length_bits = m1001.to_string().length();
unsigned int msg_length_bytes = std::ceil((float)msg_length_bits/8.0); unsigned int msg_length_bytes = std::ceil(static_cast<float>(msg_length_bits) / 8.0);
message_length = std::bitset<10>(msg_length_bytes); message_length = std::bitset<10>(msg_length_bytes);
unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits; unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits;
std::string b(zeros_to_fill, '0'); std::string b(zeros_to_fill, '0');
message_length = std::bitset<10>((int)msg_length_bytes); message_length = std::bitset<10>(static_cast<int>(msg_length_bytes));
std::string msg_content = m1001.to_string() + b; std::string msg_content = m1001.to_string() + b;
std::string msg_without_crc = preamble.to_string() + std::string msg_without_crc = preamble.to_string() +
reserved_field.to_string() + reserved_field.to_string() +

2
src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.cc
View File

@@ -268,7 +268,7 @@ float GalileoE1Pcps8msAmbiguousAcquisition::calculate_threshold(float pfa)
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_*frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa,exponent); double val = pow(1.0 - pfa,exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);

6
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc
View File

@@ -277,9 +277,9 @@ float GalileoE1PcpsAmbiguousAcquisition::calculate_threshold(float pfa)
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa; DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0-pfa,exponent); double val = pow(1.0 - pfa,exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);
float threshold = (float)quantile(mydist,val); float threshold = (float)quantile(mydist,val);

4
src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc
View File

@@ -64,7 +64,7 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition
if (sampled_ms_ % 4 != 0) if (sampled_ms_ % 4 != 0)
{ {
sampled_ms_ = (int)(sampled_ms_/4) * 4; sampled_ms_ = static_cast<int>(sampled_ms_ / 4) * 4;
LOG(WARNING) << "coherent_integration_time should be multiple of " LOG(WARNING) << "coherent_integration_time should be multiple of "
<< "Galileo code length (4 ms). coherent_integration_time = " << "Galileo code length (4 ms). coherent_integration_time = "
<< sampled_ms_ << " ms will be used."; << sampled_ms_ << " ms will be used.";
@@ -82,7 +82,7 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition
/ (Galileo_E1_CODE_CHIP_RATE_HZ / (Galileo_E1_CODE_CHIP_RATE_HZ
/ Galileo_E1_B_CODE_LENGTH_CHIPS)); / Galileo_E1_B_CODE_LENGTH_CHIPS));
vector_length_ = code_length_ * (int)(sampled_ms_/4); vector_length_ = code_length_ * static_cast<int>(sampled_ms_ / 4);
int samples_per_ms = code_length_ / 4; int samples_per_ms = code_length_ / 4;

2
src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.cc
View File

@@ -305,7 +305,7 @@ float GalileoE1PcpsQuickSyncAmbiguousAcquisition::calculate_threshold(float pfa)
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa; DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
unsigned int ncells = code_length_/folding_factor_ * frequency_bins; unsigned int ncells = code_length_/folding_factor_ * frequency_bins;
double exponent = 1 / (double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(code_length_/folding_factor_); double lambda = double(code_length_/folding_factor_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);

6
src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.cc
View File

@@ -268,10 +268,10 @@ float GalileoE1PcpsTongAmbiguousAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0-pfa,exponent); double val = pow(1.0-pfa,exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);

6
src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.cc
View File

@@ -293,9 +293,9 @@ float GalileoE5aNoncoherentIQAcquisitionCaf::calculate_threshold(float pfa)
{ {
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);

4
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc
View File

@@ -254,8 +254,8 @@ float GpsL1CaPcpsAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);

2
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.h
View File

@@ -64,7 +64,7 @@ public:
} }
/*! /*!
* \brief Returns "GPS_L1_CA_PCPS_Assisted_Acquisition" * \brief Returns "GPS_L1_CA_PCPS_Acquisition_Fine_Doppler"
*/ */
std::string implementation() std::string implementation()
{ {

8
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_multithread_acquisition.cc
View File

@@ -250,11 +250,11 @@ float GpsL1CaPcpsMultithreadAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa; DLOG(INFO) << "Channel "<< channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0-pfa,exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);
float threshold = (float)quantile(mydist,val); float threshold = (float)quantile(mydist,val);

8
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.cc
View File

@@ -249,11 +249,11 @@ float GpsL1CaPcpsOpenClAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa; DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0-pfa,exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);
float threshold = (float)quantile(mydist,val); float threshold = (float)quantile(mydist,val);

6
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.cc
View File

@@ -286,10 +286,10 @@ float GpsL1CaPcpsQuickSyncAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa; DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa;
unsigned int ncells = (code_length_/folding_factor_)*frequency_bins; unsigned int ncells = (code_length_ / folding_factor_) * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0 - pfa, exponent); double val = pow(1.0 - pfa, exponent);
double lambda = double((code_length_/folding_factor_)); double lambda = double((code_length_ / folding_factor_));
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);
float threshold = (float)quantile(mydist,val); float threshold = (float)quantile(mydist,val);

10
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.cc
View File

@@ -239,14 +239,14 @@ float GpsL1CaPcpsTongAcquisition::calculate_threshold(float pfa)
frequency_bins++; frequency_bins++;
} }
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa; DLOG(INFO) << "Channel "<< channel_ <<" Pfa = "<< pfa;
unsigned int ncells = vector_length_*frequency_bins; unsigned int ncells = vector_length_ * frequency_bins;
double exponent = 1/(double)ncells; double exponent = 1 / static_cast<double>(ncells);
double val = pow(1.0-pfa,exponent); double val = pow(1.0 - pfa,exponent);
double lambda = double(vector_length_); double lambda = double(vector_length_);
boost::math::exponential_distribution<double> mydist (lambda); boost::math::exponential_distribution<double> mydist (lambda);
float threshold = (float)quantile(mydist,val); float threshold = (float)quantile(mydist, val);
return threshold; return threshold;
} }

408
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc
View File

@@ -107,35 +107,27 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
d_both_signal_components = both_signal_components_; d_both_signal_components = both_signal_components_;
d_CAF_window_hz = CAF_window_hz_; d_CAF_window_hz = CAF_window_hz_;
//todo: do something if posix_memalign fails d_inbuffer = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_inbuffer, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_I_A = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_code_I_A, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitudeIA = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitudeIA, 16, d_fft_size * sizeof(float)) == 0){};
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
if (posix_memalign((void**)&d_fft_code_Q_A, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_Q_A = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitudeQA, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitudeQA = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
} }
// IF COHERENT INTEGRATION TIME > 1 // IF COHERENT INTEGRATION TIME > 1
if (d_sampled_ms > 1) if (d_sampled_ms > 1)
{ {
if (posix_memalign((void**)&d_fft_code_I_B, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_I_B = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitudeIB, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitudeIB = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
if (posix_memalign((void**)&d_fft_code_Q_B, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_Q_B = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitudeQB, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitudeQB = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
} }
} }
// if (posix_memalign((void**)&d_fft_code_Q_A, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// if (posix_memalign((void**)&d_magnitudeQA, 16, d_fft_size * sizeof(float)) == 0){};
// if (posix_memalign((void**)&d_fft_code_I_B, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// if (posix_memalign((void**)&d_magnitudeIB, 16, d_fft_size * sizeof(float)) == 0){};
// if (posix_memalign((void**)&d_fft_code_Q_B, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// if (posix_memalign((void**)&d_magnitudeQB, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -153,34 +145,43 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::~galileo_e5a_noncoherentIQ_acquisi
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_code_I_A); volk_free(d_inbuffer);
free(d_magnitudeIA); volk_free(d_fft_code_I_A);
volk_free(d_magnitudeIA);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
free(d_fft_code_Q_A); volk_free(d_fft_code_Q_A);
free(d_magnitudeQA); volk_free(d_magnitudeQA);
} }
// IF INTEGRATION TIME > 1 // IF INTEGRATION TIME > 1
if (d_sampled_ms > 1) if (d_sampled_ms > 1)
{ {
free(d_fft_code_I_B); volk_free(d_fft_code_I_B);
free(d_magnitudeIB); volk_free(d_magnitudeIB);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
free(d_fft_code_Q_B); volk_free(d_fft_code_Q_B);
free(d_magnitudeQB); volk_free(d_magnitudeQB);
} }
} }
if (d_CAF_window_hz > 0)
{
volk_free(d_CAF_vector);
volk_free(d_CAF_vector_I);
if (d_both_signal_components == true)
{
volk_free(d_CAF_vector_Q);
}
}
delete d_fft_if; delete d_fft_if;
delete d_ifft; delete d_ifft;
if (d_dump) if (d_dump)
{ {
d_dump_file.close(); d_dump_file.close();
@@ -197,70 +198,45 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_local_code(std::complex<f
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_I_A,d_fft_if->get_outbuf(),d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_I_A,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_I_A,d_fft_if->get_outbuf(),d_fft_size);
}
// SAME FOR PILOT SIGNAL // SAME FOR PILOT SIGNAL
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
// Three replicas of pilot primary code. CODE A: (1,1,1) // Three replicas of pilot primary code. CODE A: (1,1,1)
memcpy(d_fft_if->get_inbuf(), codeQ, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), codeQ, sizeof(gr_complex)*d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_Q_A,d_fft_if->get_outbuf(),d_fft_size);
{ }
volk_32fc_conjugate_32fc_u(d_fft_code_Q_A,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_Q_A,d_fft_if->get_outbuf(),d_fft_size);
}
}
// IF INTEGRATION TIME > 1 code, we need to evaluate the other possible combination // IF INTEGRATION TIME > 1 code, we need to evaluate the other possible combination
// Note: max integration time allowed = 3ms (dealt in adapter) // Note: max integration time allowed = 3ms (dealt in adapter)
if (d_sampled_ms > 1) if (d_sampled_ms > 1)
{ {
// DATA CODE B: First replica is inverted (0,1,1) // DATA CODE B: First replica is inverted (0,1,1)
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[0], volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[0],
&codeI[0], gr_complex(-1,0), &codeI[0], gr_complex(-1,0),
d_samples_per_code); d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code d_fft_if->execute(); // We need the FFT of local code
if (is_unaligned())
{
volk_32fc_conjugate_32fc_u(d_fft_code_I_B,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_I_B,d_fft_if->get_outbuf(),d_fft_size);
}
if (d_both_signal_components == true)
{
// PILOT CODE B: First replica is inverted (0,1,1)
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[0],
&codeQ[0], gr_complex(-1,0),
d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_I_B,d_fft_if->get_outbuf(),d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size); if (d_both_signal_components == true)
} {
else // PILOT CODE B: First replica is inverted (0,1,1)
{ volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[0],
volk_32fc_conjugate_32fc_a(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size); &codeQ[0], gr_complex(-1,0),
} d_samples_per_code);
} d_fft_if->execute(); // We need the FFT of local code
}
//Conjugate the local code
volk_32fc_conjugate_32fc(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size);
}
}
} }
void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init() void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
@@ -273,8 +249,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= (int)d_doppler_max; doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
@@ -284,10 +260,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){}; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
} }
@@ -297,11 +271,11 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
// if (d_CAF_filter) // if (d_CAF_filter)
if (d_CAF_window_hz > 0) if (d_CAF_window_hz > 0)
{ {
if (posix_memalign((void**)&d_CAF_vector, 16, d_num_doppler_bins * sizeof(float)) == 0){}; d_CAF_vector = static_cast<float*>(volk_malloc(d_num_doppler_bins * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_CAF_vector_I, 16, d_num_doppler_bins * sizeof(float)) == 0){}; d_CAF_vector_I = static_cast<float*>(volk_malloc(d_num_doppler_bins * sizeof(float), volk_get_alignment()));
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
if (posix_memalign((void**)&d_CAF_vector_Q, 16, d_num_doppler_bins * sizeof(float)) == 0){}; d_CAF_vector_Q = static_cast<float*>(volk_malloc(d_num_doppler_bins * sizeof(float), volk_get_alignment()));
} }
} }
} }
@@ -355,17 +329,17 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
{ {
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
unsigned int buff_increment; unsigned int buff_increment;
if (ninput_items[0]+d_buffer_count <= d_fft_size) if (ninput_items[0] + d_buffer_count <= d_fft_size)
{ {
buff_increment = ninput_items[0]; buff_increment = ninput_items[0];
} }
else else
{ {
buff_increment = (d_fft_size-d_buffer_count); buff_increment = (d_fft_size - d_buffer_count);
} }
memcpy(&d_inbuffer[d_buffer_count], in, sizeof(gr_complex)*buff_increment); memcpy(&d_inbuffer[d_buffer_count], in, sizeof(gr_complex) * buff_increment);
// If buffer will be full in next iteration // If buffer will be full in next iteration
if (d_buffer_count >= d_fft_size-d_gr_stream_buffer) if (d_buffer_count >= d_fft_size - d_gr_stream_buffer)
{ {
d_state=2; d_state=2;
} }
@@ -396,7 +370,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
float magt_IB = 0.0; float magt_IB = 0.0;
float magt_QA = 0.0; float magt_QA = 0.0;
float magt_QB = 0.0; float magt_QB = 0.0;
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0; d_input_power = 0.0;
d_mag = 0.0; d_mag = 0.0;
d_well_count++; d_well_count++;
@@ -408,18 +382,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitudeIA, d_inbuffer, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_inbuffer, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitudeIA, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitudeIA, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), d_inbuffer, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), d_inbuffer,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@@ -429,46 +403,46 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// CODE IA // CODE IA
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_I_A, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_I_A, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitudeIA, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_IA, d_magnitudeIA, d_fft_size); volk_32f_index_max_16u(&indext_IA, d_magnitudeIA, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt_IA = d_magnitudeIA[indext_IA] / (fft_normalization_factor * fft_normalization_factor); magt_IA = d_magnitudeIA[indext_IA] / (fft_normalization_factor * fft_normalization_factor);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
// REPEAT FOR ALL CODES. CODE_QA // REPEAT FOR ALL CODES. CODE_QA
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_Q_A, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_Q_A, d_fft_size);
d_ifft->execute(); d_ifft->execute();
volk_32fc_magnitude_squared_32f_a(d_magnitudeQA, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeQA, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_QA, d_magnitudeQA, d_fft_size); volk_32f_index_max_16u(&indext_QA, d_magnitudeQA, d_fft_size);
magt_QA = d_magnitudeQA[indext_QA] / (fft_normalization_factor * fft_normalization_factor); magt_QA = d_magnitudeQA[indext_QA] / (fft_normalization_factor * fft_normalization_factor);
} }
if (d_sampled_ms > 1) // If Integration time > 1 code if (d_sampled_ms > 1) // If Integration time > 1 code
{ {
// REPEAT FOR ALL CODES. CODE_IB // REPEAT FOR ALL CODES. CODE_IB
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_I_B, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_I_B, d_fft_size);
d_ifft->execute(); d_ifft->execute();
volk_32fc_magnitude_squared_32f_a(d_magnitudeIB, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeIB, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_IB, d_magnitudeIB, d_fft_size); volk_32f_index_max_16u(&indext_IB, d_magnitudeIB, d_fft_size);
magt_IB = d_magnitudeIB[indext_IB] / (fft_normalization_factor * fft_normalization_factor); magt_IB = d_magnitudeIB[indext_IB] / (fft_normalization_factor * fft_normalization_factor);
if (d_both_signal_components == true) if (d_both_signal_components == true)
{ {
// REPEAT FOR ALL CODES. CODE_QB // REPEAT FOR ALL CODES. CODE_QB
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_Q_B, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_Q_B, d_fft_size);
d_ifft->execute(); d_ifft->execute();
volk_32fc_magnitude_squared_32f_a(d_magnitudeQB, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitudeQB, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_QB, d_magnitudeQB, d_fft_size); volk_32f_index_max_16u(&indext_QB, d_magnitudeQB, d_fft_size);
magt_QB = d_magnitudeIB[indext_QB] / (fft_normalization_factor * fft_normalization_factor); magt_QB = d_magnitudeIB[indext_QB] / (fft_normalization_factor * fft_normalization_factor);
} }
} }
@@ -505,7 +479,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
} }
} }
} }
volk_32f_index_max_16u_a(&indext, d_magnitudeIA, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitudeIA, d_fft_size);
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
} }
else else
@@ -534,7 +508,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
} }
} }
} }
volk_32f_index_max_16u_a(&indext, d_magnitudeIB, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitudeIB, d_fft_size);
magt = d_magnitudeIB[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitudeIB[indext] / (fft_normalization_factor * fft_normalization_factor);
} }
} }
@@ -552,7 +526,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_magnitudeIA[i] += d_magnitudeQA[i]; d_magnitudeIA[i] += d_magnitudeQA[i];
} }
} }
volk_32f_index_max_16u_a(&indext, d_magnitudeIA, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitudeIA, d_fft_size);
magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitudeIA[indext] / (fft_normalization_factor * fft_normalization_factor);
} }
@@ -569,8 +543,8 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// restarted between consecutive dwells in multidwell operation. // restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{ {
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
@@ -609,104 +583,103 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// 6 OPTIONAL: CAF filter to avoid Doppler ambiguity in bit transition. // 6 OPTIONAL: CAF filter to avoid Doppler ambiguity in bit transition.
if (d_CAF_window_hz > 0) if (d_CAF_window_hz > 0)
{ {
int CAF_bins_half; int CAF_bins_half;
float* accum; float* accum = static_cast<float*>(volk_malloc(sizeof(float), volk_get_alignment()));
// double* accum; CAF_bins_half = d_CAF_window_hz / (2 * d_doppler_step);
if (posix_memalign((void**)&accum, 16, sizeof(float)) == 0){}; float weighting_factor;
CAF_bins_half = d_CAF_window_hz/(2*d_doppler_step); weighting_factor = 0.5 / static_cast<float>(CAF_bins_half);
float weighting_factor; // weighting_factor = 0;
weighting_factor = 0.5/(float)CAF_bins_half; // std::cout << "weighting_factor " << weighting_factor << std::endl;
// weighting_factor = 0; // Initialize first iterations
// std::cout << "weighting_factor " << weighting_factor << std::endl; for (int doppler_index=0; doppler_index < CAF_bins_half; doppler_index++)
// Initialize first iterations {
for (int doppler_index=0;doppler_index<CAF_bins_half;doppler_index++) d_CAF_vector[doppler_index] = 0;
{ // volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], d_CAF_vector_I, CAF_bins_half+doppler_index+1);
d_CAF_vector[doppler_index] = 0; for (int i = 0; i < CAF_bins_half + doppler_index + 1; i++)
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], d_CAF_vector_I, CAF_bins_half+doppler_index+1); {
for (int i = 0; i < CAF_bins_half+doppler_index+1; i++) d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * static_cast<unsigned int>(abs(doppler_index - i)));
{ }
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1-weighting_factor*(unsigned int)(abs(doppler_index - i))); // d_CAF_vector[doppler_index] /= CAF_bins_half+doppler_index+1;
} d_CAF_vector[doppler_index] /= 1 + CAF_bins_half+doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor*doppler_index*(doppler_index+1)/2; // triangles = [n*(n+1)/2]
// d_CAF_vector[doppler_index] /= CAF_bins_half+doppler_index+1; if (d_both_signal_components)
d_CAF_vector[doppler_index] /= 1+CAF_bins_half+doppler_index - weighting_factor*CAF_bins_half*(CAF_bins_half+1)/2 - weighting_factor*doppler_index*(doppler_index+1)/2; // triangles = [n*(n+1)/2] {
if (d_both_signal_components) accum[0] = 0;
{ // volk_32f_accumulator_s32f_a(&accum[0], d_CAF_vector_Q, CAF_bins_half+doppler_index+1);
accum[0] = 0; for (int i = 0; i < CAF_bins_half+doppler_index+1; i++)
// volk_32f_accumulator_s32f_a(&accum[0], d_CAF_vector_Q, CAF_bins_half+doppler_index+1); {
for (int i = 0; i < CAF_bins_half+doppler_index+1; i++) accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * static_cast<unsigned int>(abs(doppler_index - i)));
{ }
accum[0] += d_CAF_vector_Q[i] * (1-weighting_factor*(unsigned int)(abs(doppler_index - i))); // accum[0] /= CAF_bins_half+doppler_index+1;
} accum[0] /= 1+CAF_bins_half+doppler_index - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * doppler_index * (doppler_index + 1) / 2; // triangles = [n*(n+1)/2]
// accum[0] /= CAF_bins_half+doppler_index+1; d_CAF_vector[doppler_index] += accum[0];
accum[0] /= 1+CAF_bins_half+doppler_index - weighting_factor*CAF_bins_half*(CAF_bins_half+1)/2 - weighting_factor*doppler_index*(doppler_index+1)/2; // triangles = [n*(n+1)/2] }
d_CAF_vector[doppler_index] += accum[0]; }
} // Body loop
} for (unsigned int doppler_index = CAF_bins_half;doppler_index<d_num_doppler_bins-CAF_bins_half;doppler_index++)
// Body loop {
for (unsigned int doppler_index=CAF_bins_half;doppler_index<d_num_doppler_bins-CAF_bins_half;doppler_index++) d_CAF_vector[doppler_index] = 0;
{ // volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], 2*CAF_bins_half+1);
d_CAF_vector[doppler_index] = 0; for (int i = doppler_index-CAF_bins_half; i < doppler_index+CAF_bins_half+1; i++)
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], 2*CAF_bins_half+1); {
for (int i = doppler_index-CAF_bins_half; i < doppler_index+CAF_bins_half+1; i++) d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1-weighting_factor * static_cast<unsigned int>(abs(doppler_index - i)));
{ }
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1-weighting_factor*(unsigned int)(abs(doppler_index - i))); // d_CAF_vector[doppler_index] /= 2*CAF_bins_half+1;
} d_CAF_vector[doppler_index] /= 1 + 2 * CAF_bins_half - 2 *weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
// d_CAF_vector[doppler_index] /= 2*CAF_bins_half+1; if (d_both_signal_components)
d_CAF_vector[doppler_index] /= 1+2*CAF_bins_half - 2*weighting_factor*CAF_bins_half*(CAF_bins_half+1)/2; {
if (d_both_signal_components) accum[0] = 0;
{ // volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], 2*CAF_bins_half);
accum[0] = 0; for (int i = doppler_index-CAF_bins_half; i < doppler_index+CAF_bins_half+1; i++)
// volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], 2*CAF_bins_half); {
for (int i = doppler_index-CAF_bins_half; i < doppler_index+CAF_bins_half+1; i++) accum[0] += d_CAF_vector_Q[i] * (1 -weighting_factor * static_cast<unsigned int>(abs(doppler_index - i)));
{ }
accum[0] += d_CAF_vector_Q[i] * (1-weighting_factor*(unsigned int)(abs(doppler_index - i))); // accum[0] /= 2*CAF_bins_half+1;
} accum[0] /= 1+2*CAF_bins_half - 2*weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2;
// accum[0] /= 2*CAF_bins_half+1; d_CAF_vector[doppler_index] += accum[0];
accum[0] /= 1+2*CAF_bins_half - 2*weighting_factor*CAF_bins_half*(CAF_bins_half+1)/2; }
d_CAF_vector[doppler_index] += accum[0]; }
} // Final iterations
} for (unsigned int doppler_index = d_num_doppler_bins - CAF_bins_half;doppler_index<d_num_doppler_bins;doppler_index++)
// Final iterations {
for (unsigned int doppler_index=d_num_doppler_bins-CAF_bins_half;doppler_index<d_num_doppler_bins;doppler_index++) d_CAF_vector[doppler_index] = 0;
{ // volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index));
d_CAF_vector[doppler_index] = 0; for (int i = doppler_index - CAF_bins_half; i < d_num_doppler_bins; i++)
// volk_32f_accumulator_s32f_a(&d_CAF_vector[doppler_index], &d_CAF_vector_I[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index)); {
for (int i = doppler_index-CAF_bins_half; i < d_num_doppler_bins; i++) d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1 - weighting_factor * (abs(doppler_index - i)));
{ }
d_CAF_vector[doppler_index] += d_CAF_vector_I[i] * (1-weighting_factor*(abs(doppler_index - i))); // d_CAF_vector[doppler_index] /= CAF_bins_half+(d_num_doppler_bins-doppler_index);
} d_CAF_vector[doppler_index] /= 1+CAF_bins_half+(d_num_doppler_bins-doppler_index-1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
// d_CAF_vector[doppler_index] /= CAF_bins_half+(d_num_doppler_bins-doppler_index); if (d_both_signal_components)
d_CAF_vector[doppler_index] /= 1+CAF_bins_half+(d_num_doppler_bins-doppler_index-1) -weighting_factor*CAF_bins_half*(CAF_bins_half+1)/2 -weighting_factor*(d_num_doppler_bins-doppler_index-1)*(d_num_doppler_bins-doppler_index)/2; {
if (d_both_signal_components) accum[0] = 0;
{ // volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index));
accum[0] = 0; for (int i = doppler_index-CAF_bins_half; i < d_num_doppler_bins; i++)
// volk_32f_accumulator_s32f_a(&accum[0], &d_CAF_vector_Q[doppler_index-CAF_bins_half], CAF_bins_half + (d_num_doppler_bins-doppler_index)); {
for (int i = doppler_index-CAF_bins_half; i < d_num_doppler_bins; i++) accum[0] += d_CAF_vector_Q[i] * (1 - weighting_factor * (abs(doppler_index - i)));
{ }
accum[0] += d_CAF_vector_Q[i] * (1-weighting_factor*(abs(doppler_index - i))); // accum[0] /= CAF_bins_half+(d_num_doppler_bins-doppler_index);
} accum[0] /= 1 + CAF_bins_half + (d_num_doppler_bins - doppler_index - 1) - weighting_factor * CAF_bins_half * (CAF_bins_half + 1) / 2 - weighting_factor * (d_num_doppler_bins - doppler_index - 1) * (d_num_doppler_bins - doppler_index) / 2;
// accum[0] /= CAF_bins_half+(d_num_doppler_bins-doppler_index); d_CAF_vector[doppler_index] += accum[0];
accum[0] /= 1+CAF_bins_half+(d_num_doppler_bins-doppler_index-1) -weighting_factor*CAF_bins_half*(CAF_bins_half+1)/2 -weighting_factor*(d_num_doppler_bins-doppler_index-1)*(d_num_doppler_bins-doppler_index)/2; }
d_CAF_vector[doppler_index] += accum[0]; }
}
}
// Recompute the maximum doppler peak // Recompute the maximum doppler peak
volk_32f_index_max_16u_a(&indext, d_CAF_vector, d_num_doppler_bins); volk_32f_index_max_16u(&indext, d_CAF_vector, d_num_doppler_bins);
doppler=-(int)d_doppler_max+d_doppler_step*indext; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * indext;
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
// Dump if required, appended at the end of the file // Dump if required, appended at the end of the file
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = sizeof(float) * (d_num_doppler_bins); // noncomplex file write std::streamsize n = sizeof(float) * (d_num_doppler_bins); // noncomplex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_E5a_sat_" filename << "../data/test_statistics_E5a_sat_"
<< d_gnss_synchro->PRN << "_CAF.dat"; << d_gnss_synchro->PRN << "_CAF.dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_CAF_vector, n); d_dump_file.write((char*)d_CAF_vector, n);
d_dump_file.close(); d_dump_file.close();
} }
volk_free(accum);
} }
if (d_well_count == d_max_dwells) if (d_well_count == d_max_dwells)
@@ -725,7 +698,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_state = 1; d_state = 1;
} }
consume_each(d_fft_size-d_buffer_count); consume_each(d_fft_size - d_buffer_count);
d_buffer_count = 0; d_buffer_count = 0;
break; break;
} }
@@ -745,7 +718,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_active = false; d_active = false;
d_state = 0; d_state = 0;
acquisition_message = 1; acquisition_message = 1;
d_channel_internal_queue->push(acquisition_message); d_channel_internal_queue->push(acquisition_message);
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter

88
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc
View File

@@ -79,10 +79,9 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
//todo: do something if posix_memalign fails d_fft_code_A = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_code_A, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_B = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_code_B, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -101,14 +100,14 @@ galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_code_A); volk_free(d_fft_code_A);
free(d_fft_code_B); volk_free(d_fft_code_B);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@@ -121,37 +120,23 @@ galileo_pcps_8ms_acquisition_cc::~galileo_pcps_8ms_acquisition_cc()
void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> * code) void galileo_pcps_8ms_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
// code A: two replicas of a primary code // code A: two replicas of a primary code
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_A, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_A,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_A,d_fft_if->get_outbuf(),d_fft_size);
}
// code B: two replicas of a primary code; the second replica is inverted. // code B: two replicas of a primary code; the second replica is inverted.
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[d_samples_per_code], volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[d_samples_per_code],
&code[d_samples_per_code], gr_complex(-1,0), &code[d_samples_per_code], gr_complex(-1,0),
d_samples_per_code); d_samples_per_code);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_B, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_B,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_B,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void galileo_pcps_8ms_acquisition_cc::init() void galileo_pcps_8ms_acquisition_cc::init()
@@ -164,8 +149,8 @@ void galileo_pcps_8ms_acquisition_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= (int)d_doppler_max; doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
@@ -173,14 +158,11 @@ void galileo_pcps_8ms_acquisition_cc::init()
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index=0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){}; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], d_freq + doppler, d_fs_in, d_fft_size);
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size);
} }
} }
@@ -226,7 +208,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
float magt_A = 0.0; float magt_A = 0.0;
float magt_B = 0.0; float magt_B = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0; d_input_power = 0.0;
d_mag = 0.0; d_mag = 0.0;
@@ -241,18 +223,18 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@@ -262,15 +244,15 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code A reference using SIMD operations with // with the local FFT'd code A reference using SIMD operations with
// VOLK library // VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_A, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_A, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_A, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_A, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt_A = d_magnitude[indext_A] / (fft_normalization_factor * fft_normalization_factor); magt_A = d_magnitude[indext_A] / (fft_normalization_factor * fft_normalization_factor);
@@ -278,15 +260,15 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code B reference using SIMD operations with // with the local FFT'd code B reference using SIMD operations with
// VOLK library // VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_B, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_B, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext_B, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_B, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt_B = d_magnitude[indext_B] / (fft_normalization_factor * fft_normalization_factor); magt_B = d_magnitude[indext_B] / (fft_normalization_factor * fft_normalization_factor);
@@ -307,8 +289,8 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
if (d_mag < magt) if (d_mag < magt)
{ {
d_mag = magt; d_mag = magt;
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
} }

63
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc
View File

@@ -86,9 +86,8 @@ pcps_acquisition_cc::pcps_acquisition_cc(
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
d_bit_transition_flag = bit_transition_flag; d_bit_transition_flag = bit_transition_flag;
//todo: do something if posix_memalign fails d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -107,13 +106,13 @@ pcps_acquisition_cc::~pcps_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@@ -126,19 +125,9 @@ pcps_acquisition_cc::~pcps_acquisition_cc()
void pcps_acquisition_cc::set_local_code(std::complex<float> * code) void pcps_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
//Conjugate the local code
if (is_unaligned())
{
volk_32fc_conjugate_32fc_u(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void pcps_acquisition_cc::init() void pcps_acquisition_cc::init()
@@ -151,8 +140,8 @@ void pcps_acquisition_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= (int)d_doppler_max; doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
@@ -160,14 +149,12 @@ void pcps_acquisition_cc::init()
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){}; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], d_freq + doppler, d_fs_in, d_fft_size);
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size);
} }
} }
@@ -219,7 +206,7 @@ int pcps_acquisition_cc::general_work(int noutput_items,
unsigned int indext = 0; unsigned int indext = 0;
float magt = 0.0; float magt = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0; d_input_power = 0.0;
d_mag = 0.0; d_mag = 0.0;
@@ -234,18 +221,18 @@ int pcps_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index=0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@@ -254,15 +241,15 @@ int pcps_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
@@ -281,8 +268,8 @@ int pcps_acquisition_cc::general_work(int noutput_items,
// restarted between consecutive dwells in multidwell operation. // restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{ {
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold

704
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
View File

@@ -46,96 +46,94 @@
using google::LogMessage; using google::LogMessage;
pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc( pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump, long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump,
std::string dump_filename) std::string dump_filename)
{ {
return pcps_acquisition_fine_doppler_cc_sptr( return pcps_acquisition_fine_doppler_cc_sptr(
new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq, new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq,
fs_in, samples_per_ms, queue, dump, dump_filename)); fs_in, samples_per_ms, queue, dump, dump_filename));
} }
pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc( pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq, int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min, long freq,
long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump, long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump,
std::string dump_filename) : std::string dump_filename) :
gr::block("pcps_acquisition_fine_doppler_cc", gr::block("pcps_acquisition_fine_doppler_cc",
gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(0, 0, sizeof(gr_complex))) gr::io_signature::make(0, 0, sizeof(gr_complex)))
{ {
d_sample_counter = 0; // SAMPLE COUNTER d_sample_counter = 0; // SAMPLE COUNTER
d_active = false; d_active = false;
d_queue = queue; d_queue = queue;
d_freq = freq; d_freq = freq;
d_fs_in = fs_in; d_fs_in = fs_in;
d_samples_per_ms = samples_per_ms; d_samples_per_ms = samples_per_ms;
d_sampled_ms = sampled_ms; d_sampled_ms = sampled_ms;
d_config_doppler_max = doppler_max; d_config_doppler_max = doppler_max;
d_config_doppler_min=doppler_min; d_config_doppler_min = doppler_min;
d_fft_size = d_sampled_ms * d_samples_per_ms; d_fft_size = d_sampled_ms * d_samples_per_ms;
// HS Acquisition // HS Acquisition
d_max_dwells= max_dwells; d_max_dwells = max_dwells;
d_gnuradio_forecast_samples=d_fft_size; d_gnuradio_forecast_samples = d_fft_size;
d_input_power = 0.0; d_input_power = 0.0;
d_state=0; d_state = 0;
//todo: do something if posix_memalign fails d_carrier = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_carrier, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
// Inverse FFT // Inverse FFT
d_ifft = new gr::fft::fft_complex(d_fft_size, false); d_ifft = new gr::fft::fft_complex(d_fft_size, false);
// For dumping samples into a file // For dumping samples into a file
d_dump = dump; d_dump = dump;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
} }
void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step) void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
// Create the search grid array // Create the search grid array
d_num_doppler_points=floor(std::abs(d_config_doppler_max-d_config_doppler_min)/d_doppler_step); d_num_doppler_points = floor(std::abs(d_config_doppler_max - d_config_doppler_min) / d_doppler_step);
d_grid_data=new float*[d_num_doppler_points];
for (int i=0;i<d_num_doppler_points;i++)
{
if (posix_memalign((void**)&d_grid_data[i], 16, d_fft_size * sizeof(float)) == 0){};
}
update_carrier_wipeoff();
d_grid_data = new float*[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++)
{
d_grid_data[i] = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
}
update_carrier_wipeoff();
} }
void pcps_acquisition_fine_doppler_cc::free_grid_memory() void pcps_acquisition_fine_doppler_cc::free_grid_memory()
{ {
for (int i=0;i<d_num_doppler_points;i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
delete[] d_grid_data[i]; volk_free(d_grid_data[i]);
delete[] d_grid_doppler_wipeoffs[i]; delete[] d_grid_doppler_wipeoffs[i];
} }
delete d_grid_data; delete d_grid_data;
delete d_grid_doppler_wipeoffs; delete d_grid_doppler_wipeoffs;
} }
pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc() pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
{ {
free(d_carrier); volk_free(d_carrier);
free(d_fft_codes); volk_free(d_fft_codes);
delete d_ifft; volk_free(d_magnitude);
delete d_fft_if; delete d_ifft;
if (d_dump) delete d_fft_if;
{ if (d_dump)
d_dump_file.close(); {
} d_dump_file.close();
free_grid_memory(); }
free_grid_memory();
} }
@@ -143,378 +141,368 @@ pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
void pcps_acquisition_fine_doppler_cc::set_local_code(std::complex<float> * code) void pcps_acquisition_fine_doppler_cc::set_local_code(std::complex<float> * code)
{ {
memcpy(d_fft_if->get_inbuf(),code,sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
void pcps_acquisition_fine_doppler_cc::init() void pcps_acquisition_fine_doppler_cc::init()
{ {
d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0; d_input_power = 0.0;
d_state=0; d_state = 0;
} }
void pcps_acquisition_fine_doppler_cc::forecast (int noutput_items, void pcps_acquisition_fine_doppler_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = d_gnuradio_forecast_samples ; //set the required available samples in each call ninput_items_required[0] = d_gnuradio_forecast_samples ; //set the required available samples in each call
} }
void pcps_acquisition_fine_doppler_cc::reset_grid() void pcps_acquisition_fine_doppler_cc::reset_grid()
{ {
d_well_count=0; d_well_count = 0;
for (int i=0;i<d_num_doppler_points;i++) for (int i=0; i<d_num_doppler_points; i++)
{ {
for (unsigned int j=0;j<d_fft_size;j++) for (unsigned int j=0; j < d_fft_size; j++)
{ {
d_grid_data[i][j]=0.0; d_grid_data[i][j] = 0.0;
} }
} }
} }
void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff() void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
{ {
// create the carrier Doppler wipeoff signals // create the carrier Doppler wipeoff signals
int doppler_hz; int doppler_hz;
float phase_step_rad; float phase_step_rad;
d_grid_doppler_wipeoffs=new gr_complex*[d_num_doppler_points]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_points];
for (int doppler_index=0;doppler_index<d_num_doppler_points;doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
doppler_hz=d_config_doppler_min+d_doppler_step*doppler_index; doppler_hz = d_config_doppler_min + d_doppler_step*doppler_index;
// doppler search steps // doppler search steps
// compute the carrier doppler wipe-off signal and store it // compute the carrier doppler wipe-off signal and store it
phase_step_rad = (float)GPS_TWO_PI*doppler_hz / (float)d_fs_in; phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size];
d_grid_doppler_wipeoffs[doppler_index]=new gr_complex[d_fft_size]; fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size,0, phase_step_rad);
fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size,0, phase_step_rad); }
}
} }
double pcps_acquisition_fine_doppler_cc::search_maximum() double pcps_acquisition_fine_doppler_cc::search_maximum()
{ {
float magt = 0.0; float magt = 0.0;
float fft_normalization_factor; float fft_normalization_factor;
int index_doppler = 0; int index_doppler = 0;
unsigned int tmp_intex_t; unsigned int tmp_intex_t;
unsigned int index_time = 0; unsigned int index_time = 0;
for (int i=0;i<d_num_doppler_points;i++) for (int i=0;i<d_num_doppler_points;i++)
{ {
volk_32f_index_max_16u_a(&tmp_intex_t,d_grid_data[i],d_fft_size); volk_32f_index_max_16u(&tmp_intex_t, d_grid_data[i], d_fft_size);
if (d_grid_data[i][tmp_intex_t] > magt) if (d_grid_data[i][tmp_intex_t] > magt)
{ {
magt = d_grid_data[i][tmp_intex_t]; magt = d_grid_data[i][tmp_intex_t];
//std::cout<<magt<<std::endl; //std::cout<<magt<<std::endl;
index_doppler = i; index_doppler = i;
index_time = tmp_intex_t; index_time = tmp_intex_t;
} }
} }
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
fft_normalization_factor = (float)d_fft_size * (float)d_fft_size;; fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
magt = magt / (fft_normalization_factor * fft_normalization_factor); magt = magt / (fft_normalization_factor * fft_normalization_factor);
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
d_test_statistics = magt/(d_input_power*std::sqrt(d_well_count)); d_test_statistics = magt/(d_input_power*std::sqrt(d_well_count));
// 4- record the maximum peak and the associated synchronization parameters // 4- record the maximum peak and the associated synchronization parameters
d_gnss_synchro->Acq_delay_samples = (double)index_time; d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
d_gnss_synchro->Acq_doppler_hz = (double)(index_doppler*d_doppler_step+d_config_doppler_min); d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step + d_config_doppler_min);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
// Record results to file if required // Record results to file if required
if (d_dump) if (d_dump)
{ {
std::stringstream filename; std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_" <<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out d_dump_file.open(filename.str().c_str(), std::ios::out
| std::ios::binary); | std::ios::binary);
d_dump_file.write((char*)d_grid_data[index_doppler], n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write((char*)d_grid_data[index_doppler], n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
} }
return d_test_statistics; return d_test_statistics;
} }
float pcps_acquisition_fine_doppler_cc::estimate_input_power(gr_vector_const_void_star &input_items) float pcps_acquisition_fine_doppler_cc::estimate_input_power(gr_vector_const_void_star &input_items)
{ {
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
// 1- Compute the input signal power estimation // Compute the input signal power estimation
float power; float power = 0;
power=0; volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
if (is_unaligned()) volk_32f_accumulator_s32f(&power, d_magnitude, d_fft_size);
{ power /= static_cast<float>(d_fft_size);
volk_32fc_magnitude_squared_32f_u(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&power, d_magnitude, d_fft_size);
}
else
{
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&power, d_magnitude, d_fft_size);
}
power /= (float)d_fft_size;
return power; return power;
} }
int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items) int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
{ {
// initialize acquisition algorithm // initialize acquisition algorithm
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
DLOG(INFO) << "Channel: " << d_channel DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN << " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
<< " ,sample stamp: " << d_sample_counter << ", threshold: " << " ,sample stamp: " << d_sample_counter << ", threshold: "
<< d_threshold << ", doppler_max: " << d_config_doppler_max << d_threshold << ", doppler_max: " << d_config_doppler_max
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
float* p_tmp_vector; float* p_tmp_vector = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
for (int doppler_index=0;doppler_index<d_num_doppler_points;doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
// doppler search steps // doppler search steps
// Perform the carrier wipe-off // Perform the carrier wipe-off
volk_32fc_x2_multiply_32fc_u(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal // Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute(); d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// save the grid matrix delay file // save the grid matrix delay file
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float* old_vector=d_grid_data[doppler_index]; const float* old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f_u(d_grid_data[doppler_index],old_vector,p_tmp_vector,d_fft_size); volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
} }
free(p_tmp_vector); volk_free(p_tmp_vector);
return d_fft_size; return d_fft_size;
} }
int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star &input_items, int available_samples) int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star &input_items, int available_samples)
{ {
// Direct FFT // Direct FFT
int zero_padding_factor=16; int zero_padding_factor = 16;
int fft_size_extended=d_fft_size*zero_padding_factor; int fft_size_extended = d_fft_size * zero_padding_factor;
gr::fft::fft_complex *fft_operator=new gr::fft::fft_complex(fft_size_extended,true); gr::fft::fft_complex *fft_operator = new gr::fft::fft_complex(fft_size_extended, true);
//zero padding the entire vector
memset(fft_operator->get_inbuf(),0,fft_size_extended*sizeof(gr_complex));
//1. generate local code aligned with the acquisition code phase estimation //zero padding the entire vector
gr_complex *code_replica; memset(fft_operator->get_inbuf(), 0, fft_size_extended * sizeof(gr_complex));
if (posix_memalign((void**)&code_replica, 16, d_fft_size * sizeof(gr_complex)) == 0){};
gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
int shift_index=(int)d_gnss_synchro->Acq_delay_samples; //1. generate local code aligned with the acquisition code phase estimation
gr_complex *code_replica = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
//std::cout<<"shift_index="<<shift_index<<std::endl; gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
// Rotate to align the local code replica using acquisition time delay estimation
if (shift_index!=0)
{
std::rotate(code_replica,code_replica+(d_fft_size-shift_index),code_replica+d_fft_size-1);
}
//2. Perform code wipe-off int shift_index = static_cast<int>(d_gnss_synchro->Acq_delay_samples);
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
volk_32fc_x2_multiply_32fc_u(fft_operator->get_inbuf(), in, code_replica, d_fft_size); // Rotate to align the local code replica using acquisition time delay estimation
if (shift_index != 0)
{
std::rotate(code_replica, code_replica + (d_fft_size - shift_index), code_replica + d_fft_size - 1);
}
// 3. Perform the FFT (zero padded!) //2. Perform code wipe-off
fft_operator->execute(); const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
// 4. Compute the magnitude and find the maximum volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
float* p_tmp_vector;
if (posix_memalign((void**)&p_tmp_vector, 16, fft_size_extended * sizeof(float)) == 0){};
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended); // 3. Perform the FFT (zero padded!)
fft_operator->execute();
unsigned int tmp_index_freq=0; // 4. Compute the magnitude and find the maximum
volk_32f_index_max_16u_a(&tmp_index_freq,p_tmp_vector,fft_size_extended); float* p_tmp_vector = static_cast<float*>(volk_malloc(fft_size_extended * sizeof(float), volk_get_alignment()));
//std::cout<<"FFT maximum index present at "<<tmp_index_freq<<std::endl; volk_32fc_magnitude_squared_32f(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended);
//case even unsigned int tmp_index_freq = 0;
int counter=0; volk_32f_index_max_16u(&tmp_index_freq, p_tmp_vector, fft_size_extended);
float fftFreqBins[fft_size_extended]; //case even
int counter = 0;
for (int k=0;k<(fft_size_extended/2);k++) float fftFreqBins[fft_size_extended];
{
fftFreqBins[counter]=(((float)d_fs_in/2.0)*(float)k)/((float)fft_size_extended/2.0);
counter++;
}
for (int k=fft_size_extended/2;k>0;k--) for (int k=0; k < (fft_size_extended / 2); k++)
{ {
fftFreqBins[counter]=((-(float)d_fs_in/2)*(float)k)/((float)fft_size_extended/2.0); fftFreqBins[counter] = ((static_cast<float>(d_fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
counter++; counter++;
} }
// 5. Update the Doppler estimation in Hz for (int k = fft_size_extended / 2; k > 0; k--)
if (abs(fftFreqBins[tmp_index_freq]-d_gnss_synchro->Acq_doppler_hz)<1000) {
{ fftFreqBins[counter] = ((-static_cast<float>(d_fs_in) / 2) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
d_gnss_synchro->Acq_doppler_hz=(double)fftFreqBins[tmp_index_freq]; counter++;
//std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl; }
}else{
DLOG(INFO)<<"Abs(Grid Doppler - FFT Doppler)="<<abs(fftFreqBins[tmp_index_freq]-d_gnss_synchro->Acq_doppler_hz)<<std::endl; // 5. Update the Doppler estimation in Hz
DLOG(INFO)<<std::endl<<"Error estimating fine frequency Doppler"<<std::endl; if (abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000)
//debug log {
// d_gnss_synchro->Acq_doppler_hz = static_cast<double>(fftFreqBins[tmp_index_freq]);
// std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl; //std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
// std::stringstream filename; }
// std::streamsize n = sizeof(gr_complex) * (d_fft_size); else
// {
// filename.str(""); DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz);
// filename << "../data/code_prn_" << d_gnss_synchro->PRN << ".dat"; DLOG(INFO) << "Error estimating fine frequency Doppler";
// d_dump_file.open(filename.str().c_str(), std::ios::out //debug log
// | std::ios::binary); //
// d_dump_file.write((char*)code_replica, n); //write directly |abs(x)|^2 in this Doppler bin? // std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
// d_dump_file.close(); // std::stringstream filename;
// // std::streamsize n = sizeof(gr_complex) * (d_fft_size);
// filename.str(""); //
// filename << "../data/signal_prn_" << d_gnss_synchro->PRN << ".dat"; // filename.str("");
// d_dump_file.open(filename.str().c_str(), std::ios::out // filename << "../data/code_prn_" << d_gnss_synchro->PRN << ".dat";
// | std::ios::binary); // d_dump_file.open(filename.str().c_str(), std::ios::out
// d_dump_file.write((char*)in, n); //write directly |abs(x)|^2 in this Doppler bin? // | std::ios::binary);
// d_dump_file.close(); // d_dump_file.write((char*)code_replica, n); //write directly |abs(x)|^2 in this Doppler bin?
// // d_dump_file.close();
// //
// n = sizeof(float) * (fft_size_extended); // filename.str("");
// filename.str(""); // filename << "../data/signal_prn_" << d_gnss_synchro->PRN << ".dat";
// filename << "../data/fft_prn_" << d_gnss_synchro->PRN << ".dat"; // d_dump_file.open(filename.str().c_str(), std::ios::out
// d_dump_file.open(filename.str().c_str(), std::ios::out // | std::ios::binary);
// | std::ios::binary); // d_dump_file.write((char*)in, n); //write directly |abs(x)|^2 in this Doppler bin?
// d_dump_file.write((char*)p_tmp_vector, n); //write directly |abs(x)|^2 in this Doppler bin? // d_dump_file.close();
// d_dump_file.close(); //
} //
// n = sizeof(float) * (fft_size_extended);
// filename.str("");
// filename << "../data/fft_prn_" << d_gnss_synchro->PRN << ".dat";
// d_dump_file.open(filename.str().c_str(), std::ios::out
// | std::ios::binary);
// d_dump_file.write((char*)p_tmp_vector, n); //write directly |abs(x)|^2 in this Doppler bin?
// d_dump_file.close();
}
// free memory!! // free memory!!
delete fft_operator; delete fft_operator;
free(code_replica); volk_free(code_replica);
free(p_tmp_vector); volk_free(p_tmp_vector);
return d_fft_size; return d_fft_size;
} }
int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items, int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)
{ {
/*! /*!
* TODO: High sensitivity acquisition algorithm: * TODO: High sensitivity acquisition algorithm:
* State Mechine: * State Mechine:
* S0. StandBy. If d_active==1 -> S1 * S0. StandBy. If d_active==1 -> S1
* S1. ComputeGrid. Perform the FFT acqusition doppler and delay grid. * S1. ComputeGrid. Perform the FFT acqusition doppler and delay grid.
* Accumulate the search grid matrix (#doppler_bins x #fft_size) * Accumulate the search grid matrix (#doppler_bins x #fft_size)
* Compare maximum to threshold and decide positive or negative * Compare maximum to threshold and decide positive or negative
* If T>=gamma -> S4 else * If T>=gamma -> S4 else
* If d_well_count<max_dwells -> S2 * If d_well_count<max_dwells -> S2
* else -> S5. * else -> S5.
* S4. Positive_Acq: Send message and stop acq -> S0 * S4. Positive_Acq: Send message and stop acq -> S0
* S5. Negative_Acq: Send message and stop acq -> S0 * S5. Negative_Acq: Send message and stop acq -> S0
*/ */
switch (d_state) switch (d_state)
{ {
case 0: // S0. StandBy case 0: // S0. StandBy
//DLOG(INFO) <<"S0"<<std::endl; //DLOG(INFO) <<"S0"<<std::endl;
if (d_active==true) if (d_active == true)
{ {
reset_grid(); reset_grid();
d_state=1; d_state = 1;
} }
break; break;
case 1: // S1. ComputeGrid case 1: // S1. ComputeGrid
//DLOG(INFO) <<"S1"<<std::endl; //DLOG(INFO) <<"S1"<<std::endl;
compute_and_accumulate_grid(input_items); compute_and_accumulate_grid(input_items);
d_well_count++; d_well_count++;
if (d_well_count>=d_max_dwells) if (d_well_count >= d_max_dwells)
{ {
d_state=2; d_state = 2;
} }
break; break;
case 2: // Compute test statistics and decide case 2: // Compute test statistics and decide
//DLOG(INFO) <<"S2"<<std::endl; //DLOG(INFO) <<"S2"<<std::endl;
d_input_power=estimate_input_power(input_items); d_input_power = estimate_input_power(input_items);
d_test_statistics=search_maximum(); d_test_statistics = search_maximum();
if (d_test_statistics > d_threshold) if (d_test_statistics > d_threshold)
{ {
d_state=3; //perform fine doppler estimation d_state = 3; //perform fine doppler estimation
}else{ }
d_state=5; //negative acquisition else
} {
break; d_state = 5; //negative acquisition
}
break;
case 3: // Fine doppler estimation case 3: // Fine doppler estimation
//DLOG(INFO) <<"S3"<<std::endl; //DLOG(INFO) <<"S3"<<std::endl;
DLOG(INFO) << "Performing fine Doppler estimation"; DLOG(INFO) << "Performing fine Doppler estimation";
estimate_Doppler(input_items, ninput_items[0]); //disabled in repo estimate_Doppler(input_items, ninput_items[0]); //disabled in repo
d_state=4; d_state = 4;
break; break;
case 4: // Positive_Acq case 4: // Positive_Acq
//DLOG(INFO) <<"S4"<<std::endl; //DLOG(INFO) <<"S4"<<std::endl;
DLOG(INFO) << "positive acquisition"; DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL // Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
d_channel_internal_queue->push(1); // 1-> positive acquisition d_channel_internal_queue->push(1); // 1-> positive acquisition
d_state=0; d_state = 0;
break; break;
case 5: // Negative_Acq case 5: // Negative_Acq
//DLOG(INFO) <<"S5"<<std::endl; //DLOG(INFO) <<"S5"<<std::endl;
DLOG(INFO) << "negative acquisition"; DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter; DLOG(INFO) << "sample_stamp " << d_sample_counter;
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL // Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
d_channel_internal_queue->push(2); // 2-> negative acquisition d_channel_internal_queue->push(2); // 2-> negative acquisition
d_state=0; d_state = 0;
break; break;
default: default:
d_state=0; d_state = 0;
break; break;
} }
//DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl; //DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl;
d_sample_counter += d_fft_size; // sample counter d_sample_counter += d_fft_size; // sample counter
consume_each(d_fft_size); consume_each(d_fft_size);
return 0; return 0;
} }

53
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
View File

@@ -82,9 +82,8 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_state = 0; d_state = 0;
d_disable_assist = false; d_disable_assist = false;
//todo: do something if posix_memalign fails d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_carrier, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_carrier = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -120,8 +119,8 @@ void pcps_assisted_acquisition_cc::free_grid_memory()
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc() pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
{ {
free(d_carrier); volk_free(d_carrier);
free(d_fft_codes); volk_free(d_fft_codes);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
if (d_dump) if (d_dump)
@@ -150,7 +149,7 @@ void pcps_assisted_acquisition_cc::init()
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
@@ -214,7 +213,7 @@ void pcps_assisted_acquisition_cc::redefine_grid()
d_doppler_min = d_config_doppler_min; d_doppler_min = d_config_doppler_min;
} }
// Create the search grid array // Create the search grid array
d_num_doppler_points = floor(std::abs(d_doppler_max-d_doppler_min)/d_doppler_step); d_num_doppler_points = floor(std::abs(d_doppler_max - d_doppler_min) / d_doppler_step);
d_grid_data = new float*[d_num_doppler_points]; d_grid_data = new float*[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++) for (int i = 0; i < d_num_doppler_points; i++)
@@ -231,7 +230,7 @@ void pcps_assisted_acquisition_cc::redefine_grid()
doppler_hz = d_doppler_min + d_doppler_step*doppler_index; doppler_hz = d_doppler_min + d_doppler_step*doppler_index;
// doppler search steps // doppler search steps
// compute the carrier doppler wipe-off signal and store it // compute the carrier doppler wipe-off signal and store it
phase_step_rad = (float)GPS_TWO_PI*doppler_hz / (float)d_fs_in; phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size]; d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size];
fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, 0, phase_step_rad); fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, 0, phase_step_rad);
} }
@@ -259,15 +258,15 @@ double pcps_assisted_acquisition_cc::search_maximum()
} }
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
magt = magt / (fft_normalization_factor * fft_normalization_factor); magt = magt / (fft_normalization_factor * fft_normalization_factor);
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
d_test_statistics = 2 * d_fft_size * magt /(d_input_power*d_well_count); d_test_statistics = 2 * d_fft_size * magt / (d_input_power * d_well_count);
// 4- record the maximum peak and the associated synchronization parameters // 4- record the maximum peak and the associated synchronization parameters
d_gnss_synchro->Acq_delay_samples = (double)index_time; d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
d_gnss_synchro->Acq_doppler_hz = (double)(index_doppler*d_doppler_step + d_doppler_min); d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step + d_doppler_min);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
// Record results to file if required // Record results to file if required
@@ -293,15 +292,15 @@ float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_st
{ {
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
float* p_tmp_vector; float* p_tmp_vector = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
volk_32fc_magnitude_squared_32f_u(p_tmp_vector, in, d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, in, d_fft_size);
const float* p_const_tmp_vector = p_tmp_vector; const float* p_const_tmp_vector = p_tmp_vector;
float power; float power;
volk_32f_accumulator_s32f_a(&power, p_const_tmp_vector, d_fft_size); volk_32f_accumulator_s32f(&power, p_const_tmp_vector, d_fft_size);
free(p_tmp_vector); volk_free(p_tmp_vector);
return ( power / (float)d_fft_size); return ( power / static_cast<float>(d_fft_size));
} }
@@ -319,33 +318,35 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
float* p_tmp_vector; float* p_tmp_vector = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
// doppler search steps // doppler search steps
// Perform the carrier wipe-off // Perform the carrier wipe-off
volk_32fc_x2_multiply_32fc_u(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal // Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute(); d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// save the grid matrix delay file // save the grid matrix delay file
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float* old_vector = d_grid_data[doppler_index]; const float* old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f_a(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size); volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
} }
free(p_tmp_vector); volk_free(p_tmp_vector);
return d_fft_size; return d_fft_size;
} }
int pcps_assisted_acquisition_cc::general_work(int noutput_items, int pcps_assisted_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)

98
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc
View File

@@ -86,14 +86,13 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
//todo: do something if posix_memalign fails d_fft_code_data = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_code_data, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_code_pilot = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_code_pilot, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_data_correlation = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_data_correlation, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_pilot_correlation = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_pilot_correlation, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_correlation_plus = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_correlation_plus, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_correlation_minus = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_correlation_minus, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -112,18 +111,18 @@ pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_code_data); volk_free(d_fft_code_data);
free(d_fft_code_pilot); volk_free(d_fft_code_pilot);
free(d_data_correlation); volk_free(d_data_correlation);
free(d_pilot_correlation); volk_free(d_pilot_correlation);
free(d_correlation_plus); volk_free(d_correlation_plus);
free(d_correlation_minus); volk_free(d_correlation_minus);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@@ -134,38 +133,24 @@ pcps_cccwsr_acquisition_cc::~pcps_cccwsr_acquisition_cc()
} }
} }
void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> * code_data, void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float>* code_data,
std::complex<float> * code_pilot) std::complex<float>* code_pilot)
{ {
// Data code (E1B) // Data code (E1B)
memcpy(d_fft_if->get_inbuf(), code_data, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code_data, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size);
}
// Pilot code (E1C) // Pilot code (E1C)
memcpy(d_fft_if->get_inbuf(), code_pilot, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code_pilot, sizeof(gr_complex) * d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code, //Conjugate the local code,
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_code_pilot, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_code_pilot,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_code_pilot,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void pcps_cccwsr_acquisition_cc::init() void pcps_cccwsr_acquisition_cc::init()
@@ -178,8 +163,8 @@ void pcps_cccwsr_acquisition_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= (int)d_doppler_max; doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
@@ -187,12 +172,11 @@ void pcps_cccwsr_acquisition_cc::init()
// Create the carrier Doppler wipeoff signals // Create the carrier Doppler wipeoff signals
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
} }
@@ -239,7 +223,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
float magt_plus = 0.0; float magt_plus = 0.0;
float magt_minus = 0.0; float magt_minus = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_sample_counter += d_fft_size; // sample counter d_sample_counter += d_fft_size; // sample counter
@@ -252,18 +236,18 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@@ -273,7 +257,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd data code reference (E1B) using SIMD operations // with the local FFT'd data code reference (E1B) using SIMD operations
// with VOLK library // with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_data, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_data, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
@@ -286,7 +270,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd pilot code reference (E1C) using SIMD operations // with the local FFT'd pilot code reference (E1C) using SIMD operations
// with VOLK library // with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_code_pilot, d_fft_size); d_fft_if->get_outbuf(), d_fft_code_pilot, d_fft_size);
// Compute the inverse FFT // Compute the inverse FFT
@@ -307,12 +291,12 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
d_data_correlation[i].imag() - d_pilot_correlation[i].real()); d_data_correlation[i].imag() - d_pilot_correlation[i].real());
} }
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_correlation_plus, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_plus, d_fft_size);
volk_32f_index_max_16u_a(&indext_plus, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_plus, d_magnitude, d_fft_size);
magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor); magt_plus = d_magnitude[indext_plus] / (fft_normalization_factor * fft_normalization_factor);
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_correlation_minus, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_minus, d_fft_size);
volk_32f_index_max_16u_a(&indext_minus, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext_minus, d_magnitude, d_fft_size);
magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor); magt_minus = d_magnitude[indext_minus] / (fft_normalization_factor * fft_normalization_factor);
if (magt_plus >= magt_minus) if (magt_plus >= magt_minus)
@@ -330,8 +314,8 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
if (d_mag < magt) if (d_mag < magt)
{ {
d_mag = magt; d_mag = magt;
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
} }

46
src/algorithms/acquisition/gnuradio_blocks/pcps_multithread_acquisition_cc.cc
View File

@@ -92,12 +92,10 @@ pcps_multithread_acquisition_cc::pcps_multithread_acquisition_cc(
//todo: do something if posix_memalign fails //todo: do something if posix_memalign fails
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
if (posix_memalign((void**)&d_in_buffer[i], 16, d_in_buffer[i] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){};
} }
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -116,19 +114,19 @@ pcps_multithread_acquisition_cc::~pcps_multithread_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
free(d_in_buffer[i]); volk_free(d_in_buffer[i]);
} }
delete[] d_in_buffer; delete[] d_in_buffer;
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@@ -160,10 +158,9 @@ void pcps_multithread_acquisition_cc::init()
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; int doppler = -(int)d_doppler_max + d_doppler_step * doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
} }
@@ -176,14 +173,7 @@ void pcps_multithread_acquisition_cc::set_local_code(std::complex<float> * code)
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(),d_fft_size);
}
} }
void pcps_multithread_acquisition_cc::acquisition_core() void pcps_multithread_acquisition_cc::acquisition_core()
@@ -208,18 +198,18 @@ void pcps_multithread_acquisition_cc::acquisition_core()
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= (float)d_fft_size;
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@@ -228,15 +218,15 @@ void pcps_multithread_acquisition_cc::acquisition_core()
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);

134
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc
View File

@@ -105,7 +105,7 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
d_well_count = 0; d_well_count = 0;
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
d_fft_size = d_sampled_ms * d_samples_per_ms; d_fft_size = d_sampled_ms * d_samples_per_ms;
d_fft_size_pow2 = pow(2, ceil(log2(2*d_fft_size))); d_fft_size_pow2 = pow(2, ceil(log2(2 * d_fft_size)));
d_mag = 0; d_mag = 0;
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
@@ -114,17 +114,13 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
d_cl_fft_batch_size = 1; d_cl_fft_batch_size = 1;
d_in_buffer = new gr_complex*[d_max_dwells]; d_in_buffer = new gr_complex*[d_max_dwells];
//todo: do something if posix_memalign fails
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
if (posix_memalign((void**)&d_in_buffer[i], 16, d_in_buffer[i] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){};
} }
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){}; d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size_pow2 * sizeof(gr_complex)) == 0){}; d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size_pow2 * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_zero_vector, 16, (d_fft_size_pow2-d_fft_size) * sizeof(gr_complex)) == 0){}; d_zero_vector = static_cast<gr_complex*>(volk_malloc((d_fft_size_pow2 - d_fft_size) * sizeof(gr_complex), volk_get_alignment()));
for (unsigned int i = 0; i < (d_fft_size_pow2-d_fft_size); i++) for (unsigned int i = 0; i < (d_fft_size_pow2-d_fft_size); i++)
{ {
@@ -156,20 +152,20 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
for (unsigned int i = 0; i < d_max_dwells; i++) for (unsigned int i = 0; i < d_max_dwells; i++)
{ {
free(d_in_buffer[i]); volk_free(d_in_buffer[i]);
} }
delete[] d_in_buffer; delete[] d_in_buffer;
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
free(d_zero_vector); volk_free(d_zero_vector);
if (d_opencl == 0) if (d_opencl == 0)
{ {
@@ -302,8 +298,8 @@ void pcps_opencl_acquisition_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= (int)d_doppler_max; doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
@@ -318,10 +314,9 @@ void pcps_opencl_acquisition_cc::init()
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler= -(int)d_doppler_max + d_doppler_step*doppler_index; int doppler= -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
@@ -347,44 +342,37 @@ void pcps_opencl_acquisition_cc::init()
void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> * code) void pcps_opencl_acquisition_cc::set_local_code(std::complex<float> * code)
{ {
if(d_opencl == 0) if(d_opencl == 0)
{ {
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0, d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0,
sizeof(gr_complex)*d_fft_size, code); sizeof(gr_complex)*d_fft_size, code);
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)*d_fft_size, d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)*d_fft_size,
sizeof(gr_complex)*(d_fft_size_pow2 - 2*d_fft_size), sizeof(gr_complex)*(d_fft_size_pow2 - 2*d_fft_size),
d_zero_vector); d_zero_vector);
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex) d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)
*(d_fft_size_pow2 - d_fft_size), *(d_fft_size_pow2 - d_fft_size),
sizeof(gr_complex)*d_fft_size, code); sizeof(gr_complex)*d_fft_size, code);
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size, clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(), clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
0, NULL, NULL); 0, NULL, NULL);
//Conjucate the local code //Conjucate the local code
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector"); cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
kernel.setArg(0, *d_cl_buffer_2); //input kernel.setArg(0, *d_cl_buffer_2); //input
kernel.setArg(1, *d_cl_buffer_fft_codes); //output kernel.setArg(1, *d_cl_buffer_fft_codes); //output
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange); d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
} }
else else
{ {
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size); memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size);
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{ }
volk_32fc_conjugate_32fc_u(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
}
} }
void pcps_opencl_acquisition_cc::acquisition_core_volk() void pcps_opencl_acquisition_cc::acquisition_core_volk()
@@ -393,7 +381,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
int doppler; int doppler;
unsigned int indext = 0; unsigned int indext = 0;
float magt = 0.0; float magt = 0.0;
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
gr_complex* in = d_in_buffer[d_well_count]; gr_complex* in = d_in_buffer[d_well_count];
unsigned long int samplestamp = d_sample_counter_buffer[d_well_count]; unsigned long int samplestamp = d_sample_counter_buffer[d_well_count];
@@ -409,19 +397,17 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@@ -430,15 +416,15 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Search maximum // Search maximum
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
@@ -457,8 +443,8 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
// restarted between consecutive dwells in multidwell operation. // restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{ {
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samplestamp; d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
@@ -517,7 +503,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
int doppler; int doppler;
unsigned int indext = 0; unsigned int indext = 0;
float magt = 0.0; float magt = 0.0;
float fft_normalization_factor = ((float)d_fft_size_pow2 * (float)d_fft_size); //This works, but I am not sure why. float fft_normalization_factor = (static_cast<float>(d_fft_size_pow2) * static_cast<float>(d_fft_size)); //This works, but I am not sure why.
gr_complex* in = d_in_buffer[d_well_count]; gr_complex* in = d_in_buffer[d_well_count];
unsigned long int samplestamp = d_sample_counter_buffer[d_well_count]; unsigned long int samplestamp = d_sample_counter_buffer[d_well_count];
@@ -543,9 +529,9 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= static_cast<float>(d_fft_size);
cl::Kernel kernel; cl::Kernel kernel;
@@ -554,7 +540,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
{ {
// doppler search steps // doppler search steps
doppler = -(int)d_doppler_max + d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step*doppler_index;
//Multiply input signal with doppler wipe-off //Multiply input signal with doppler wipe-off
kernel = cl::Kernel(d_cl_program, "mult_vectors"); kernel = cl::Kernel(d_cl_program, "mult_vectors");
@@ -600,7 +586,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
// Search maximum // Search maximum
// @TODO: find an efficient way to search the maximum with OpenCL in the GPU. // @TODO: find an efficient way to search the maximum with OpenCL in the GPU.
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude, d_fft_size);
// Normalize the maximum value to correct the scale factor introduced by FFTW // Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
@@ -619,8 +605,8 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
// restarted between consecutive dwells in multidwell operation. // restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag)
{ {
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samplestamp; d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
// 5- Compute the test statistics and compare to the threshold // 5- Compute the test statistics and compare to the threshold
@@ -716,7 +702,7 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
// Fill internal buffer with d_max_dwells signal blocks. This step ensures that // Fill internal buffer with d_max_dwells signal blocks. This step ensures that
// consecutive signal blocks will be processed in multi-dwell operation. This is // consecutive signal blocks will be processed in multi-dwell operation. This is
// essential when d_bit_transition_flag = true. // essential when d_bit_transition_flag = true.
unsigned int num_dwells = std::min((int)(d_max_dwells-d_in_dwell_count), ninput_items[0]); unsigned int num_dwells = std::min(static_cast<int>(d_max_dwells-d_in_dwell_count), ninput_items[0]);
for (unsigned int i = 0; i < num_dwells; i++) for (unsigned int i = 0; i < num_dwells; i++)
{ {
memcpy(d_in_buffer[d_in_dwell_count++], (gr_complex*)input_items[i], memcpy(d_in_buffer[d_in_dwell_count++], (gr_complex*)input_items[i],
@@ -725,7 +711,7 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
d_sample_counter_buffer.push_back(d_sample_counter); d_sample_counter_buffer.push_back(d_sample_counter);
} }
if (ninput_items[0] > (int)num_dwells) if (ninput_items[0] > static_cast<int>(num_dwells))
{ {
d_sample_counter += d_fft_size * (ninput_items[0] - num_dwells); d_sample_counter += d_fft_size * (ninput_items[0] - num_dwells);
} }

193
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc
View File

@@ -70,9 +70,9 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
bool bit_transition_flag, bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename): std::string dump_filename):
gr::block("pcps_quicksync_acquisition_cc", gr::block("pcps_quicksync_acquisition_cc",
gr::io_signature::make(1, 1, (sizeof(gr_complex)*sampled_ms * samples_per_ms )), gr::io_signature::make(1, 1, (sizeof(gr_complex)*sampled_ms * samples_per_ms )),
gr::io_signature::make(0, 0, (sizeof(gr_complex)*sampled_ms * samples_per_ms ))) gr::io_signature::make(0, 0, (sizeof(gr_complex)*sampled_ms * samples_per_ms )))
{ {
//DLOG(INFO) << "START CONSTRUCTOR"; //DLOG(INFO) << "START CONSTRUCTOR";
@@ -97,20 +97,17 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
//fft size is reduced. //fft size is reduced.
d_fft_size = (d_samples_per_code) / d_folding_factor; d_fft_size = (d_samples_per_code) / d_folding_factor;
d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
//todo: do something if posix_memalign fails d_magnitude = static_cast<float*>(volk_malloc(d_samples_per_code * d_folding_factor * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude_folded = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitude, 16, d_samples_per_code * d_folding_factor * sizeof(float)) == 0){};
if (posix_memalign((void**)&d_magnitude_folded, 16, d_fft_size * sizeof(float)) == 0){};
d_possible_delay = new unsigned int[d_folding_factor]; d_possible_delay = new unsigned int[d_folding_factor];
d_corr_output_f = new float[d_folding_factor]; d_corr_output_f = new float[d_folding_factor];
/*Create the d_code signal , which would store the values of the code in its /*Create the d_code signal , which would store the values of the code in its
original form to perform later correlation in time domain*/ original form to perform later correlation in time domain*/
d_code = new gr_complex[d_samples_per_code](); d_code = new gr_complex[d_samples_per_code]();
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
// Inverse FFT // Inverse FFT
@@ -130,25 +127,21 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
} }
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
free(d_magnitude_folded); volk_free(d_magnitude_folded);
delete d_ifft; delete d_ifft;
d_ifft = NULL;
delete d_fft_if; delete d_fft_if;
d_fft_if = NULL;
delete d_code; delete d_code;
d_code = NULL;
delete d_possible_delay; delete d_possible_delay;
d_possible_delay = NULL;
delete d_corr_output_f; delete d_corr_output_f;
d_corr_output_f = NULL;
if (d_dump) if (d_dump)
{ {
d_dump_file.close(); d_dump_file.close();
@@ -156,44 +149,33 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
// DLOG(INFO) << "END DESTROYER"; // DLOG(INFO) << "END DESTROYER";
} }
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float> * code)
void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float>* code)
{ {
// DLOG(INFO) << "START LOCAL CODE";
/*save a local copy of the code without the folding process to perform corre- /*save a local copy of the code without the folding process to perform corre-
lation in time in the final steps of the acquisition stage*/ lation in time in the final steps of the acquisition stage*/
memcpy(d_code, code, sizeof(gr_complex)*d_samples_per_code); memcpy(d_code, code, sizeof(gr_complex) * d_samples_per_code);
d_code_folded = new gr_complex[d_fft_size](); gr_complex* d_code_folded = new gr_complex[d_fft_size]();
memcpy(d_fft_if->get_inbuf(), d_code_folded, sizeof(gr_complex)*(d_fft_size)); memcpy(d_fft_if->get_inbuf(), d_code_folded, sizeof(gr_complex) * (d_fft_size));
/*perform folding of the code by the factorial factor parameter. Notice that /*perform folding of the code by the factorial factor parameter. Notice that
folding of the code in the time stage would result in a downsampled spectrum folding of the code in the time stage would result in a downsampled spectrum
in the frequency domain after applying the fftw operation*/ in the frequency domain after applying the fftw operation*/
for (unsigned int i = 0; i < d_folding_factor; i++) for (unsigned int i = 0; i < d_folding_factor; i++)
{ {
std::transform ((code + i*d_fft_size), (code + ((i+1)*d_fft_size)) , std::transform ((code + i * d_fft_size), (code + ((i + 1) * d_fft_size)) ,
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(), d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
std::plus<gr_complex>()); std::plus<gr_complex>());
} }
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_codes,d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(), d_fft_size);
}
// DLOG(INFO) << "END LOCAL CODE";
} }
void pcps_quicksync_acquisition_cc::init() void pcps_quicksync_acquisition_cc::init()
{ {
//DLOG(INFO) << "START init"; //DLOG(INFO) << "START init";
@@ -205,8 +187,8 @@ void pcps_quicksync_acquisition_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= (int)(d_doppler_max); doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
@@ -216,10 +198,8 @@ void pcps_quicksync_acquisition_cc::init()
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_get_alignment()));
d_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){}; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_freq + doppler, d_fs_in,
d_samples_per_code * d_folding_factor); d_samples_per_code * d_folding_factor);
@@ -227,6 +207,7 @@ void pcps_quicksync_acquisition_cc::init()
// DLOG(INFO) << "end init"; // DLOG(INFO) << "end init";
} }
int pcps_quicksync_acquisition_cc::general_work(int noutput_items, int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)
@@ -278,27 +259,21 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
float magt = 0.0; float magt = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
gr_complex *in_temp; gr_complex* in_temp = static_cast<gr_complex*>(volk_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&(in_temp), 16,d_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){}; gr_complex* in_temp_folded = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
gr_complex *in_temp_folded;
if (posix_memalign((void**)&(in_temp_folded), 16,d_fft_size * sizeof(gr_complex)) == 0){};
/*Create a signal to store a signal of size 1ms, to perform correlation /*Create a signal to store a signal of size 1ms, to perform correlation
in time. No folding on this data is required*/ in time. No folding on this data is required*/
gr_complex *in_1code; gr_complex* in_1code = static_cast<gr_complex*>(volk_malloc(d_samples_per_code * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&(in_1code), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
/*Stores the values of the correlation output between the local code /*Stores the values of the correlation output between the local code
and the signal with doppler shift corrected */ and the signal with doppler shift corrected */
gr_complex *corr_output; gr_complex* corr_output = static_cast<gr_complex*>(volk_malloc(d_samples_per_code * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&(corr_output), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
/*Stores a copy of the folded version of the signal.This is used for /*Stores a copy of the folded version of the signal.This is used for
the FFT operations in future steps of excecution*/ the FFT operations in future steps of excecution*/
// gr_complex in_folded[d_fft_size]; // gr_complex in_folded[d_fft_size];
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0; d_input_power = 0.0;
d_mag = 0.0; d_mag = 0.0;
@@ -322,40 +297,37 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
/* 1- Compute the input signal power estimation. This operation is /* 1- Compute the input signal power estimation. This operation is
being performed in a signal of size nxp */ being performed in a signal of size nxp */
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_samples_per_code * d_folding_factor); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_samples_per_code * d_folding_factor);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
d_input_power /= (float)(d_samples_per_code * d_folding_factor); d_input_power /= static_cast<float>(d_samples_per_code * d_folding_factor);
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
for (unsigned int doppler_index=0;doppler_index<d_num_doppler_bins;doppler_index++)
{ {
/*Ensure that the signal is going to start with all samples /*Ensure that the signal is going to start with all samples
at zero. This is done to avoid over acumulation when performing at zero. This is done to avoid over acumulation when performing
the folding process to be stored in d_fft_if->get_inbuf()*/ the folding process to be stored in d_fft_if->get_inbuf()*/
d_signal_folded = new gr_complex[d_fft_size](); d_signal_folded = new gr_complex[d_fft_size]();
memcpy( d_fft_if->get_inbuf(),d_signal_folded, memcpy( d_fft_if->get_inbuf(), d_signal_folded, sizeof(gr_complex) * (d_fft_size));
sizeof(gr_complex)*(d_fft_size));
/*Doppler search steps and then multiplication of the incoming /*Doppler search steps and then multiplication of the incoming
signal with the doppler wipeoffs to eliminate frequency offset signal with the doppler wipeoffs to eliminate frequency offset
*/ */
doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
/*Perform multiplication of the incoming signal with the /*Perform multiplication of the incoming signal with the
complex exponential vector. This removes the frequency doppler complex exponential vector. This removes the frequency doppler
shift offset*/ shift offset*/
volk_32fc_x2_multiply_32fc_a(in_temp, in, volk_32fc_x2_multiply_32fc(in_temp, in,
d_grid_doppler_wipeoffs[doppler_index], d_grid_doppler_wipeoffs[doppler_index],
d_samples_per_code * d_folding_factor); d_samples_per_code * d_folding_factor);
/*Perform folding of the carrier wiped-off incoming signal. Since /*Perform folding of the carrier wiped-off incoming signal. Since
superlinear method is being used the folding factor in the superlinear method is being used the folding factor in the
incoming raw data signal is of d_folding_factor^2*/ incoming raw data signal is of d_folding_factor^2*/
for ( int i = 0; i < (int)(d_folding_factor*d_folding_factor); i++) for ( int i = 0; i < static_cast<int>(d_folding_factor * d_folding_factor); i++)
{ {
std::transform ((in_temp+i*d_fft_size), std::transform ((in_temp + i * d_fft_size),
(in_temp+((i+1)*d_fft_size)) , (in_temp + ((i + 1) * d_fft_size)) ,
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
d_fft_if->get_inbuf(), d_fft_if->get_inbuf(),
std::plus<gr_complex>()); std::plus<gr_complex>());
@@ -368,7 +340,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
/*Multiply carrier wiped--off, Fourier transformed incoming /*Multiply carrier wiped--off, Fourier transformed incoming
signal with the local FFT'd code reference using SIMD signal with the local FFT'd code reference using SIMD
operations with VOLK library*/ operations with VOLK library*/
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
/* compute the inverse FFT of the aliased signal*/ /* compute the inverse FFT of the aliased signal*/
@@ -376,16 +348,16 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
/* Compute the magnitude and get the maximum value with its /* Compute the magnitude and get the maximum value with its
index position*/ index position*/
volk_32fc_magnitude_squared_32f_a(d_magnitude_folded, volk_32fc_magnitude_squared_32f(d_magnitude_folded,
d_ifft->get_outbuf(), d_fft_size); d_ifft->get_outbuf(), d_fft_size);
/* Normalize the maximum value to correct the scale factor /* Normalize the maximum value to correct the scale factor
introduced by FFTW*/ introduced by FFTW*/
//volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded, //volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded,
// (1 / (fft_normalization_factor * fft_normalization_factor)), d_fft_size); // (1 / (fft_normalization_factor * fft_normalization_factor)), d_fft_size);
volk_32f_index_max_16u_a(&indext, d_magnitude_folded, d_fft_size); volk_32f_index_max_16u(&indext, d_magnitude_folded, d_fft_size);
magt = d_magnitude_folded[indext]/ (fft_normalization_factor * fft_normalization_factor); magt = d_magnitude_folded[indext] / (fft_normalization_factor * fft_normalization_factor);
delete d_signal_folded; delete d_signal_folded;
@@ -405,88 +377,62 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
{ {
unsigned int detected_delay_samples_folded = 0; unsigned int detected_delay_samples_folded = 0;
detected_delay_samples_folded = (indext % d_samples_per_code); detected_delay_samples_folded = (indext % d_samples_per_code);
//float d_corr_output_f[d_folding_factor];
gr_complex complex_acumulator[100]; gr_complex complex_acumulator[100];
//gr_complex complex_acumulator[d_folding_factor]; //gr_complex complex_acumulator[d_folding_factor];
//const int ff = d_folding_factor;
//gr_complex complex_acumulator[ff];
//gr_complex complex_acumulator[];
//complex_acumulator = new gr_complex[d_folding_factor]();
for (int i = 0; i < (int)d_folding_factor; i++) for (int i = 0; i < static_cast<int>(d_folding_factor); i++)
{ {
d_possible_delay[i]= detected_delay_samples_folded+ d_possible_delay[i] = detected_delay_samples_folded + (i) * d_fft_size;
(i)*d_fft_size;
} }
for ( int i = 0; i < (int)d_folding_factor; i++) for ( int i = 0; i < static_cast<int>(d_folding_factor); i++)
{ {
/*Copy a signal of 1 code length into suggested buffer. /*Copy a signal of 1 code length into suggested buffer.
The copied signal must have doppler effect corrected*/ The copied signal must have doppler effect corrected*/
memcpy(in_1code,&in_temp[d_possible_delay[i]], memcpy(in_1code,&in_temp[d_possible_delay[i]],
sizeof(gr_complex)*(d_samples_per_code)); sizeof(gr_complex) * (d_samples_per_code));
/*Perform multiplication of the unmodified local /*Perform multiplication of the unmodified local
generated code with the incoming signal with doppler generated code with the incoming signal with doppler
effect corrected and accumulates its value. This effect corrected and accumulates its value. This
is indeed correlation in time for an specific value is indeed correlation in time for an specific value
of a shift*/ of a shift*/
volk_32fc_x2_multiply_32fc_a(corr_output, in_1code, volk_32fc_x2_multiply_32fc(corr_output, in_1code, d_code, d_samples_per_code);
d_code, d_samples_per_code);
for(int j=0; j < (d_samples_per_code); j++) for(int j = 0; j < d_samples_per_code; j++)
{ {
complex_acumulator[i] += (corr_output[j]); complex_acumulator[i] += (corr_output[j]);
} }
} }
/*Obtain maximun value of correlation given the /*Obtain maximun value of correlation given the possible delay selected */
possible delay selected */ volk_32fc_magnitude_squared_32f(d_corr_output_f, complex_acumulator, d_folding_factor);
volk_32fc_magnitude_squared_32f_a(d_corr_output_f, volk_32f_index_max_16u(&indext, d_corr_output_f, d_folding_factor);
complex_acumulator, d_folding_factor);
volk_32f_index_max_16u_a(&indext, d_corr_output_f,
d_folding_factor);
/*Now save the real code phase in the gnss_syncro /*Now save the real code phase in the gnss_syncro block for use in other stages*/
block for use in other stages*/ d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
d_gnss_synchro->Acq_delay_samples = (double) d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
(d_possible_delay[indext]);
d_gnss_synchro->Acq_doppler_hz = (double)doppler;
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
/* 5- Compute the test statistics and compare to the threshold d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
/* 5- Compute the test statistics and compare to the threshold
d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
d_test_statistics = d_mag / d_input_power; d_test_statistics = d_mag / d_input_power;
//delete complex_acumulator; //delete complex_acumulator;
} }
} }
// Record results to file if required // Record results to file if required
if (d_dump) if (d_dump)
{ {
/*
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_ifft->get_outbuf(), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
*/
/*Since QuickSYnc performs a folded correlation in frequency by means /*Since QuickSYnc performs a folded correlation in frequency by means
of the FFT, it is esential to also keep the values obtained from the of the FFT, it is esential to also keep the values obtained from the
possible delay to show how it is maximize*/ possible delay to show how it is maximize*/
std::stringstream filename; std::stringstream filename;
std::streamsize n = sizeof(float) * (d_fft_size); // complex file write std::streamsize n = sizeof(float) * (d_fft_size); // complex file write
filename.str(""); filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System filename << "../data/test_statistics_" << d_gnss_synchro->System
<<"_" << d_gnss_synchro->Signal << "_sat_" <<"_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat"; << d_gnss_synchro->PRN << "_doppler_" << doppler << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write((char*)d_magnitude_folded, n); //write directly |abs(x)|^2 in this Doppler bin? d_dump_file.write((char*)d_magnitude_folded, n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close(); d_dump_file.close();
@@ -521,15 +467,12 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
} }
} }
volk_free(in_temp);
volk_free(in_temp_folded);
volk_free(in_1code);
volk_free(corr_output);
consume_each(1); consume_each(1);
delete d_code_folded;
d_code_folded = NULL;
free(in_temp);
free(in_1code);
free(corr_output);
break; break;
} }
@@ -543,8 +486,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
DLOG(INFO) << "test statistics value " << d_test_statistics; DLOG(INFO) << "test statistics value " << d_test_statistics;
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor " << d_folding_factor; DLOG(INFO) << "folding factor " << d_folding_factor;
DLOG(INFO) << "possible delay correlation output"; DLOG(INFO) << "possible delay correlation output";
for (int i = 0; i < (int)d_folding_factor; i++) DLOG(INFO) << d_possible_delay[i] <<"\t\t\t"<<d_corr_output_f[i]; for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag; DLOG(INFO) << "magnitude folded " << d_mag;
@@ -573,7 +516,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
DLOG(INFO) << "test statistics threshold " << d_threshold; DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "folding factor "<<d_folding_factor; DLOG(INFO) << "folding factor "<<d_folding_factor;
DLOG(INFO) << "possible delay corr output"; DLOG(INFO) << "possible delay corr output";
for (int i = 0; i < (int)d_folding_factor; i++) DLOG(INFO) << d_possible_delay[i] <<"\t\t\t"<<d_corr_output_f[i]; for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "magnitude folded " << d_mag; DLOG(INFO) << "magnitude folded " << d_mag;

12
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.h
View File

@@ -112,12 +112,12 @@ private:
gr_complex* d_code; gr_complex* d_code;
unsigned int d_folding_factor; // also referred in the paper as 'p' unsigned int d_folding_factor; // also referred in the paper as 'p'
float * d_corr_acumulator; float* d_corr_acumulator;
unsigned int *d_possible_delay; unsigned int* d_possible_delay;
float *d_corr_output_f; float* d_corr_output_f;
float * d_magnitude_folded; float* d_magnitude_folded;
gr_complex *d_signal_folded; gr_complex* d_signal_folded;
gr_complex *d_code_folded; gr_complex* d_code_folded;
float d_noise_floor_power; float d_noise_floor_power;
long d_fs_in; long d_fs_in;

60
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc
View File

@@ -99,9 +99,8 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
d_input_power = 0.0; d_input_power = 0.0;
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
//todo: do something if posix_memalign fails d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){}; d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
// Direct FFT // Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true); d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -120,15 +119,15 @@ pcps_tong_acquisition_cc::~pcps_tong_acquisition_cc()
{ {
for (unsigned int i = 0; i < d_num_doppler_bins; i++) for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{ {
free(d_grid_doppler_wipeoffs[i]); volk_free(d_grid_doppler_wipeoffs[i]);
free(d_grid_data[i]); volk_free(d_grid_data[i]);
} }
delete[] d_grid_doppler_wipeoffs; delete[] d_grid_doppler_wipeoffs;
delete[] d_grid_data; delete[] d_grid_data;
} }
free(d_fft_codes); volk_free(d_fft_codes);
free(d_magnitude); volk_free(d_magnitude);
delete d_ifft; delete d_ifft;
delete d_fft_if; delete d_fft_if;
@@ -146,14 +145,7 @@ void pcps_tong_acquisition_cc::set_local_code(std::complex<float> * code)
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
if (is_unaligned()) volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
{
volk_32fc_conjugate_32fc_u(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
} }
void pcps_tong_acquisition_cc::init() void pcps_tong_acquisition_cc::init()
@@ -166,8 +158,8 @@ void pcps_tong_acquisition_cc::init()
// Count the number of bins // Count the number of bins
d_num_doppler_bins = 0; d_num_doppler_bins = 0;
for (int doppler = (int)(-d_doppler_max); for (int doppler = static_cast<int>(-d_doppler_max);
doppler <= (int)d_doppler_max; doppler <= static_cast<int>(d_doppler_max);
doppler += d_doppler_step) doppler += d_doppler_step)
{ {
d_num_doppler_bins++; d_num_doppler_bins++;
@@ -178,16 +170,14 @@ void pcps_tong_acquisition_cc::init()
d_grid_data = new float*[d_num_doppler_bins]; d_grid_data = new float*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16, d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
d_fft_size * sizeof(gr_complex)) == 0){};
int doppler=-(int)d_doppler_max+d_doppler_step*doppler_index; int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index], complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
d_freq + doppler, d_fs_in, d_fft_size); d_freq + doppler, d_fs_in, d_fft_size);
if (posix_memalign((void**)&(d_grid_data[doppler_index]), 16, d_grid_data[doppler_index] = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
d_fft_size * sizeof(float)) == 0){};
for (unsigned int i = 0; i < d_fft_size; i++) for (unsigned int i = 0; i < d_fft_size; i++)
{ {
@@ -242,7 +232,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
unsigned int indext = 0; unsigned int indext = 0;
float magt = 0.0; float magt = 0.0;
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
float fft_normalization_factor = (float)d_fft_size * (float)d_fft_size; float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
d_input_power = 0.0; d_input_power = 0.0;
d_mag = 0.0; d_mag = 0.0;
@@ -257,18 +247,18 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
<< ", doppler_step: " << d_doppler_step; << ", doppler_step: " << d_doppler_step;
// 1- Compute the input signal power estimation // 1- Compute the input signal power estimation
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size); volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
d_input_power /= (float)d_fft_size; d_input_power /= static_cast<float>(d_fft_size);
// 2- Doppler frequency search loop // 2- Doppler frequency search loop
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{ {
// doppler search steps // doppler search steps
doppler = -(int)d_doppler_max + d_doppler_step*doppler_index; doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in, volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in,
d_grid_doppler_wipeoffs[doppler_index], d_fft_size); d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search) // 3- Perform the FFT-based convolution (parallel time search)
@@ -277,25 +267,25 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
// Multiply carrier wiped--off, Fourier transformed incoming signal // Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library // with the local FFT'd code reference using SIMD operations with VOLK library
volk_32fc_x2_multiply_32fc_a(d_ifft->get_inbuf(), volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(),
d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT // compute the inverse FFT
d_ifft->execute(); d_ifft->execute();
// Compute magnitude // Compute magnitude
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
// Compute vector of test statistics corresponding to current doppler index. // Compute vector of test statistics corresponding to current doppler index.
volk_32f_s32f_multiply_32f_a(d_magnitude, d_magnitude, volk_32f_s32f_multiply_32f(d_magnitude, d_magnitude,
1/(fft_normalization_factor*fft_normalization_factor*d_input_power), 1/(fft_normalization_factor*fft_normalization_factor*d_input_power),
d_fft_size); d_fft_size);
// Accumulate test statistics in d_grid_data. // Accumulate test statistics in d_grid_data.
volk_32f_x2_add_32f_a(d_grid_data[doppler_index], d_magnitude, d_grid_data[doppler_index], d_fft_size); volk_32f_x2_add_32f(d_grid_data[doppler_index], d_magnitude, d_grid_data[doppler_index], d_fft_size);
// Search maximum // Search maximum
volk_32f_index_max_16u_a(&indext, d_grid_data[doppler_index], d_fft_size); volk_32f_index_max_16u(&indext, d_grid_data[doppler_index], d_fft_size);
magt = d_grid_data[doppler_index][indext]; magt = d_grid_data[doppler_index][indext];
@@ -303,8 +293,8 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
if (d_mag < magt) if (d_mag < magt)
{ {
d_mag = magt; d_mag = magt;
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code); d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = (double)doppler; d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter; d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
} }

9
src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_cc.cc
View File

@@ -60,15 +60,14 @@ direct_resampler_conditioner_cc::direct_resampler_conditioner_cc(
d_history(1) d_history(1)
{ {
// Computes the phase step multiplying the resampling ratio by 2^32 = 4294967296 // Computes the phase step multiplying the resampling ratio by 2^32 = 4294967296
const double two_32 = 4294967296.0;
if (d_sample_freq_in >= d_sample_freq_out) if (d_sample_freq_in >= d_sample_freq_out)
{ {
d_phase_step = (unsigned int)floor((double)4294967296.0 d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_out / sample_freq_in));
* sample_freq_out / sample_freq_in);
} }
else else
{ {
d_phase_step = (unsigned int)floor((double)4294967296.0 d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_in / sample_freq_out));
* sample_freq_in / sample_freq_out);
} }
set_relative_rate(1.0 * sample_freq_out / sample_freq_in); set_relative_rate(1.0 * sample_freq_out / sample_freq_in);
set_output_multiple(1); set_output_multiple(1);
@@ -87,7 +86,7 @@ direct_resampler_conditioner_cc::~direct_resampler_conditioner_cc()
void direct_resampler_conditioner_cc::forecast(int noutput_items, void direct_resampler_conditioner_cc::forecast(int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
int nreqd = std::max((unsigned)1, (int)((double)(noutput_items + 1) int nreqd = std::max(static_cast<unsigned>(1), static_cast<int>(static_cast<double>(noutput_items + 1)
* sample_freq_in() / sample_freq_out()) + history() - 1); * sample_freq_in() / sample_freq_out()) + history() - 1);
unsigned ninputs = ninput_items_required.size(); unsigned ninputs = ninput_items_required.size();
for (unsigned i = 0; i < ninputs; i++) for (unsigned i = 0; i < ninputs; i++)

10
src/algorithms/resampler/gnuradio_blocks/direct_resampler_conditioner_ss.cc
View File

@@ -58,17 +58,15 @@ direct_resampler_conditioner_ss::direct_resampler_conditioner_ss(
d_sample_freq_in(sample_freq_in), d_sample_freq_out( d_sample_freq_in(sample_freq_in), d_sample_freq_out(
sample_freq_out), d_phase(0), d_lphase(0), d_history(1) sample_freq_out), d_phase(0), d_lphase(0), d_history(1)
{ {
const double two_32 = 4294967296.0;
// Computes the phase step multiplying the resampling ratio by 2^32 = 4294967296 // Computes the phase step multiplying the resampling ratio by 2^32 = 4294967296
if (d_sample_freq_in >= d_sample_freq_out) if (d_sample_freq_in >= d_sample_freq_out)
{ {
d_phase_step = (unsigned int)floor((double)4294967296.0 d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_out / sample_freq_in));
* sample_freq_out / sample_freq_in);
} }
else else
{ {
d_phase_step = (unsigned int)floor((double)4294967296.0 d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_in / sample_freq_out));
* sample_freq_in / sample_freq_out);
} }
set_relative_rate(1.0 * sample_freq_out / sample_freq_in); set_relative_rate(1.0 * sample_freq_out / sample_freq_in);
@@ -84,7 +82,7 @@ void direct_resampler_conditioner_ss::forecast(int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
int nreqd = std::max((unsigned)1, (int)((double)(noutput_items + 1) int nreqd = std::max(static_cast<unsigned>(1), (int)(static_cast<double>(noutput_items + 1)
* sample_freq_in() / sample_freq_out()) + history() - 1); * sample_freq_in() / sample_freq_out()) + history() - 1);
unsigned ninputs = ninput_items_required.size(); unsigned ninputs = ninput_items_required.size();

151
src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc
View File

@@ -28,16 +28,18 @@
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include "signal_generator_c.h"
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <volk/volk.h> #include <volk/volk.h>
#include "signal_generator_c.h"
#include "gps_sdr_signal_processing.h" #include "gps_sdr_signal_processing.h"
#include "galileo_e1_signal_processing.h" #include "galileo_e1_signal_processing.h"
#include "nco_lib.h" #include "nco_lib.h"
#include "galileo_e5_signal_processing.h" #include "galileo_e5_signal_processing.h"
#include "Galileo_E5a.h" #include "Galileo_E5a.h"
#include <iostream>
#include <fstream>
/* /*
* Create a new instance of signal_generator_c and return * Create a new instance of signal_generator_c and return
* a boost shared_ptr. This is effectively the public constructor. * a boost shared_ptr. This is effectively the public constructor.
@@ -61,7 +63,7 @@ signal_generator_c::signal_generator_c (std::vector<std::string> signal1, std::v
unsigned int fs_in, unsigned int vector_length, float BW_BB) : unsigned int fs_in, unsigned int vector_length, float BW_BB) :
gr::block ("signal_gen_cc", gr::io_signature::make(0, 0, sizeof(gr_complex)), gr::block ("signal_gen_cc", gr::io_signature::make(0, 0, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(gr_complex)*vector_length)), gr::io_signature::make(1, 1, sizeof(gr_complex) * vector_length)),
signal_(signal1), signal_(signal1),
system_(system), system_(system),
PRN_(PRN), PRN_(PRN),
@@ -74,7 +76,7 @@ signal_generator_c::signal_generator_c (std::vector<std::string> signal1, std::v
fs_in_(fs_in), fs_in_(fs_in),
num_sats_(PRN.size()), num_sats_(PRN.size()),
vector_length_(vector_length), vector_length_(vector_length),
BW_BB_(BW_BB*(float)fs_in/2.0) BW_BB_(BW_BB * static_cast<float>(fs_in) / 2.0)
{ {
init(); init();
generate_codes(); generate_codes();
@@ -84,10 +86,10 @@ void signal_generator_c::init()
{ {
work_counter_ = 0; work_counter_ = 0;
if (posix_memalign((void**)&complex_phase_, 16, vector_length_ * sizeof(gr_complex)) == 0){}; complex_phase_ = static_cast<gr_complex*>(volk_malloc(vector_length_ * sizeof(gr_complex), volk_get_alignment()));
// True if Galileo satellites are present // True if Galileo satellites are present
bool gallileo_signal = std::find(system_.begin(), system_.end(), "E") != system_.end(); bool galileo_signal = std::find(system_.begin(), system_.end(), "E") != system_.end();
for (unsigned int sat = 0; sat < num_sats_; sat++) for (unsigned int sat = 0; sat < num_sats_; sat++)
{ {
@@ -95,24 +97,23 @@ void signal_generator_c::init()
current_data_bit_int_.push_back(1); current_data_bit_int_.push_back(1);
current_data_bits_.push_back(gr_complex(1, 0)); current_data_bits_.push_back(gr_complex(1, 0));
ms_counter_.push_back(0); ms_counter_.push_back(0);
data_modulation_.push_back((Galileo_E5a_I_SECONDARY_CODE.at(0)=='0' ? 1 : -1)); data_modulation_.push_back((Galileo_E5a_I_SECONDARY_CODE.at(0) == '0' ? 1 : -1));
pilot_modulation_.push_back((Galileo_E5a_Q_SECONDARY_CODE[PRN_[sat]].at(0)=='0' ? 1 : -1)); pilot_modulation_.push_back((Galileo_E5a_Q_SECONDARY_CODE[PRN_[sat]].at(0) == '0' ? 1 : -1));
if (system_[sat] == "G") if (system_[sat] == "G")
{ {
samples_per_code_.push_back(round((float)fs_in_ samples_per_code_.push_back(round(static_cast<float>(fs_in_)
/ (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS))); / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)));
num_of_codes_per_vector_.push_back(gallileo_signal ? 4*(int)Galileo_E1_C_SECONDARY_CODE_LENGTH : 1); num_of_codes_per_vector_.push_back(galileo_signal ? 4 * static_cast<int>(Galileo_E1_C_SECONDARY_CODE_LENGTH) : 1);
data_bit_duration_ms_.push_back(1e3/GPS_CA_TELEMETRY_RATE_BITS_SECOND); data_bit_duration_ms_.push_back(1e3 / GPS_CA_TELEMETRY_RATE_BITS_SECOND);
} }
else if (system_[sat] == "E") else if (system_[sat] == "E")
{ {
if (signal_[sat].at(0)=='5') if (signal_[sat].at(0) == '5')
{ {
int codelen = (int)Galileo_E5a_CODE_LENGTH_CHIPS; int codelen = static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS);
samples_per_code_.push_back(round((float)fs_in_ / (Galileo_E5a_CODE_CHIP_RATE_HZ samples_per_code_.push_back(round(static_cast<float>(fs_in_) / (Galileo_E5a_CODE_CHIP_RATE_HZ
/ codelen))); / codelen)));
num_of_codes_per_vector_.push_back(1); num_of_codes_per_vector_.push_back(1);
@@ -120,38 +121,18 @@ void signal_generator_c::init()
} }
else else
{ {
samples_per_code_.push_back(round((float)fs_in_ / (Galileo_E1_CODE_CHIP_RATE_HZ samples_per_code_.push_back(round(static_cast<float>(fs_in_) / (Galileo_E1_CODE_CHIP_RATE_HZ
/ Galileo_E1_B_CODE_LENGTH_CHIPS))); / Galileo_E1_B_CODE_LENGTH_CHIPS)));
num_of_codes_per_vector_.push_back((int)Galileo_E1_C_SECONDARY_CODE_LENGTH); num_of_codes_per_vector_.push_back(static_cast<int>(Galileo_E1_C_SECONDARY_CODE_LENGTH));
data_bit_duration_ms_.push_back(1e3/Galileo_E1_B_SYMBOL_RATE_BPS); data_bit_duration_ms_.push_back(1e3 / Galileo_E1_B_SYMBOL_RATE_BPS);
} }
} }
} }
random_ = new gr::random(); random_ = new gr::random();
// std::cout << "fs_in: " << fs_in_ << std::endl;
// std::cout << "data_flag: " << data_flag_ << std::endl;
// std::cout << "noise_flag_: " << noise_flag_ << std::endl;
// std::cout << "num_sats_: " << num_sats_ << std::endl;
// std::cout << "vector_length_: " << vector_length_ << std::endl;
// std::cout << "BW_BB_: " << BW_BB_ << std::endl;
// for (unsigned int i = 0; i < num_sats_; i++)
// {
// std::cout << "Sat " << i << ": " << std::endl;
// std::cout << " System " << system_[i] << ": " << std::endl;
// std::cout << " PRN: " << PRN_[i] << std::endl;
// std::cout << " CN0: " << CN0_dB_[i] << std::endl;
// std::cout << " Doppler: " << doppler_Hz_[i] << std::endl;
// std::cout << " Delay: " << delay_chips_[i] << std::endl;
// std::cout << " Samples per code = " << samples_per_code_[i] << std::endl;
// std::cout << " codes per vector = " << num_of_codes_per_vector_[i] << std::endl;
// std::cout << " data_bit_duration = " << data_bit_duration_ms_[i] << std::endl;
// }
} }
void signal_generator_c::generate_codes() void signal_generator_c::generate_codes()
{ {
sampled_code_data_.reset(new gr_complex*[num_sats_]); sampled_code_data_.reset(new gr_complex*[num_sats_]);
@@ -159,16 +140,15 @@ void signal_generator_c::generate_codes()
for (unsigned int sat = 0; sat < num_sats_; sat++) for (unsigned int sat = 0; sat < num_sats_; sat++)
{ {
if (posix_memalign((void**)&(sampled_code_data_[sat]), 16, sampled_code_data_[sat] = static_cast<gr_complex*>(std::malloc(vector_length_ * sizeof(gr_complex)));
vector_length_ * sizeof(gr_complex)) == 0){};
gr_complex code[64000];//[samples_per_code_[sat]]; gr_complex code[64000]; //[samples_per_code_[sat]];
if (system_[sat] == "G") if (system_[sat] == "G")
{ {
// Generate one code-period of 1C signal // Generate one code-period of 1C signal
gps_l1_ca_code_gen_complex_sampled(code, PRN_[sat], fs_in_, gps_l1_ca_code_gen_complex_sampled(code, PRN_[sat], fs_in_,
(int)GPS_L1_CA_CODE_LENGTH_CHIPS - delay_chips_[sat]); static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) - delay_chips_[sat]);
// Obtain the desired CN0 assuming that Pn = 1. // Obtain the desired CN0 assuming that Pn = 1.
if (noise_flag_) if (noise_flag_)
@@ -182,25 +162,19 @@ void signal_generator_c::generate_codes()
// Concatenate "num_of_codes_per_vector_" codes // Concatenate "num_of_codes_per_vector_" codes
for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++) for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++)
{ {
memcpy(&(sampled_code_data_[sat][i*samples_per_code_[sat]]), memcpy(&(sampled_code_data_[sat][i * samples_per_code_[sat]]),
code, sizeof(gr_complex)*samples_per_code_[sat]); code, sizeof(gr_complex) * samples_per_code_[sat]);
} }
} }
else if (system_[sat] == "E") else if (system_[sat] == "E")
{ {
if(signal_[sat].at(0)=='5') if(signal_[sat].at(0) == '5')
{ {
char signal[3]; char signal[3];
strcpy(signal,"5X"); strcpy(signal, "5X");
if (posix_memalign((void**)&(sampled_code_data_[sat]), 16,
vector_length_ * sizeof(gr_complex)) == 0){};
galileo_e5_a_code_gen_complex_sampled(sampled_code_data_[sat] , signal, PRN_[sat], fs_in_, galileo_e5_a_code_gen_complex_sampled(sampled_code_data_[sat] , signal, PRN_[sat], fs_in_,
(int)Galileo_E5a_CODE_LENGTH_CHIPS-delay_chips_[sat]); static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) - delay_chips_[sat]);
//noise //noise
if (noise_flag_) if (noise_flag_)
{ {
@@ -209,7 +183,6 @@ void signal_generator_c::generate_codes()
sampled_code_data_[sat][i] *= sqrt(pow(10, CN0_dB_[sat] / 10) / BW_BB_ / 2); sampled_code_data_[sat][i] *= sqrt(pow(10, CN0_dB_[sat] / 10) / BW_BB_ / 2);
} }
} }
} }
else else
{ {
@@ -219,7 +192,7 @@ void signal_generator_c::generate_codes()
strcpy(signal, "1B"); strcpy(signal, "1B");
galileo_e1_code_gen_complex_sampled(code, signal, cboc, PRN_[sat], fs_in_, galileo_e1_code_gen_complex_sampled(code, signal, cboc, PRN_[sat], fs_in_,
(int)Galileo_E1_B_CODE_LENGTH_CHIPS - delay_chips_[sat]); static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS) - delay_chips_[sat]);
// Obtain the desired CN0 assuming that Pn = 1. // Obtain the desired CN0 assuming that Pn = 1.
if (noise_flag_) if (noise_flag_)
@@ -233,18 +206,17 @@ void signal_generator_c::generate_codes()
// Concatenate "num_of_codes_per_vector_" codes // Concatenate "num_of_codes_per_vector_" codes
for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++) for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++)
{ {
memcpy(&(sampled_code_data_[sat][i*samples_per_code_[sat]]), memcpy(&(sampled_code_data_[sat][i * samples_per_code_[sat]]),
code, sizeof(gr_complex)*samples_per_code_[sat]); code, sizeof(gr_complex) * samples_per_code_[sat]);
} }
// Generate E1C signal (25 code-periods, with secondary code) // Generate E1C signal (25 code-periods, with secondary code)
if (posix_memalign((void**)&(sampled_code_pilot_[sat]), 16, sampled_code_pilot_[sat] = static_cast<gr_complex*>(std::malloc(vector_length_ * sizeof(gr_complex)));
vector_length_ * sizeof(gr_complex)) == 0){};
strcpy(signal, "1C"); strcpy(signal, "1C");
galileo_e1_code_gen_complex_sampled(sampled_code_pilot_[sat], signal, cboc, PRN_[sat], fs_in_, galileo_e1_code_gen_complex_sampled(sampled_code_pilot_[sat], signal, cboc, PRN_[sat], fs_in_,
(int)Galileo_E1_B_CODE_LENGTH_CHIPS-delay_chips_[sat], true); static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS) - delay_chips_[sat], true);
// Obtain the desired CN0 assuming that Pn = 1. // Obtain the desired CN0 assuming that Pn = 1.
if (noise_flag_) if (noise_flag_)
@@ -260,20 +232,18 @@ void signal_generator_c::generate_codes()
} }
/*
* Our virtual destructor.
*/
signal_generator_c::~signal_generator_c() signal_generator_c::~signal_generator_c()
{ {
for (unsigned int sat = 0; sat < num_sats_; sat++) /* for (unsigned int sat = 0; sat < num_sats_; sat++)
{ {
free(sampled_code_data_[sat]); std::free(sampled_code_data_[sat]);
if (system_[sat] == "E" && signal_[sat].at(0)!='5') if (system_[sat] == "E" && signal_[sat].at(0) != '5')
{ {
free(sampled_code_pilot_[sat]); std::free(sampled_code_pilot_[sat]);
} }
} } */
volk_free(complex_phase_);
delete random_; delete random_;
} }
@@ -298,7 +268,7 @@ gr_vector_void_star &output_items)
for (unsigned int sat = 0; sat < num_sats_; sat++) for (unsigned int sat = 0; sat < num_sats_; sat++)
{ {
float phase_step_rad = -(float)GPS_TWO_PI*doppler_Hz_[sat] / (float)fs_in_; float phase_step_rad = -static_cast<float>(GPS_TWO_PI) * doppler_Hz_[sat] / static_cast<float>(fs_in_);
fxp_nco(complex_phase_, vector_length_, start_phase_rad_[sat], phase_step_rad); fxp_nco(complex_phase_, vector_length_, start_phase_rad_[sat], phase_step_rad);
start_phase_rad_[sat] += vector_length_ * phase_step_rad; start_phase_rad_[sat] += vector_length_ * phase_step_rad;
@@ -306,7 +276,7 @@ gr_vector_void_star &output_items)
if (system_[sat] == "G") if (system_[sat] == "G")
{ {
unsigned int delay_samples = (delay_chips_[sat] % (int)GPS_L1_CA_CODE_LENGTH_CHIPS) unsigned int delay_samples = (delay_chips_[sat] % static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS))
* samples_per_code_[sat] / GPS_L1_CA_CODE_LENGTH_CHIPS; * samples_per_code_[sat] / GPS_L1_CA_CODE_LENGTH_CHIPS;
for (i = 0; i < num_of_codes_per_vector_[sat]; i++) for (i = 0; i < num_of_codes_per_vector_[sat]; i++)
@@ -322,7 +292,7 @@ gr_vector_void_star &output_items)
if (ms_counter_[sat] == 0 && data_flag_) if (ms_counter_[sat] == 0 && data_flag_)
{ {
// New random data bit // New random data bit
current_data_bits_[sat] = gr_complex((rand()%2) == 0 ? 1 : -1, 0); current_data_bits_[sat] = gr_complex((rand() % 2) == 0 ? 1 : -1, 0);
} }
for (k = delay_samples; k < samples_per_code_[sat]; k++) for (k = delay_samples; k < samples_per_code_[sat]; k++)
@@ -333,7 +303,7 @@ gr_vector_void_star &output_items)
out_idx++; out_idx++;
} }
ms_counter_[sat] = (ms_counter_[sat] + (int)round(1e3*GPS_L1_CA_CODE_PERIOD)) ms_counter_[sat] = (ms_counter_[sat] + static_cast<int>(round(1e3*GPS_L1_CA_CODE_PERIOD)))
% data_bit_duration_ms_[sat]; % data_bit_duration_ms_[sat];
} }
} }
@@ -343,12 +313,12 @@ gr_vector_void_star &output_items)
if(signal_[sat].at(0)=='5') if(signal_[sat].at(0)=='5')
{ {
// EACH WORK outputs 1 modulated primary code // EACH WORK outputs 1 modulated primary code
int codelen = (int)Galileo_E5a_CODE_LENGTH_CHIPS; int codelen = static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS);
unsigned int delay_samples = (delay_chips_[sat] % codelen) unsigned int delay_samples = (delay_chips_[sat] % codelen)
* samples_per_code_[sat] / codelen; * samples_per_code_[sat] / codelen;
for (k = 0; k < delay_samples; k++) for (k = 0; k < delay_samples; k++)
{ {
out[out_idx] += (gr_complex(sampled_code_data_[sat][out_idx].real()*data_modulation_[sat] , out[out_idx] += (gr_complex(sampled_code_data_[sat][out_idx].real()*data_modulation_[sat],
sampled_code_data_[sat][out_idx].imag()*pilot_modulation_[sat]) ) sampled_code_data_[sat][out_idx].imag()*pilot_modulation_[sat]) )
* complex_phase_[out_idx]; * complex_phase_[out_idx];
out_idx++; out_idx++;
@@ -360,22 +330,21 @@ gr_vector_void_star &output_items)
current_data_bit_int_[sat] = (rand()%2) == 0 ? 1 : -1; current_data_bit_int_[sat] = (rand()%2) == 0 ? 1 : -1;
} }
data_modulation_[sat] = current_data_bit_int_[sat] * (Galileo_E5a_I_SECONDARY_CODE.at((ms_counter_[sat]+delay_sec_[sat])%20)=='0' ? 1 : -1); data_modulation_[sat] = current_data_bit_int_[sat] * (Galileo_E5a_I_SECONDARY_CODE.at((ms_counter_[sat]+delay_sec_[sat])%20)=='0' ? 1 : -1);
pilot_modulation_[sat] = (Galileo_E5a_Q_SECONDARY_CODE[PRN_[sat]-1].at((ms_counter_[sat]+delay_sec_[sat])%100)=='0' ? 1 : -1); pilot_modulation_[sat] = (Galileo_E5a_Q_SECONDARY_CODE[PRN_[sat] - 1].at((ms_counter_[sat] + delay_sec_[sat]) % 100)=='0' ? 1 : -1);
ms_counter_[sat] = ms_counter_[sat] + (int)round(1e3*GALILEO_E5a_CODE_PERIOD); ms_counter_[sat] = ms_counter_[sat] + static_cast<int>(round(1e3*GALILEO_E5a_CODE_PERIOD));
for (k = delay_samples; k < samples_per_code_[sat]; k++) for (k = delay_samples; k < samples_per_code_[sat]; k++)
{ {
out[out_idx] += (gr_complex(sampled_code_data_[sat][out_idx].real()*data_modulation_[sat] , out[out_idx] += (gr_complex(sampled_code_data_[sat][out_idx].real() * data_modulation_[sat] ,
sampled_code_data_[sat][out_idx].imag()*pilot_modulation_[sat]) ) sampled_code_data_[sat][out_idx].imag() * pilot_modulation_[sat]) )
* complex_phase_[out_idx]; * complex_phase_[out_idx];
out_idx++; out_idx++;
} }
} }
else else
{ {
unsigned int delay_samples = (delay_chips_[sat] % (int)Galileo_E1_B_CODE_LENGTH_CHIPS) unsigned int delay_samples = (delay_chips_[sat] % static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS))
* samples_per_code_[sat] / Galileo_E1_B_CODE_LENGTH_CHIPS; * samples_per_code_[sat] / Galileo_E1_B_CODE_LENGTH_CHIPS;
for (i = 0; i < num_of_codes_per_vector_[sat]; i++) for (i = 0; i < num_of_codes_per_vector_[sat]; i++)
@@ -383,29 +352,25 @@ gr_vector_void_star &output_items)
for (k = 0; k < delay_samples; k++) for (k = 0; k < delay_samples; k++)
{ {
out[out_idx] += (sampled_code_data_[sat][out_idx] * current_data_bits_[sat] out[out_idx] += (sampled_code_data_[sat][out_idx] * current_data_bits_[sat]
- sampled_code_pilot_[sat][out_idx]) - sampled_code_pilot_[sat][out_idx]) * complex_phase_[out_idx];
* complex_phase_[out_idx];
out_idx++; out_idx++;
} }
if (ms_counter_[sat] == 0 && data_flag_) if (ms_counter_[sat] == 0 && data_flag_)
{ {
// New random data bit // New random data bit
current_data_bits_[sat] = gr_complex((rand()%2) == 0 ? 1 : -1, 0); current_data_bits_[sat] = gr_complex((rand() % 2) == 0 ? 1 : -1, 0);
} }
for (k = delay_samples; k < samples_per_code_[sat]; k++) for (k = delay_samples; k < samples_per_code_[sat]; k++)
{ {
out[out_idx] += (sampled_code_data_[sat][out_idx] * current_data_bits_[sat] out[out_idx] += (sampled_code_data_[sat][out_idx] * current_data_bits_[sat]
- sampled_code_pilot_[sat][out_idx]) - sampled_code_pilot_[sat][out_idx])
* complex_phase_[out_idx]; * complex_phase_[out_idx];
out_idx++; out_idx++;
} }
ms_counter_[sat] = (ms_counter_[sat] + static_cast<int>(round(1e3 * Galileo_E1_CODE_PERIOD))) % data_bit_duration_ms_[sat];
ms_counter_[sat] = (ms_counter_[sat] + (int)round(1e3*Galileo_E1_CODE_PERIOD))
% data_bit_duration_ms_[sat];
} }
} }
} }

10
src/algorithms/signal_source/adapters/file_signal_source.cc
View File

@@ -68,6 +68,8 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
dump_ = configuration->property(role + ".dump", false); dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false); enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false);
std::string s = "InputFilter";
double IF = configuration->property(s + ".IF", 0.0);
if (item_type_.compare("gr_complex") == 0) if (item_type_.compare("gr_complex") == 0)
{ {
@@ -147,9 +149,11 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
CHECK(samples_ > 0) << "File does not contain enough samples to process."; CHECK(samples_ > 0) << "File does not contain enough samples to process.";
double signal_duration_s; double signal_duration_s;
signal_duration_s = (double)samples_ * ( 1 /(double)sampling_frequency_); signal_duration_s = (double)samples_ * ( 1 /(double)sampling_frequency_);
#ifdef ARCH_64BITS
signal_duration_s /= 2; if ((item_type_.compare("gr_complex") != 0) && (IF < 1e6) ) // if IF < BW/2, signal is complex (interleaved)
#endif {
signal_duration_s /= 2;
}
DLOG(INFO) << "Total number samples to be processed= " << samples_ << " GNSS signal duration= " << signal_duration_s << " [s]"; DLOG(INFO) << "Total number samples to be processed= " << samples_ << " GNSS signal duration= " << signal_duration_s << " [s]";
std::cout << "GNSS signal recorded time to be processed: " << signal_duration_s << " [s]" << std::endl; std::cout << "GNSS signal recorded time to be processed: " << signal_duration_s << " [s]" << std::endl;

10
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
View File

@@ -456,7 +456,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items, gr_vector
{ {
d_CRC_error_counter = 0; d_CRC_error_counter = 0;
d_flag_preamble = true; //valid preamble indicator (initialized to false every work()) d_flag_preamble = true; //valid preamble indicator (initialized to false every work())
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs - ((double)(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); //record the PRN start sample index associated to the preamble start. d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs - (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); //record the PRN start sample index associated to the preamble start.
if (!d_flag_frame_sync) if (!d_flag_frame_sync)
{ {
d_flag_frame_sync = true; d_flag_frame_sync = true;
@@ -505,25 +505,25 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items, gr_vector
if (d_nav.flag_TOW_1 == true) if (d_nav.flag_TOW_1 == true)
{ {
d_TOW_at_Preamble = d_nav.FNAV_TOW_1; d_TOW_at_Preamble = d_nav.FNAV_TOW_1;
d_TOW_at_current_symbol = d_TOW_at_Preamble + ((double)(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_nav.flag_TOW_1 = false; d_nav.flag_TOW_1 = false;
} }
if (d_nav.flag_TOW_2 == true) if (d_nav.flag_TOW_2 == true)
{ {
d_TOW_at_Preamble = d_nav.FNAV_TOW_2; d_TOW_at_Preamble = d_nav.FNAV_TOW_2;
d_TOW_at_current_symbol = d_TOW_at_Preamble + ((double)(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_nav.flag_TOW_2 = false; d_nav.flag_TOW_2 = false;
} }
if (d_nav.flag_TOW_3 == true) if (d_nav.flag_TOW_3 == true)
{ {
d_TOW_at_Preamble = d_nav.FNAV_TOW_3; d_TOW_at_Preamble = d_nav.FNAV_TOW_3;
d_TOW_at_current_symbol = d_TOW_at_Preamble + ((double)(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_nav.flag_TOW_3 = false; d_nav.flag_TOW_3 = false;
} }
if (d_nav.flag_TOW_4 == true) if (d_nav.flag_TOW_4 == true)
{ {
d_TOW_at_Preamble = d_nav.FNAV_TOW_4; d_TOW_at_Preamble = d_nav.FNAV_TOW_4;
d_TOW_at_current_symbol = d_TOW_at_Preamble + ((double)(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
d_nav.flag_TOW_4 = false; d_nav.flag_TOW_4 = false;
} }
else else

151
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
View File

@@ -83,7 +83,7 @@ galileo_e1_dll_pll_veml_make_tracking_cc(
void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items, void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call ninput_items_required[0] = static_cast<int>(d_vector_length) * 2; //set the required available samples in each call
} }
@@ -124,33 +124,25 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc(
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
d_ca_code = new gr_complex[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 4)]; d_ca_code = static_cast<gr_complex*>(volk_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 4) * sizeof(gr_complex), volk_get_alignment()));
/* If an array is partitioned for more than one thread to operate on, d_very_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* having the sub-array boundaries unaligned to cache lines could lead d_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* to performance degradation. Here we allocate memory d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes d_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
*/ d_very_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_carr_sign = static_cast<gr_complex*>(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_very_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_very_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
// correlator outputs (scalar) // correlator outputs (scalar)
d_Very_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Very_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Prompt = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Very_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Very_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment());
//--- Initializations ------------------------------ //--- Initializations ------------------------------
// Initial code frequency basis of NCO // Initial code frequency basis of NCO
d_code_freq_chips = (double)Galileo_E1_CODE_CHIP_RATE_HZ; d_code_freq_chips = static_cast<double>(Galileo_E1_CODE_CHIP_RATE_HZ);
// Residual code phase (in chips) // Residual code phase (in chips)
d_rem_code_phase_samples = 0.0; d_rem_code_phase_samples = 0.0;
// Residual carrier phase // Residual carrier phase
@@ -165,7 +157,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc(
d_pull_in = false; d_pull_in = false;
d_last_seg = 0; d_last_seg = 0;
d_current_prn_length_samples = (int)d_vector_length; d_current_prn_length_samples = static_cast<int>(d_vector_length);
// CN0 estimation and lock detector buffers // CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
@@ -176,11 +168,11 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc(
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD;
systemName["E"] = std::string("Galileo"); systemName["E"] = std::string("Galileo");
*d_Very_Early=gr_complex(0,0); *d_Very_Early = gr_complex(0,0);
*d_Early=gr_complex(0,0); *d_Early = gr_complex(0,0);
*d_Prompt=gr_complex(0,0); *d_Prompt = gr_complex(0,0);
*d_Late=gr_complex(0,0); *d_Late = gr_complex(0,0);
*d_Very_Late=gr_complex(0,0); *d_Very_Late = gr_complex(0,0);
} }
void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
@@ -198,13 +190,13 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
d_acquisition_gnss_synchro->Signal, d_acquisition_gnss_synchro->Signal,
false, false,
d_acquisition_gnss_synchro->PRN, d_acquisition_gnss_synchro->PRN,
2*Galileo_E1_CODE_CHIP_RATE_HZ, 2 * Galileo_E1_CODE_CHIP_RATE_HZ,
0); 0);
// Fill head and tail // Fill head and tail
d_ca_code[0] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS)]; d_ca_code[0] = d_ca_code[static_cast<int>(2 * Galileo_E1_B_CODE_LENGTH_CHIPS)];
d_ca_code[1] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 1)]; d_ca_code[1] = d_ca_code[static_cast<int>(2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 1)];
d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 2)] = d_ca_code[2]; d_ca_code[static_cast<int>(2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 2)] = d_ca_code[2];
d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 3)] = d_ca_code[3]; d_ca_code[static_cast<int>(2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 3)] = d_ca_code[3];
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0.0; d_rem_code_phase_samples = 0.0;
@@ -236,7 +228,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
double tcode_half_chips; double tcode_half_chips;
float rem_code_phase_half_chips; float rem_code_phase_half_chips;
int associated_chip_index; int associated_chip_index;
int code_length_half_chips = (int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS); int code_length_half_chips = static_cast<int>(Galileo_E1_B_CODE_LENGTH_CHIPS) * 2;
double code_phase_step_chips; double code_phase_step_chips;
double code_phase_step_half_chips; double code_phase_step_half_chips;
int early_late_spc_samples; int early_late_spc_samples;
@@ -244,34 +236,34 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
int epl_loop_length_samples; int epl_loop_length_samples;
// unified loop for VE, E, P, L, VL code vectors // unified loop for VE, E, P, L, VL code vectors
code_phase_step_chips = ((double)d_code_freq_chips) / ((double)d_fs_in); code_phase_step_chips = (static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
code_phase_step_half_chips = (2.0*(double)d_code_freq_chips) / ((double)d_fs_in); code_phase_step_half_chips = (2.0 * static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in); rem_code_phase_half_chips = d_rem_code_phase_samples * (2*d_code_freq_chips / d_fs_in);
tcode_half_chips = -(double)rem_code_phase_half_chips; tcode_half_chips = - static_cast<double>(rem_code_phase_half_chips);
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips); very_early_late_spc_samples = round(d_very_early_late_spc_chips / code_phase_step_chips);
epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples * 2;
for (int i = 0; i < epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 2 + round(fmod(tcode_half_chips - 2*d_very_early_late_spc_chips, code_length_half_chips)); associated_chip_index = 2 + round(fmod(tcode_half_chips - 2 * d_very_early_late_spc_chips, code_length_half_chips));
d_very_early_code[i] = d_ca_code[associated_chip_index]; d_very_early_code[i] = d_ca_code[associated_chip_index];
tcode_half_chips = tcode_half_chips + code_phase_step_half_chips; tcode_half_chips = tcode_half_chips + code_phase_step_half_chips;
} }
memcpy(d_early_code, &d_very_early_code[very_early_late_spc_samples - early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_early_code, &d_very_early_code[very_early_late_spc_samples - early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
memcpy(d_prompt_code, &d_very_early_code[very_early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_prompt_code, &d_very_early_code[very_early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
memcpy(d_late_code, &d_very_early_code[very_early_late_spc_samples + early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_late_code, &d_very_early_code[very_early_late_spc_samples + early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
memcpy(d_very_late_code, &d_very_early_code[2*very_early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_very_late_code, &d_very_early_code[2 * very_early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
} }
void galileo_e1_dll_pll_veml_tracking_cc::update_local_carrier() void galileo_e1_dll_pll_veml_tracking_cc::update_local_carrier()
{ {
float phase_rad, phase_step_rad; float phase_rad, phase_step_rad;
// Compute the carrier phase step for the K-1 carrier doppler estimation // Compute the carrier phase step for the K-1 carrier doppler estimation
phase_step_rad = (float)GPS_TWO_PI*d_carrier_doppler_hz / (float)d_fs_in; phase_step_rad = static_cast<float>(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
// Initialize the carrier phase with the remanent carrier phase of the K-2 loop // Initialize the carrier phase with the remanent carrier phase of the K-2 loop
phase_rad = d_rem_carr_phase_rad; phase_rad = d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++) for(int i = 0; i < d_current_prn_length_samples; i++)
@@ -296,8 +288,8 @@ galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc()
volk_free(d_Prompt); volk_free(d_Prompt);
volk_free(d_Late); volk_free(d_Late);
volk_free(d_Very_Late); volk_free(d_Very_Late);
volk_free(d_ca_code);
delete[] d_ca_code;
delete[] d_Prompt_buffer; delete[] d_Prompt_buffer;
} }
@@ -322,7 +314,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
@@ -356,12 +348,11 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
d_Early, d_Early,
d_Prompt, d_Prompt,
d_Late, d_Late,
d_Very_Late, d_Very_Late);
is_unaligned());
// ################## PLL ########################################################## // ################## PLL ##########################################################
// PLL discriminator // PLL discriminator
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)GPS_TWO_PI; carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
// Carrier discriminator filter // Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation // New carrier Doppler frequency estimation
@@ -382,7 +373,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
//Code phase accumulator //Code phase accumulator
float code_error_filt_secs; float code_error_filt_secs;
code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds]
//code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*(float)d_fs_in; //[seconds] //code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*static_cast<float>(d_fs_in); //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
@@ -392,10 +383,10 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
double T_prn_samples; double T_prn_samples;
double K_blk_samples; double K_blk_samples;
// Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation
T_chip_seconds = 1 / (double)d_code_freq_chips; T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * (double)d_fs_in; T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)d_fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
@@ -441,25 +432,25 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
// ########### Output the tracking results to Telemetry block ########## // ########### Output the tracking results to Telemetry block ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
// Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!) // Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!)
//compute remnant code phase samples BEFORE the Tracking timestamp //compute remnant code phase samples BEFORE the Tracking timestamp
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter +
// (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / (double)d_fs_in; // (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??) // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_rem_code_phase_samples) / (double)d_fs_in; current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
//compute remnant code phase samples AFTER the Tracking timestamp //compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0; current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz; current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
// ########## DEBUG OUTPUT // ########## DEBUG OUTPUT
@@ -531,35 +522,35 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
try try
{ {
// Dump correlators output // Dump correlators output
d_dump_file.write((char*)&tmp_VE, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_VE), sizeof(float));
d_dump_file.write((char*)&tmp_E, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
d_dump_file.write((char*)&tmp_VL, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_VL), sizeof(float));
// PROMPT I and Q (to analyze navigation symbols) // PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
// PRN start sample stamp // PRN start sample stamp
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase // accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
// carrier and code frequency // carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
d_dump_file.write((char*)&d_code_freq_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
//PLL commands //PLL commands
d_dump_file.write((char*)&carr_error_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
d_dump_file.write((char*)&carr_error_filt_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
//DLL commands //DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
d_dump_file.write((char*)&code_error_filt_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
// CN0 and carrier lock test // CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples; tmp_float = d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples); tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
{ {

36
src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
View File

@@ -127,29 +127,22 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
d_ca_code = new gr_complex[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 4)]; d_ca_code = static_cast<gr_complex*>(volk_malloc(((2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 4)) * sizeof(gr_complex), volk_get_alignment()));
/* If an array is partitioned for more than one thread to operate on, d_very_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* having the sub-array boundaries unaligned to cache lines could lead d_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* to performance degradation. Here we allocate memory d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes d_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
*/ d_very_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_carr_sign = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_very_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_very_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
// correlator outputs (scalar) // correlator outputs (scalar)
d_Very_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Very_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Prompt = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Very_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Very_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
@@ -291,8 +284,8 @@ Galileo_E1_Tcp_Connector_Tracking_cc::~Galileo_E1_Tcp_Connector_Tracking_cc()
volk_free(d_Prompt); volk_free(d_Prompt);
volk_free(d_Late); volk_free(d_Late);
volk_free(d_Very_Late); volk_free(d_Very_Late);
volk_free(d_ca_code);
delete[] d_ca_code;
delete[] d_Prompt_buffer; delete[] d_Prompt_buffer;
d_tcp_com.close_tcp_connection(d_port); d_tcp_com.close_tcp_connection(d_port);
@@ -352,8 +345,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
d_Early, d_Early,
d_Prompt, d_Prompt,
d_Late, d_Late,
d_Very_Late, d_Very_Late);
is_unaligned());
// ################## TCP CONNECTOR ########################################################## // ################## TCP CONNECTOR ##########################################################
//! Variable used for control //! Variable used for control

257
src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc
View File

@@ -77,7 +77,7 @@ galileo_e5a_dll_pll_make_tracking_cc(
float early_late_space_chips) float early_late_space_chips)
{ {
return galileo_e5a_dll_pll_tracking_cc_sptr(new Galileo_E5a_Dll_Pll_Tracking_cc(if_freq, return galileo_e5a_dll_pll_tracking_cc_sptr(new Galileo_E5a_Dll_Pll_Tracking_cc(if_freq,
fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz,pll_bw_init_hz, dll_bw_init_hz, ti_ms, early_late_space_chips)); fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, pll_bw_init_hz, dll_bw_init_hz, ti_ms, early_late_space_chips));
} }
@@ -85,7 +85,7 @@ galileo_e5a_dll_pll_make_tracking_cc(
void Galileo_E5a_Dll_Pll_Tracking_cc::forecast (int noutput_items, void Galileo_E5a_Dll_Pll_Tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call ninput_items_required[0] = static_cast<int>(d_vector_length)*2; //set the required available samples in each call
} }
Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc( Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc(
@@ -130,29 +130,20 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc(
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the E5a primary code replicas sampled 1x/chip // Get space for a vector with the E5a primary code replicas sampled 1x/chip
d_codeQ = new gr_complex[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 2]; d_codeQ = new gr_complex[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) + 2];
d_codeI = new gr_complex[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 2]; d_codeI = new gr_complex[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) + 2];
/* If an array is partitioned for more than one thread to operate on, d_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* having the sub-array boundaries unaligned to cache lines could lead d_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* to performance degradation. Here we allocate memory d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes d_prompt_data_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
*/ d_carr_sign = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); // correlator outputs (complex number)
d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_Early = gr_complex(0, 0);
d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_Prompt = gr_complex(0, 0);
d_Late = gr_complex(0, 0);
d_prompt_data_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_Prompt_data = gr_complex(0, 0);
d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment());
// correlator outputs (scalar)
d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment());
d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment());
d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment());
d_Prompt_data=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment());
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
@@ -161,23 +152,19 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc(
d_rem_code_phase_samples = 0.0; d_rem_code_phase_samples = 0.0;
// define residual carrier phase // define residual carrier phase
d_rem_carr_phase_rad = 0.0; d_rem_carr_phase_rad = 0.0;
//Filter error vars //Filter error vars
d_code_error_filt_secs = 0.0; d_code_error_filt_secs = 0.0;
// sample synchronization // sample synchronization
d_sample_counter = 0; d_sample_counter = 0;
d_acq_sample_stamp = 0; d_acq_sample_stamp = 0;
d_last_seg = 0; d_last_seg = 0;
d_first_transition = false; d_first_transition = false;
d_secondary_lock=false; d_secondary_lock = false;
d_secondary_delay=0; d_secondary_delay = 0;
d_integration_counter = 0; d_integration_counter = 0;
d_current_prn_length_samples = (int)d_vector_length; d_current_prn_length_samples = static_cast<int>(d_vector_length);
// CN0 estimation and lock detector buffers // CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
@@ -187,29 +174,21 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc(
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD;
systemName["G"] = std::string("GPS");
systemName["R"] = std::string("GLONASS");
systemName["S"] = std::string("SBAS");
systemName["E"] = std::string("Galileo"); systemName["E"] = std::string("Galileo");
systemName["C"] = std::string("Compass");
} }
Galileo_E5a_Dll_Pll_Tracking_cc::~Galileo_E5a_Dll_Pll_Tracking_cc () Galileo_E5a_Dll_Pll_Tracking_cc::~Galileo_E5a_Dll_Pll_Tracking_cc ()
{ {
d_dump_file.close(); d_dump_file.close();
volk_free(d_prompt_code); volk_free(d_prompt_code);
volk_free(d_late_code); volk_free(d_late_code);
volk_free(d_early_code); volk_free(d_early_code);
volk_free(d_carr_sign); volk_free(d_carr_sign);
volk_free(d_prompt_data_code); volk_free(d_prompt_data_code);
volk_free(d_Prompt_data);
delete[] d_codeQ;
delete[] d_codeI; delete[] d_codeI;
delete[] d_codeQ;
delete[] d_Prompt_buffer; delete[] d_Prompt_buffer;
} }
@@ -224,9 +203,9 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking()
long int acq_trk_diff_samples; long int acq_trk_diff_samples;
float acq_trk_diff_seconds; float acq_trk_diff_seconds;
acq_trk_diff_samples = (long int)d_sample_counter - (long int)d_acq_sample_stamp;//-d_vector_length; acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in; acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect //doppler effect
// Fd=(C/(C+Vr))*F // Fd=(C/(C+Vr))*F
float radial_velocity; float radial_velocity;
@@ -238,18 +217,18 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking()
d_code_freq_chips = radial_velocity * Galileo_E5a_CODE_CHIP_RATE_HZ; d_code_freq_chips = radial_velocity * Galileo_E5a_CODE_CHIP_RATE_HZ;
T_chip_mod_seconds = 1/d_code_freq_chips; T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E5a_CODE_LENGTH_CHIPS; T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E5a_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in; T_prn_mod_samples = T_prn_mod_seconds * static_cast<float>(d_fs_in);
d_current_prn_length_samples = round(T_prn_mod_samples); d_current_prn_length_samples = round(T_prn_mod_samples);
float T_prn_true_seconds = Galileo_E5a_CODE_LENGTH_CHIPS / Galileo_E5a_CODE_CHIP_RATE_HZ; float T_prn_true_seconds = Galileo_E5a_CODE_LENGTH_CHIPS / Galileo_E5a_CODE_CHIP_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in; float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
float T_prn_diff_seconds; float T_prn_diff_seconds;
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
float N_prn_diff; float N_prn_diff;
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
float corrected_acq_phase_samples, delay_correction_samples; float corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * (float)d_fs_in), T_prn_true_samples); corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0) if (corrected_acq_phase_samples < 0)
{ {
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
@@ -268,13 +247,13 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking()
char sig[3]; char sig[3];
strcpy(sig,"5Q"); strcpy(sig,"5Q");
galileo_e5_a_code_gen_complex_primary(&d_codeQ[1], d_acquisition_gnss_synchro->PRN, sig); galileo_e5_a_code_gen_complex_primary(&d_codeQ[1], d_acquisition_gnss_synchro->PRN, sig);
d_codeQ[0] = d_codeQ[(int)Galileo_E5a_CODE_LENGTH_CHIPS]; d_codeQ[0] = d_codeQ[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS)];
d_codeQ[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 1] = d_codeQ[1]; d_codeQ[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) + 1] = d_codeQ[1];
strcpy(sig,"5I"); strcpy(sig,"5I");
galileo_e5_a_code_gen_complex_primary(&d_codeI[1], d_acquisition_gnss_synchro->PRN, sig); galileo_e5_a_code_gen_complex_primary(&d_codeI[1], d_acquisition_gnss_synchro->PRN, sig);
d_codeI[0] = d_codeI[(int)Galileo_E5a_CODE_LENGTH_CHIPS]; d_codeI[0] = d_codeI[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS)];
d_codeI[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 1] = d_codeI[1]; d_codeI[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) + 1] = d_codeI[1];
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0; d_rem_code_phase_samples = 0;
@@ -304,51 +283,51 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::acquire_secondary()
{ {
// 1. Transform replica to 1 and -1 // 1. Transform replica to 1 and -1
int sec_code_signed[Galileo_E5a_Q_SECONDARY_CODE_LENGTH]; int sec_code_signed[Galileo_E5a_Q_SECONDARY_CODE_LENGTH];
for (unsigned int i=0; i<Galileo_E5a_Q_SECONDARY_CODE_LENGTH; i++) for (unsigned int i = 0; i < Galileo_E5a_Q_SECONDARY_CODE_LENGTH; i++)
{ {
if (Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN-1].at(i) == '0') if (Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN-1].at(i) == '0')
{ {
sec_code_signed[i]=1; sec_code_signed[i] = 1;
} }
else else
{ {
sec_code_signed[i]=-1; sec_code_signed[i] = -1;
} }
} }
// 2. Transform buffer to 1 and -1 // 2. Transform buffer to 1 and -1
int in_corr[CN0_ESTIMATION_SAMPLES]; int in_corr[CN0_ESTIMATION_SAMPLES];
for (unsigned int i=0; i<CN0_ESTIMATION_SAMPLES; i++) for (unsigned int i = 0; i < CN0_ESTIMATION_SAMPLES; i++)
{ {
if (d_Prompt_buffer[i].real() >0) if (d_Prompt_buffer[i].real() >0)
{ {
in_corr[i]=1; in_corr[i] = 1;
} }
else else
{ {
in_corr[i]=-1; in_corr[i] = -1;
} }
} }
// 3. Serial search // 3. Serial search
int out_corr; int out_corr;
int current_best_=0; int current_best_ = 0;
for (unsigned int i=0; i<Galileo_E5a_Q_SECONDARY_CODE_LENGTH; i++) for (unsigned int i = 0; i < Galileo_E5a_Q_SECONDARY_CODE_LENGTH; i++)
{ {
out_corr=0; out_corr = 0;
for (unsigned int j=0; j<CN0_ESTIMATION_SAMPLES; j++) for (unsigned int j = 0; j < CN0_ESTIMATION_SAMPLES; j++)
{ {
//reverse replica sign since i*i=-1 (conjugated complex) //reverse replica sign since i*i=-1 (conjugated complex)
out_corr += in_corr[j] * -sec_code_signed[(j+i)%Galileo_E5a_Q_SECONDARY_CODE_LENGTH]; out_corr += in_corr[j] * -sec_code_signed[(j+i) % Galileo_E5a_Q_SECONDARY_CODE_LENGTH];
} }
if (abs(out_corr) > current_best_) if (abs(out_corr) > current_best_)
{ {
current_best_ = abs(out_corr); current_best_ = abs(out_corr);
d_secondary_delay=i; d_secondary_delay = i;
} }
} }
if (current_best_ == CN0_ESTIMATION_SAMPLES) // all bits correlate if (current_best_ == CN0_ESTIMATION_SAMPLES) // all bits correlate
{ {
d_secondary_lock = true; d_secondary_lock = true;
d_secondary_delay = (d_secondary_delay+CN0_ESTIMATION_SAMPLES-1)%Galileo_E5a_Q_SECONDARY_CODE_LENGTH; d_secondary_delay = (d_secondary_delay + CN0_ESTIMATION_SAMPLES - 1) % Galileo_E5a_Q_SECONDARY_CODE_LENGTH;
} }
} }
@@ -358,19 +337,19 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_code()
double rem_code_phase_chips; double rem_code_phase_chips;
int associated_chip_index; int associated_chip_index;
int associated_chip_index_data; int associated_chip_index_data;
int code_length_chips = (int)Galileo_E5a_CODE_LENGTH_CHIPS; int code_length_chips = static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS);
double code_phase_step_chips; double code_phase_step_chips;
int early_late_spc_samples; int early_late_spc_samples;
int epl_loop_length_samples; int epl_loop_length_samples;
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
code_phase_step_chips = ((double)d_code_freq_chips) / ((double)d_fs_in); code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
tcode_chips = -rem_code_phase_chips; tcode_chips = -rem_code_phase_chips;
// Alternative EPL code generation (40% of speed improvement!) // Alternative EPL code generation (40% of speed improvement!)
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples * 2;
for (int i = 0; i < epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
@@ -380,8 +359,8 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_code()
d_prompt_data_code[i] = d_codeI[associated_chip_index_data]; d_prompt_data_code[i] = d_codeI[associated_chip_index_data];
tcode_chips = tcode_chips + code_phase_step_chips; tcode_chips = tcode_chips + code_phase_step_chips;
} }
memcpy(d_prompt_code,&d_early_code[early_late_spc_samples],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
memcpy(d_late_code,&d_early_code[early_late_spc_samples*2],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_late_code, &d_early_code[early_late_spc_samples * 2], d_current_prn_length_samples * sizeof(gr_complex));
} }
@@ -389,7 +368,7 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_carrier()
{ {
float phase_rad, phase_step_rad; float phase_rad, phase_step_rad;
phase_step_rad = (float)2*GALILEO_PI*d_carrier_doppler_hz / (float)d_fs_in; phase_step_rad = 2 * static_cast<float>(GALILEO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
phase_rad = d_rem_carr_phase_rad; phase_rad = d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++) for(int i = 0; i < d_current_prn_length_samples; i++)
{ {
@@ -440,10 +419,10 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
std::cout << tmp_str_stream.rdbuf() << std::flush; std::cout << tmp_str_stream.rdbuf() << std::flush;
} }
} }
*d_Early = gr_complex(0,0); d_Early = gr_complex(0,0);
*d_Prompt = gr_complex(0,0); d_Prompt = gr_complex(0,0);
*d_Late = gr_complex(0,0); d_Late = gr_complex(0,0);
*d_Prompt_data = gr_complex(0,0); d_Prompt_data = gr_complex(0,0);
*out[0] = *d_acquisition_gnss_synchro; *out[0] = *d_acquisition_gnss_synchro;
@@ -455,16 +434,16 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
std::cout<<" samples_offset="<<samples_offset<<"\r\n"; DLOG(INFO) << " samples_offset=" << samples_offset;
d_state = 2; // start in Ti = 1 code, until secondary code lock. d_state = 2; // start in Ti = 1 code, until secondary code lock.
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0; current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in; current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0; current_synchro_data.CN0_dB_hz = 0.0;
@@ -485,13 +464,13 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
{ {
// sec_sign_Q = gr_complex((Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN-1].at(d_secondary_delay)=='0' ? 1 : -1),0); // sec_sign_Q = gr_complex((Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN-1].at(d_secondary_delay)=='0' ? 1 : -1),0);
// sec_sign_I = gr_complex((Galileo_E5a_I_SECONDARY_CODE.at(d_secondary_delay%Galileo_E5a_I_SECONDARY_CODE_LENGTH)=='0' ? 1 : -1),0); // sec_sign_I = gr_complex((Galileo_E5a_I_SECONDARY_CODE.at(d_secondary_delay%Galileo_E5a_I_SECONDARY_CODE_LENGTH)=='0' ? 1 : -1),0);
sec_sign_Q = gr_complex((Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN-1].at(d_secondary_delay)=='0' ? -1 : 1),0); sec_sign_Q = gr_complex((Galileo_E5a_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN-1].at(d_secondary_delay) == '0' ? -1 : 1), 0);
sec_sign_I = gr_complex((Galileo_E5a_I_SECONDARY_CODE.at(d_secondary_delay%Galileo_E5a_I_SECONDARY_CODE_LENGTH)=='0' ? -1 : 1),0); sec_sign_I = gr_complex((Galileo_E5a_I_SECONDARY_CODE.at(d_secondary_delay % Galileo_E5a_I_SECONDARY_CODE_LENGTH) == '0' ? -1 : 1), 0);
} }
else else
{ {
sec_sign_Q = gr_complex(1.0,0.0); sec_sign_Q = gr_complex(1.0, 0.0);
sec_sign_I = gr_complex(1.0,0.0); sec_sign_I = gr_complex(1.0, 0.0);
} }
// Reset integration counter // Reset integration counter
if (d_integration_counter == d_current_ti_ms) if (d_integration_counter == d_current_ti_ms)
@@ -504,9 +483,9 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
update_local_code(); update_local_code();
update_local_carrier(); update_local_carrier();
// Reset accumulated values // Reset accumulated values
*d_Early = gr_complex(0,0); d_Early = gr_complex(0,0);
*d_Prompt = gr_complex(0,0); d_Prompt = gr_complex(0,0);
*d_Late = gr_complex(0,0); d_Late = gr_complex(0,0);
} }
gr_complex single_early; gr_complex single_early;
gr_complex single_prompt; gr_complex single_prompt;
@@ -524,18 +503,17 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
&single_early, &single_early,
&single_prompt, &single_prompt,
&single_late, &single_late,
d_Prompt_data, &d_Prompt_data);
is_unaligned());
// Accumulate results (coherent integration since there are no bit transitions in pilot signal) // Accumulate results (coherent integration since there are no bit transitions in pilot signal)
*d_Early += single_early * sec_sign_Q; d_Early += single_early * sec_sign_Q;
*d_Prompt += single_prompt * sec_sign_Q; d_Prompt += single_prompt * sec_sign_Q;
*d_Late += single_late * sec_sign_Q; d_Late += single_late * sec_sign_Q;
*d_Prompt_data *= sec_sign_I; d_Prompt_data *= sec_sign_I;
d_integration_counter++; d_integration_counter++;
// check for samples consistency (this should be done before in the receiver / here only if the source is a file) // check for samples consistency (this should be done before in the receiver / here only if the source is a file)
if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan((d_Prompt).real()) == true or std::isnan((d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{ {
const int samples_available = ninput_items[0]; const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available; d_sample_counter = d_sample_counter + samples_available;
@@ -545,7 +523,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0; current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/(double)d_fs_in; current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0; current_synchro_data.CN0_dB_hz = 0.0;
@@ -561,11 +539,11 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
{ {
if (d_secondary_lock == true) if (d_secondary_lock == true)
{ {
carr_error_hz = pll_four_quadrant_atan(*d_Prompt) / (float)GALILEO_PI*2; carr_error_hz = pll_four_quadrant_atan(d_Prompt) / static_cast<float>(GALILEO_PI) * 2;
} }
else else
{ {
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)GALILEO_PI*2; carr_error_hz = pll_cloop_two_quadrant_atan(d_Prompt) / static_cast<float>(GALILEO_PI) * 2;
} }
// Carrier discriminator filter // Carrier discriminator filter
@@ -576,20 +554,20 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
d_code_freq_chips = Galileo_E5a_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E5a_CODE_CHIP_RATE_HZ) / Galileo_E5a_FREQ_HZ); d_code_freq_chips = Galileo_E5a_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E5a_CODE_CHIP_RATE_HZ) / Galileo_E5a_FREQ_HZ);
} }
//carrier phase accumulator for (K) doppler estimation //carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + 2*GALILEO_PI*d_carrier_doppler_hz*GALILEO_E5a_CODE_PERIOD; d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO //remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad+2*GALILEO_PI*d_carrier_doppler_hz*GALILEO_E5a_CODE_PERIOD; d_rem_carr_phase_rad = d_rem_carr_phase_rad + 2*GALILEO_PI * d_carrier_doppler_hz * GALILEO_E5a_CODE_PERIOD;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, 2*GALILEO_PI); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, 2*GALILEO_PI);
// ################## DLL ########################################################## // ################## DLL ##########################################################
if (d_integration_counter == d_current_ti_ms) if (d_integration_counter == d_current_ti_ms)
{ {
// DLL discriminator // DLL discriminator
code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late); //[chips/Ti] code_error_chips = dll_nc_e_minus_l_normalized(d_Early, d_Late); //[chips/Ti]
// Code discriminator filter // Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator //Code phase accumulator
d_code_error_filt_secs = (GALILEO_E5a_CODE_PERIOD*code_error_filt_chips)/Galileo_E5a_CODE_CHIP_RATE_HZ; //[seconds] d_code_error_filt_secs = (GALILEO_E5a_CODE_PERIOD * code_error_filt_chips) / Galileo_E5a_CODE_CHIP_RATE_HZ; //[seconds]
} }
d_acc_code_phase_secs = d_acc_code_phase_secs + d_code_error_filt_secs; d_acc_code_phase_secs = d_acc_code_phase_secs + d_code_error_filt_secs;
@@ -597,17 +575,13 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
// keep alignment parameters for the next input buffer // keep alignment parameters for the next input buffer
double T_chip_seconds; double T_chip_seconds;
double T_prn_seconds; double T_prn_seconds;
// float T_prn_samples;
// float K_blk_samples;
//double T_chip_seconds;
// double T_prn_seconds;
double T_prn_samples; double T_prn_samples;
double K_blk_samples; double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
T_chip_seconds = 1 / (double)d_code_freq_chips; T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
T_prn_seconds = T_chip_seconds * Galileo_E5a_CODE_LENGTH_CHIPS; T_prn_seconds = T_chip_seconds * Galileo_E5a_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * (double)d_fs_in; T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + d_code_error_filt_secs*(float)d_fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + d_code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
@@ -615,12 +589,12 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES-1) if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES-1)
{ {
// fill buffer with prompt correlator output values // fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt; d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
} }
else else
{ {
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt; d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
// ATTEMPT SECONDARY CODE ACQUISITION // ATTEMPT SECONDARY CODE ACQUISITION
if (d_secondary_lock == false) if (d_secondary_lock == false)
{ {
@@ -632,8 +606,6 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
// Change loop parameters ========================================== // Change loop parameters ==========================================
d_code_loop_filter.set_pdi(d_current_ti_ms * GALILEO_E5a_CODE_PERIOD); d_code_loop_filter.set_pdi(d_current_ti_ms * GALILEO_E5a_CODE_PERIOD);
d_carrier_loop_filter.set_pdi(d_current_ti_ms * GALILEO_E5a_CODE_PERIOD); d_carrier_loop_filter.set_pdi(d_current_ti_ms * GALILEO_E5a_CODE_PERIOD);
// d_code_loop_filter.initialize();
// d_carrier_loop_filter.initialize();
d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); d_code_loop_filter.set_DLL_BW(d_dll_bw_hz);
d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz);
} }
@@ -645,12 +617,11 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
{ {
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
ControlMessageFactory* cmf = new ControlMessageFactory(); std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (d_queue != gr::msg_queue::sptr()) if (d_queue != gr::msg_queue::sptr())
{ {
d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
} }
delete cmf;
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_state = 0; // TODO: check if disabling tracking is consistent with the channel state machine d_state = 0; // TODO: check if disabling tracking is consistent with the channel state machine
} }
@@ -675,12 +646,11 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
{ {
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
ControlMessageFactory* cmf = new ControlMessageFactory(); std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (d_queue != gr::msg_queue::sptr()) if (d_queue != gr::msg_queue::sptr())
{ {
d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
} }
delete cmf;
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_state = 0; d_state = 0;
} }
@@ -688,7 +658,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
} }
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
} }
if (d_secondary_lock && (d_secondary_delay%Galileo_E5a_I_SECONDARY_CODE_LENGTH)==0) if (d_secondary_lock && (d_secondary_delay % Galileo_E5a_I_SECONDARY_CODE_LENGTH) == 0)
{ {
d_first_transition = true; d_first_transition = true;
} }
@@ -696,23 +666,22 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
// The first Prompt output not equal to 0 is synchronized with the transition of a navigation data bit. // The first Prompt output not equal to 0 is synchronized with the transition of a navigation data bit.
if (d_secondary_lock && d_first_transition) if (d_secondary_lock && d_first_transition)
{ {
current_synchro_data.Prompt_I = (double)((*d_Prompt_data).real()); current_synchro_data.Prompt_I = static_cast<double>((d_Prompt_data).real());
current_synchro_data.Prompt_Q = (double)((*d_Prompt_data).imag()); current_synchro_data.Prompt_Q = static_cast<double>((d_Prompt_data).imag());
// Tracking_timestamp_secs is aligned with the PRN start sample // Tracking_timestamp_secs is aligned with the PRN start sample
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples)/(double)d_fs_in; current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_current_prn_length_samples) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0; current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz; current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
} }
else else
{ {
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0; current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in; current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0; current_synchro_data.CN0_dB_hz = 0.0;
@@ -729,49 +698,49 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
float prompt_Q; float prompt_Q;
float tmp_float; float tmp_float;
double tmp_double; double tmp_double;
prompt_I = (*d_Prompt_data).real(); prompt_I = (d_Prompt_data).real();
prompt_Q = (*d_Prompt_data).imag(); prompt_Q = (d_Prompt_data).imag();
if (d_integration_counter == d_current_ti_ms) if (d_integration_counter == d_current_ti_ms)
{ {
tmp_E = std::abs<float>(*d_Early); tmp_E = std::abs<float>(d_Early);
tmp_P = std::abs<float>(*d_Prompt); tmp_P = std::abs<float>(d_Prompt);
tmp_L = std::abs<float>(*d_Late); tmp_L = std::abs<float>(d_Late);
} }
try try
{ {
// EPR // EPR
d_dump_file.write((char*)&tmp_E, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
// PROMPT I and Q (to analyze navigation symbols) // PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
// PRN start sample stamp // PRN start sample stamp
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase // accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
// carrier and code frequency // carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
d_dump_file.write((char*)&d_code_freq_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
//PLL commands //PLL commands
d_dump_file.write((char*)&carr_error_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
d_dump_file.write((char*)&carr_error_filt_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
//DLL commands //DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
d_dump_file.write((char*)&code_error_filt_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
// CN0 and carrier lock test // CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples; tmp_float = d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples); tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
{ {
@@ -779,7 +748,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
} }
} }
d_secondary_delay = (d_secondary_delay + 1)%Galileo_E5a_Q_SECONDARY_CODE_LENGTH; d_secondary_delay = (d_secondary_delay + 1) % Galileo_E5a_Q_SECONDARY_CODE_LENGTH;
d_sample_counter += d_current_prn_length_samples; //count for the processed samples d_sample_counter += d_current_prn_length_samples; //count for the processed samples
consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false

8
src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.h
View File

@@ -151,10 +151,10 @@ private:
gr_complex* d_prompt_data_code; gr_complex* d_prompt_data_code;
gr_complex* d_carr_sign; gr_complex* d_carr_sign;
gr_complex *d_Early; gr_complex d_Early;
gr_complex *d_Prompt; gr_complex d_Prompt;
gr_complex *d_Late; gr_complex d_Late;
gr_complex *d_Prompt_data; gr_complex d_Prompt_data;
float tmp_E; float tmp_E;
float tmp_P; float tmp_P;

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

@@ -84,7 +84,7 @@ gps_l1_ca_dll_fll_pll_tracking_cc_sptr gps_l1_ca_dll_fll_pll_make_tracking_cc(
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = d_vector_length*2; //set the required available samples in each call ninput_items_required[0] = d_vector_length * 2; //set the required available samples in each call
} }
@@ -109,10 +109,10 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
d_acquisition_gnss_synchro = NULL; d_acquisition_gnss_synchro = NULL;
d_if_freq = (double)if_freq; d_if_freq = static_cast<double>(if_freq);
d_fs_in = (double)fs_in; d_fs_in = static_cast<double>(fs_in);
d_vector_length = vector_length; d_vector_length = vector_length;
d_early_late_spc_chips = (double)early_late_space_chips; // Define early-late offset (in chips) d_early_late_spc_chips = static_cast<double>(early_late_space_chips); // Define early-late offset (in chips)
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
// Initialize tracking variables ========================================== // Initialize tracking variables ==========================================
@@ -122,25 +122,20 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
// Get space for a vector with the C/A code replica sampled 1x/chip // Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2]; d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
/* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory
* (gr_complex array of size 2*d_vector_length) aligned to cache of N bytes (machine dependent!)
*/
// Get space for the resampled early / prompt / late local replicas // Get space for the resampled early / prompt / late local replicas
d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_early_code = static_cast<gr_complex*>(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_prompt_code = static_cast<gr_complex*>(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_late_code = static_cast<gr_complex*>(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_carr_sign = static_cast<gr_complex*>(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// correlator outputs (scalar) // correlator outputs (scalar)
d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Prompt = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
// sample synchronization // sample synchronization
d_sample_counter = 0; d_sample_counter = 0;
@@ -148,7 +143,7 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
d_last_seg = 0;// this is for debug output only d_last_seg = 0;// this is for debug output only
d_code_phase_samples = 0; d_code_phase_samples = 0;
d_enable_tracking = false; d_enable_tracking = false;
d_current_prn_length_samples = (int)d_vector_length; d_current_prn_length_samples = static_cast<int>(d_vector_length);
// CN0 estimation and lock detector buffers // CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
@@ -179,8 +174,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
long int acq_trk_diff_samples; long int acq_trk_diff_samples;
float acq_trk_diff_seconds; float acq_trk_diff_seconds;
acq_trk_diff_samples = (long int)d_sample_counter - (long int)d_acq_sample_stamp; acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);
acq_trk_diff_seconds = (double)acq_trk_diff_samples / d_fs_in; acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / d_fs_in;
//doppler effect //doppler effect
// Fd=(C/(C+Vr))*F // Fd=(C/(C+Vr))*F
double radial_velocity; double radial_velocity;
@@ -219,8 +214,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
// generate local reference ALWAYS starting at chip 1 (1 sample per chip) // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0); gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0);
d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS]; d_ca_code[0] = d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS)];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1]; d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1];
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_Prompt_prev = 0; d_Prompt_prev = 0;
@@ -259,7 +254,7 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
int epl_loop_length_samples; int epl_loop_length_samples;
int associated_chip_index; int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS);
code_phase_step_chips = d_code_freq_hz / d_fs_in; code_phase_step_chips = d_code_freq_hz / d_fs_in;
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in); rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
@@ -312,8 +307,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier()
Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc() Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc()
{ {
d_dump_file.close(); d_dump_file.close();
delete[] d_ca_code;
volk_free(d_ca_code);
volk_free(d_prompt_code); volk_free(d_prompt_code);
volk_free(d_late_code); volk_free(d_late_code);
volk_free(d_early_code); volk_free(d_early_code);
@@ -321,6 +316,7 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc()
volk_free(d_Early); volk_free(d_Early);
volk_free(d_Prompt); volk_free(d_Prompt);
volk_free(d_Late); volk_free(d_Late);
delete[] d_Prompt_buffer; delete[] d_Prompt_buffer;
} }
@@ -355,8 +351,8 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
int samples_offset; int samples_offset;
double acq_trk_shif_correction_samples; double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter-d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((double)acq_to_trk_delay_samples, (double)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE // /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
@@ -366,7 +362,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0; current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in; current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / d_fs_in;
current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0; current_synchro_data.CN0_dB_hz = 0.0;
@@ -389,8 +385,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
d_late_code, d_late_code,
d_Early, d_Early,
d_Prompt, d_Prompt,
d_Late, d_Late);
is_unaligned());
// check for samples consistency (this should be done before in the receiver / here only if the source is a file) // check for samples consistency (this should be done before in the receiver / here only if the source is a file)
if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{ {
@@ -402,13 +397,13 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0; current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in; current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / d_fs_in;
current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0; current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking = false; current_synchro_data.Flag_valid_tracking = false;
*out[0] =current_synchro_data; *out[0] = current_synchro_data;
return 1; return 1;
} }
@@ -422,7 +417,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips);
//compute FLL error //compute FLL error
correlation_time_s = ((double)d_current_prn_length_samples) / d_fs_in; correlation_time_s = (static_cast<double>(d_current_prn_length_samples)) / d_fs_in;
if (d_FLL_wait == 1) if (d_FLL_wait == 1)
{ {
d_Prompt_prev = *d_Prompt; d_Prompt_prev = *d_Prompt;
@@ -495,7 +490,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
{ {
if (floor(d_sample_counter/d_fs_in) != d_last_seg) if (floor(d_sample_counter/d_fs_in) != d_last_seg)
{ {
d_last_seg = floor(d_sample_counter/d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
LOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; LOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
} }
@@ -514,12 +509,12 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
double T_prn_seconds; double T_prn_seconds;
double T_prn_samples; double T_prn_samples;
double K_blk_samples; double K_blk_samples;
T_chip_seconds = 1 / (double)d_code_freq_hz; T_chip_seconds = 1 / static_cast<double>(d_code_freq_hz);
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * d_fs_in; T_prn_samples = T_prn_seconds * d_fs_in;
float code_error_filt_samples; float code_error_filt_samples;
code_error_filt_samples = T_prn_seconds * code_error_filt_chips * T_chip_seconds * (double)d_fs_in; //[seconds] code_error_filt_samples = T_prn_seconds * code_error_filt_chips * T_chip_seconds * static_cast<double>(d_fs_in); //[seconds]
d_acc_code_phase_samples = d_acc_code_phase_samples + code_error_filt_samples; d_acc_code_phase_samples = d_acc_code_phase_samples + code_error_filt_samples;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_samples; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_samples;
@@ -527,11 +522,11 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
// Tracking_timestamp_secs is aligned with the PRN start sample // Tracking_timestamp_secs is aligned with the PRN start sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / (double)d_fs_in; //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / (double)d_fs_in;
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_rem_code_phase_samples)/(double)d_fs_in; current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples))/static_cast<double>(d_fs_in);
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, Code_phase_secs=0 // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0; current_synchro_data.Code_phase_secs = 0;

2
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h
View File

@@ -132,7 +132,7 @@ private:
void CN0_estimation_and_lock_detectors(); void CN0_estimation_and_lock_detectors();
// class private vars // class private vars
Gnss_Synchro *d_acquisition_gnss_synchro; Gnss_Synchro* d_acquisition_gnss_synchro;
boost::shared_ptr<gr::msg_queue> d_queue; boost::shared_ptr<gr::msg_queue> d_queue;
concurrent_queue<int> *d_channel_internal_queue; concurrent_queue<int> *d_channel_internal_queue;
unsigned int d_vector_length; unsigned int d_vector_length;

91
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
View File

@@ -107,7 +107,7 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
d_if_freq = if_freq; d_if_freq = if_freq;
d_fs_in = fs_in; d_fs_in = fs_in;
d_vector_length = vector_length; d_vector_length = vector_length;
d_gnuradio_forecast_samples = (int)d_vector_length*2; d_gnuradio_forecast_samples = static_cast<int>(d_vector_length) * 2;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
// Initialize tracking ========================================== // Initialize tracking ==========================================
@@ -119,25 +119,20 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip // Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2]; d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
/* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory
* (gr_complex array of size 2*d_vector_length) aligned to cache of N bytes (machine dependent!)
*/
// Get space for the resampled early / prompt / late local replicas // Get space for the resampled early / prompt / late local replicas
d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_carr_sign = static_cast<gr_complex*>(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// correlator outputs (scalar) // correlator outputs (scalar)
d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Prompt = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
@@ -156,7 +151,7 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
d_pull_in = false; d_pull_in = false;
d_last_seg = 0; d_last_seg = 0;
d_current_prn_length_samples = (int)d_vector_length; d_current_prn_length_samples = static_cast<int>(d_vector_length);
// CN0 estimation and lock detector buffers // CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
@@ -167,10 +162,6 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD;
systemName["G"] = std::string("GPS"); systemName["G"] = std::string("GPS");
systemName["R"] = std::string("GLONASS");
systemName["S"] = std::string("SBAS");
systemName["E"] = std::string("Galileo");
systemName["C"] = std::string("Compass");
} }
@@ -183,13 +174,13 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
long int acq_trk_diff_samples; long int acq_trk_diff_samples;
float acq_trk_diff_seconds; float acq_trk_diff_seconds;
acq_trk_diff_samples = (long int)d_sample_counter - (long int)d_acq_sample_stamp; //-d_vector_length; acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp); //-d_vector_length;
LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in; acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect //doppler effect
// Fd=(C/(C+Vr))*F // Fd=(C/(C+Vr))*F
float radial_velocity; float radial_velocity;
radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ; radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler // new chip and prn sequence periods based on acq Doppler
float T_chip_mod_seconds; float T_chip_mod_seconds;
float T_prn_mod_seconds; float T_prn_mod_seconds;
@@ -197,17 +188,15 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds = 1/d_code_freq_chips; T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in; T_prn_mod_samples = T_prn_mod_seconds * static_cast<float>(d_fs_in);
d_current_prn_length_samples = round(T_prn_mod_samples); d_current_prn_length_samples = round(T_prn_mod_samples);
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in; float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
float T_prn_diff_seconds; float 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 = acq_trk_diff_seconds / T_prn_true_seconds;
float N_prn_diff;
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
float corrected_acq_phase_samples, delay_correction_samples; float corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * (float)d_fs_in), T_prn_true_samples); corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0) if (corrected_acq_phase_samples < 0)
{ {
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
@@ -222,21 +211,21 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
// generate local reference ALWAYS starting at chip 1 (1 sample per chip) // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0); gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0);
d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS]; d_ca_code[0] = d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS)];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1]; d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1];
//****************************************************************************** //******************************************************************************
// Experimental: pre-sampled local signal replica at nominal code frequency. // Experimental: pre-sampled local signal replica at nominal code frequency.
// No code doppler correction // No code doppler correction
double tcode_chips; double tcode_chips;
int associated_chip_index; int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = static_cast<float>(GPS_L1_CA_CODE_LENGTH_CHIPS);
double code_phase_step_chips; double code_phase_step_chips;
int early_late_spc_samples; int early_late_spc_samples;
int epl_loop_length_samples; int epl_loop_length_samples;
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
code_phase_step_chips = ((double)d_code_freq_chips) / ((double)d_fs_in); code_phase_step_chips = (static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
tcode_chips = 0; tcode_chips = 0;
// Alternative EPL code generation (40% of speed improvement!) // Alternative EPL code generation (40% of speed improvement!)
@@ -283,13 +272,13 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::update_local_code()
double tcode_chips; double tcode_chips;
double rem_code_phase_chips; double rem_code_phase_chips;
int associated_chip_index; int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS);
double code_phase_step_chips; double code_phase_step_chips;
int early_late_spc_samples; int early_late_spc_samples;
int epl_loop_length_samples; int epl_loop_length_samples;
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
code_phase_step_chips = ((double)d_code_freq_chips) / ((double)d_fs_in); code_phase_step_chips = (static_cast<double>(d_code_freq_chips)) / (static_cast<double>(d_fs_in));
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
tcode_chips = -rem_code_phase_chips; tcode_chips = -rem_code_phase_chips;
@@ -311,7 +300,7 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::update_local_code()
void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::update_local_carrier() void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::update_local_carrier()
{ {
float phase_step_rad; float phase_step_rad;
phase_step_rad = (float)GPS_TWO_PI*d_carrier_doppler_hz / (float)d_fs_in; phase_step_rad = static_cast<float>(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
fxp_nco(d_carr_sign, d_current_prn_length_samples, d_rem_carr_phase_rad, phase_step_rad); fxp_nco(d_carr_sign, d_current_prn_length_samples, d_rem_carr_phase_rad, phase_step_rad);
} }
@@ -355,7 +344,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
@@ -385,8 +374,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
d_late_code, d_late_code,
d_Early, d_Early,
d_Prompt, d_Prompt,
d_Late, d_Late);
is_unaligned());
#else #else
d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples, d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples,
in, in,
@@ -396,12 +384,11 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
d_late_code, d_late_code,
d_Early, d_Early,
d_Prompt, d_Prompt,
d_Late, d_Late);
is_unaligned());
#endif #endif
// ################## PLL ########################################################## // ################## PLL ##########################################################
// PLL discriminator // PLL discriminator
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)GPS_TWO_PI; carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
// Carrier discriminator filter // Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation // New carrier Doppler frequency estimation
@@ -431,10 +418,10 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
double T_prn_samples; double T_prn_samples;
double K_blk_samples; double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
T_chip_seconds = 1 / (double)d_code_freq_chips; T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * (double)d_fs_in; T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)d_fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
@@ -475,17 +462,17 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
} }
} }
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
// Tracking_timestamp_secs is aligned with the PRN start sample // Tracking_timestamp_secs is aligned with the PRN start sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / (double)d_fs_in; //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / static_cast<double>(d_fs_in);
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_rem_code_phase_samples)/(double)d_fs_in; current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0; current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz; current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
// ########## DEBUG OUTPUT // ########## DEBUG OUTPUT

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

@@ -82,7 +82,7 @@ gps_l1_ca_dll_pll_make_tracking_cc(
void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items, void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call ninput_items_required[0] = static_cast<int>(d_vector_length) * 2; //set the required available samples in each call
} }
@@ -117,25 +117,20 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip // Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2]; d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
/* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory
* (gr_complex array of size 2*d_vector_length) aligned to cache of N bytes (machine dependent!)
*/
// Get space for the resampled early / prompt / late local replicas // Get space for the resampled early / prompt / late local replicas
d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_carr_sign = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// correlator outputs (scalar) // correlator outputs (scalar)
d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Prompt = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
@@ -155,7 +150,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
d_pull_in = false; d_pull_in = false;
d_last_seg = 0; d_last_seg = 0;
d_current_prn_length_samples = (int)d_vector_length; d_current_prn_length_samples = static_cast<int>(d_vector_length);
// CN0 estimation and lock detector buffers // CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
@@ -166,10 +161,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD;
systemName["G"] = std::string("GPS"); systemName["G"] = std::string("GPS");
systemName["R"] = std::string("GLONASS");
systemName["S"] = std::string("SBAS"); systemName["S"] = std::string("SBAS");
systemName["E"] = std::string("Galileo");
systemName["C"] = std::string("Compass");
} }
@@ -184,13 +176,12 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
long int acq_trk_diff_samples; long int acq_trk_diff_samples;
float acq_trk_diff_seconds; float acq_trk_diff_seconds;
acq_trk_diff_samples = (long int)d_sample_counter - (long int)d_acq_sample_stamp;//-d_vector_length; acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);//-d_vector_length;
LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in; acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
//doppler effect //doppler effect
// Fd=(C/(C+Vr))*F // Fd=(C/(C+Vr))*F
float radial_velocity; float radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler // new chip and prn sequence periods based on acq Doppler
float T_chip_mod_seconds; float T_chip_mod_seconds;
float T_prn_mod_seconds; float T_prn_mod_seconds;
@@ -198,18 +189,16 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ; d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds = 1/d_code_freq_chips; T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in; T_prn_mod_samples = T_prn_mod_seconds * static_cast<float>(d_fs_in);
d_current_prn_length_samples = round(T_prn_mod_samples); d_current_prn_length_samples = round(T_prn_mod_samples);
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in; float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
float T_prn_diff_seconds; float 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 = acq_trk_diff_seconds / T_prn_true_seconds;
float N_prn_diff;
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
float corrected_acq_phase_samples, delay_correction_samples; float corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * (float)d_fs_in), T_prn_true_samples); corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<float>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0) if (corrected_acq_phase_samples < 0)
{ {
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
@@ -226,8 +215,8 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
// generate local reference ALWAYS starting at chip 1 (1 sample per chip) // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0); gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0);
d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS]; d_ca_code[0] = d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS)];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1]; d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1];
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0; d_rem_code_phase_samples = 0;
@@ -263,19 +252,19 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
double tcode_chips; double tcode_chips;
double rem_code_phase_chips; double rem_code_phase_chips;
int associated_chip_index; int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS);
double code_phase_step_chips; double code_phase_step_chips;
int early_late_spc_samples; int early_late_spc_samples;
int epl_loop_length_samples; int epl_loop_length_samples;
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
code_phase_step_chips = ((double)d_code_freq_chips) / ((double)d_fs_in); code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in); rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / d_fs_in);
tcode_chips = -rem_code_phase_chips; tcode_chips = -rem_code_phase_chips;
// Alternative EPL code generation (40% of speed improvement!) // Alternative EPL code generation (40% of speed improvement!)
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples * 2;
for (int i = 0; i < epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
@@ -283,8 +272,8 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
tcode_chips = tcode_chips + code_phase_step_chips; tcode_chips = tcode_chips + code_phase_step_chips;
} }
memcpy(d_prompt_code,&d_early_code[early_late_spc_samples],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples * sizeof(gr_complex));
memcpy(d_late_code,&d_early_code[early_late_spc_samples*2],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_late_code, &d_early_code[early_late_spc_samples * 2], d_current_prn_length_samples * sizeof(gr_complex));
} }
@@ -294,7 +283,7 @@ 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)GPS_TWO_PI*d_carrier_doppler_hz / (float)d_fs_in; phase_step_rad = static_cast<float>(GPS_TWO_PI) * d_carrier_doppler_hz / static_cast<float>(d_fs_in);
phase_rad = d_rem_carr_phase_rad; phase_rad = d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++) for(int i = 0; i < d_current_prn_length_samples; i++)
{ {
@@ -319,8 +308,8 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc()
volk_free(d_Early); volk_free(d_Early);
volk_free(d_Prompt); volk_free(d_Prompt);
volk_free(d_Late); volk_free(d_Late);
volk_free(d_ca_code);
delete[] d_ca_code;
delete[] d_Prompt_buffer; delete[] d_Prompt_buffer;
} }
@@ -344,10 +333,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE // /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
//d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / (double)d_fs_in); //d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / static_cast<double>(d_fs_in));
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n"; //std::cout<<" samples_offset="<<samples_offset<<"\r\n";
@@ -377,8 +366,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
d_late_code, d_late_code,
d_Early, d_Early,
d_Prompt, d_Prompt,
d_Late, d_Late);
is_unaligned());
// check for samples consistency (this should be done before in the receiver / here only if the source is a file) // check for samples consistency (this should be done before in the receiver / here only if the source is a file)
if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
@@ -391,7 +379,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0; current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/(double)d_fs_in; current_synchro_data.Tracking_timestamp_secs = static_cast<double>(d_sample_counter) / static_cast<double>(d_fs_in);
current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0; current_synchro_data.CN0_dB_hz = 0.0;
@@ -404,7 +392,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// ################## PLL ########################################################## // ################## PLL ##########################################################
// PLL discriminator // PLL discriminator
carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)GPS_TWO_PI; carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / static_cast<float>(GPS_TWO_PI);
// Carrier discriminator filter // Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation // New carrier Doppler frequency estimation
@@ -434,10 +422,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
double T_prn_samples; double T_prn_samples;
double K_blk_samples; double K_blk_samples;
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
T_chip_seconds = 1 / (double)d_code_freq_chips; T_chip_seconds = 1 / static_cast<double>(d_code_freq_chips);
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * (double)d_fs_in; T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)d_fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
@@ -478,25 +466,25 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
} }
} }
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
// Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!) // Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!)
//compute remnant code phase samples BEFORE the Tracking timestamp //compute remnant code phase samples BEFORE the Tracking timestamp
//d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples)/(double)d_fs_in; //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples)/static_cast<double>(d_fs_in);
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??) // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!, but some glitches??)
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_rem_code_phase_samples)/(double)d_fs_in; current_synchro_data.Tracking_timestamp_secs = (static_cast<double>(d_sample_counter) + static_cast<double>(d_rem_code_phase_samples)) / static_cast<double>(d_fs_in);
//compute remnant code phase samples AFTER the Tracking timestamp //compute remnant code phase samples AFTER the Tracking timestamp
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
//current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/(double)d_fs_in; //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter)/static_cast<double>(d_fs_in);
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0; current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz; current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
// ########## DEBUG OUTPUT // ########## DEBUG OUTPUT
@@ -569,39 +557,39 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
try try
{ {
// EPR // EPR
d_dump_file.write((char*)&tmp_E, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
// PROMPT I and Q (to analyze navigation symbols) // PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
// PRN start sample stamp // PRN start sample stamp
//tmp_float=(float)d_sample_counter; //tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
// accumulated carrier phase // accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(float));
// carrier and code frequency // carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(float));
d_dump_file.write((char*)&d_code_freq_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
//PLL commands //PLL commands
d_dump_file.write((char*)&carr_error_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(float));
d_dump_file.write((char*)&carr_error_filt_hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
//DLL commands //DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(float));
d_dump_file.write((char*)&code_error_filt_chips, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
// CN0 and carrier lock test // CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples; tmp_float = d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write(reinterpret_cast<char*>(&tmp_float), sizeof(float));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples); tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
{ {

28
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc
View File

@@ -126,26 +126,21 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip // Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2]; d_ca_code = static_cast<gr_complex*>(volk_malloc((GPS_L1_CA_CODE_LENGTH_CHIPS + 2) * sizeof(gr_complex), volk_get_alignment()));
d_carr_sign = new gr_complex[d_vector_length*2]; d_carr_sign = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
/* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes
*/
// Get space for the resampled early / prompt / late local replicas // Get space for the resampled early / prompt / late local replicas
d_early_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_early_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_prompt_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
d_late_code=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_late_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
d_carr_sign=(gr_complex*)volk_malloc(2*d_vector_length * sizeof(gr_complex),volk_get_alignment()); d_carr_sign = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
// correlator outputs (scalar) // correlator outputs (scalar)
d_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Early = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Prompt=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Prompt = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
d_Late=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment()); d_Late = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
@@ -336,8 +331,8 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::~Gps_L1_Ca_Tcp_Connector_Tracking_cc()
volk_free(d_Early); volk_free(d_Early);
volk_free(d_Prompt); volk_free(d_Prompt);
volk_free(d_Late); volk_free(d_Late);
volk_free(d_ca_code);
delete[] d_ca_code;
delete[] d_Prompt_buffer; delete[] d_Prompt_buffer;
d_tcp_com.close_tcp_connection(d_port); d_tcp_com.close_tcp_connection(d_port);
@@ -405,8 +400,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
d_late_code, d_late_code,
d_Early, d_Early,
d_Prompt, d_Prompt,
d_Late, d_Late);
is_unaligned());
// check for samples consistency (this should be done before in the receiver / here only if the source is a file) // check for samples consistency (this should be done before in the receiver / here only if the source is a file)
if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)

259
src/algorithms/tracking/libs/correlator.cc
View File

@@ -79,234 +79,75 @@ void Correlator::Carrier_wipeoff_and_EPL_generic(int signal_length_samples, cons
void Correlator::Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, bool input_vector_unaligned) void Correlator::Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out)
{ {
gr_complex* bb_signal; gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
//gr_complex* input_aligned;
//todo: do something if posix_memalign fails volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, signal_length_samples);
if (posix_memalign((void**)&bb_signal, 16, signal_length_samples * sizeof(gr_complex)) == 0) {}; volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples);
if (input_vector_unaligned == true) volk_free(bb_signal);
{
//todo: do something if posix_memalign fails
//if (posix_memalign((void**)&input_aligned, 16, signal_length_samples * sizeof(gr_complex)) == 0){};
//memcpy(input_aligned,input,signal_length_samples * sizeof(gr_complex));
volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, signal_length_samples);
}
else
{
/*
* todo: There is a problem with the aligned version of volk_32fc_x2_multiply_32fc_a.
* It crashes even if the is_aligned() work function returns true. Im keeping the unaligned version in both cases..
*/
//use directly the input vector
volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, signal_length_samples);
}
volk_32fc_x2_dot_prod_32fc_a(E_out, bb_signal, E_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(P_out, bb_signal, P_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(L_out, bb_signal, L_code, signal_length_samples);
free(bb_signal);
//if (input_vector_unaligned==false)
//{
// free(input_aligned);
//}
} }
//void Correlator::Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out, bool input_vector_unaligned) //void Correlator::Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out)
//{ //{
// gr_complex* bb_signal; // gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
// //gr_complex* input_aligned; // volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, integration_time * prn_length_samples);
// // volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, integration_time * prn_length_samples);
// //todo: do something if posix_memalign fails // volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, integration_time * prn_length_samples);
// if (posix_memalign((void**)&bb_signal, 16, integration_time * prn_length_samples * sizeof(gr_complex)) == 0) {}; // volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, integration_time * prn_length_samples);
//
// if (input_vector_unaligned == true)
// {
// //todo: do something if posix_memalign fails
// //if (posix_memalign((void**)&input_aligned, 16, signal_length_samples * sizeof(gr_complex)) == 0){};
// //memcpy(input_aligned,input,signal_length_samples * sizeof(gr_complex));
//
// volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, integration_time * prn_length_samples);
// }
// else
// {
// /*
// * todo: There is a problem with the aligned version of volk_32fc_x2_multiply_32fc_a.
// * It crashes even if the is_aligned() work function returns true. Im keeping the unaligned version in both cases..
// */
// //use directly the input vector
// volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, integration_time * prn_length_samples);
// }
//
// volk_32fc_x2_dot_prod_32fc_a(E_out, bb_signal, E_code, integration_time * prn_length_samples);
// volk_32fc_x2_dot_prod_32fc_a(P_out, bb_signal, P_code, integration_time * prn_length_samples);
// volk_32fc_x2_dot_prod_32fc_a(L_out, bb_signal, L_code, integration_time * prn_length_samples);
// // Vector of Prompts of I code // // Vector of Prompts of I code
// for (int i = 0; i < integration_time; i++) // for (int i = 0; i < integration_time; i++)
// { // {
// volk_32fc_x2_dot_prod_32fc_a(&P_data_out[i], &bb_signal[i*prn_length_samples], P_data_code, prn_length_samples); // volk_32fc_x2_dot_prod_32fc(&P_data_out[i], &bb_signal[i*prn_length_samples], P_data_code, prn_length_samples);
// } // }
// //
// free(bb_signal); // volk_free(bb_signal);
//
//} //}
void Correlator::Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out, bool input_vector_unaligned)
void Correlator::Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out)
{ {
gr_complex* bb_signal; gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
//gr_complex* input_aligned;
bb_signal=(gr_complex*)volk_malloc(signal_length_samples * sizeof(gr_complex),volk_get_alignment()); volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples);
if (input_vector_unaligned == true) volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples);
{ volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples);
//todo: do something if posix_memalign fails volk_32fc_x2_dot_prod_32fc(P_data_out, bb_signal, P_data_code, signal_length_samples);
//if (posix_memalign((void**)&input_aligned, 16, signal_length_samples * sizeof(gr_complex)) == 0){};
//memcpy(input_aligned,input,signal_length_samples * sizeof(gr_complex));
volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, signal_length_samples);
}
else
{
/*
* todo: There is a problem with the aligned version of volk_32fc_x2_multiply_32fc_a.
* It crashes even if the is_aligned() work function returns true. Im keeping the unaligned version in both cases..
*/
//use directly the input vector
volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, signal_length_samples);
}
volk_32fc_x2_dot_prod_32fc_a(E_out, bb_signal, E_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(P_out, bb_signal, P_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(L_out, bb_signal, L_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(P_data_out, bb_signal, P_data_code, signal_length_samples);
free(bb_signal);
volk_free(bb_signal);
} }
void Correlator::Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out)
{
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
volk_32fc_x2_multiply_32fc(bb_signal, input, carrier, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(VE_out, bb_signal, VE_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc(VL_out, bb_signal, VL_code, signal_length_samples);
volk_free(bb_signal);
}
Correlator::Correlator ()
{}
Correlator::~Correlator ()
{}
#if USING_VOLK_CW_EPL_CORR_CUSTOM #if USING_VOLK_CW_EPL_CORR_CUSTOM
void Correlator::Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples, const gr_complex* input, gr_complex* carrier,gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, bool input_vector_unaligned) void Correlator::Carrier_wipeoff_and_EPL_volk_custom(int signal_length_samples, const gr_complex* input, gr_complex* carrier,gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out)
{ {
volk_cw_epl_corr_u(input, carrier, E_code, P_code, L_code, E_out, P_out, L_out, signal_length_samples); volk_cw_epl_corr_u(input, carrier, E_code, P_code, L_code, E_out, P_out, L_out, signal_length_samples);
} }
#endif #endif
void Correlator::Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out, bool input_vector_unaligned)
{
gr_complex* bb_signal;
//gr_complex* input_aligned;
bb_signal=(gr_complex*)volk_malloc(signal_length_samples * sizeof(gr_complex),volk_get_alignment());
if (input_vector_unaligned == false)
{
//todo: do something if posix_memalign fails
//if (posix_memalign((void**)&input_aligned, 16, signal_length_samples * sizeof(gr_complex)) == 0){};
//memcpy(input_aligned,input,signal_length_samples * sizeof(gr_complex));
volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, signal_length_samples);
}
else
{
//use directly the input vector
volk_32fc_x2_multiply_32fc_u(bb_signal, input, carrier, signal_length_samples);
}
volk_32fc_x2_dot_prod_32fc_a(VE_out, bb_signal, VE_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(E_out, bb_signal, E_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(P_out, bb_signal, P_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(L_out, bb_signal, L_code, signal_length_samples);
volk_32fc_x2_dot_prod_32fc_a(VL_out, bb_signal, VL_code, signal_length_samples);
free(bb_signal);
//if (input_vector_unaligned == false)
//{
//free(input_aligned);
//}
}
/*
void Correlator::cpu_arch_test_volk_32fc_x2_dot_prod_32fc_a()
{
//
//struct volk_func_desc desc=volk_32fc_x2_dot_prod_32fc_a_get_func_desc();
volk_func_desc_t desc = volk_32fc_x2_dot_prod_32fc_get_func_desc();
std::vector<std::string> arch_list;
for(int i = 0; i < desc.n_archs; ++i)
{
//if(!(archs[i+1] & volk_get_lvarch())) continue; //this arch isn't available on this pc
arch_list.push_back(std::string(desc.indices[i]));
}
//first let's get a list of available architectures for the test
if(arch_list.size() < 2)
{
std::cout << "no architectures to test" << std::endl;
this->volk_32fc_x2_dot_prod_32fc_a_best_arch = "generic";
}
else
{
std::cout << "Detected architectures in this machine for volk_32fc_x2_dot_prod_32fc_a:" << std::endl;
for (unsigned int i=0; i < arch_list.size(); ++i)
{
std::cout << "Arch " << i << ":" << arch_list.at(i) << std::endl;
}
// TODO: Make a test to find the best architecture
this->volk_32fc_x2_dot_prod_32fc_a_best_arch = arch_list.at(arch_list.size() - 1);
}
std::cout << "Selected architecture for volk_32fc_x2_dot_prod_32fc_a is " << this->volk_32fc_x2_dot_prod_32fc_a_best_arch << std::endl;
}
void Correlator::cpu_arch_test_volk_32fc_x2_multiply_32fc_a()
{
//
volk_func_desc_t desc = volk_32fc_x2_multiply_32fc_a_get_func_desc();
std::vector<std::string> arch_list;
for(int i = 0; i < desc.n_archs; ++i)
{
//if(!(archs[i+1] & volk_get_lvarch())) continue; //this arch isn't available on this pc
arch_list.push_back(std::string(desc.indices[i]));
}
this->volk_32fc_x2_multiply_32fc_a_best_arch = "generic";
//first let's get a list of available architectures for the test
if(arch_list.size() < 2)
{
std::cout << "no architectures to test" << std::endl;
}
else
{
std::cout << "Detected architectures in this machine for volk_32fc_x2_multiply_32fc_a:" << std::endl;
for (unsigned int i=0; i < arch_list.size(); ++i)
{
std::cout << "Arch " << i << ":" << arch_list.at(i) << std::endl;
if (arch_list.at(i).find("sse") != std::string::npos)
{
// TODO: Make a test to find the best architecture
this->volk_32fc_x2_multiply_32fc_a_best_arch = arch_list.at(i);
}
}
}
std::cout << "Selected architecture for volk_32fc_x2_multiply_32fc_a_best_arch is " << this->volk_32fc_x2_multiply_32fc_a_best_arch << std::endl;
}
*/
Correlator::Correlator ()
{
//cpu_arch_test_volk_32fc_x2_dot_prod_32fc_a();
//cpu_arch_test_volk_32fc_x2_multiply_32fc_a();
}
Correlator::~Correlator ()
{}

12
src/algorithms/tracking/libs/correlator.h
View File

@@ -57,10 +57,10 @@ class Correlator
{ {
public: public:
void Carrier_wipeoff_and_EPL_generic(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out); void Carrier_wipeoff_and_EPL_generic(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
void Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, bool input_vector_unaligned); void Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out);
void Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out, bool input_vector_unaligned); void Carrier_wipeoff_and_VEPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* VE_code, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* VL_code, gr_complex* VE_out, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* VL_out);
// void Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out, bool input_vector_unaligned); // void Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out);
void Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out, bool input_vector_unaligned); void Carrier_wipeoff_and_EPL_volk_IQ(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out);
Correlator(); Correlator();
~Correlator(); ~Correlator();
#ifndef USING_VOLK_CW_EPL_CORR #ifndef USING_VOLK_CW_EPL_CORR
@@ -68,11 +68,7 @@ public:
#endif #endif
private: private:
std::string volk_32fc_x2_multiply_32fc_a_best_arch;
std::string volk_32fc_x2_dot_prod_32fc_a_best_arch;
unsigned long next_power_2(unsigned long v); unsigned long next_power_2(unsigned long v);
void cpu_arch_test_volk_32fc_x2_dot_prod_32fc_a();
void cpu_arch_test_volk_32fc_x2_multiply_32fc_a();
}; };
#endif #endif

16
src/core/libs/string_converter.cc
View File

@@ -6,7 +6,7 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -16,7 +16,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -40,7 +40,7 @@ StringConverter::StringConverter()
StringConverter::~StringConverter() StringConverter::~StringConverter()
{} {}
bool StringConverter::convert(std::string value, bool default_value) bool StringConverter::convert(const std::string& value, bool default_value)
{ {
if(value.compare("true") == 0) if(value.compare("true") == 0)
{ {
@@ -57,7 +57,7 @@ bool StringConverter::convert(std::string value, bool default_value)
} }
long StringConverter::convert(std::string value, long default_value) long StringConverter::convert(const std::string& value, long default_value)
{ {
std::stringstream stream(value); std::stringstream stream(value);
@@ -75,7 +75,7 @@ long StringConverter::convert(std::string value, long default_value)
} }
int StringConverter::convert(std::string value, int default_value) int StringConverter::convert(const std::string& value, int default_value)
{ {
std::stringstream stream(value); std::stringstream stream(value);
@@ -95,7 +95,7 @@ int StringConverter::convert(std::string value, int default_value)
unsigned int StringConverter::convert(std::string value, unsigned int default_value) unsigned int StringConverter::convert(const std::string& value, unsigned int default_value)
{ {
std::stringstream stream(value); std::stringstream stream(value);
@@ -115,7 +115,7 @@ unsigned int StringConverter::convert(std::string value, unsigned int default_va
float StringConverter::convert(std::string value, float default_value) float StringConverter::convert(const std::string& value, float default_value)
{ {
std::stringstream stream(value); std::stringstream stream(value);
@@ -136,7 +136,7 @@ float StringConverter::convert(std::string value, float default_value)
double StringConverter::convert(std::string value, double default_value) double StringConverter::convert(const std::string& value, double default_value)
{ {
std::stringstream stream(value); std::stringstream stream(value);

16
src/core/libs/string_converter.h
View File

@@ -6,7 +6,7 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -16,7 +16,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -45,12 +45,12 @@ public:
StringConverter(); StringConverter();
virtual ~StringConverter(); virtual ~StringConverter();
bool convert(std::string value, bool default_value); bool convert(const std::string& value, bool default_value);
long convert(std::string value, long default_value); long convert(const std::string& value, long default_value);
int convert(std::string value, int default_value); int convert(const std::string& value, int default_value);
unsigned int convert(std::string value, unsigned int default_value); unsigned int convert(const std::string& value, unsigned int default_value);
float convert(std::string value, float default_value); float convert(const std::string& value, float default_value);
double convert(std::string value, double default_value); double convert(const std::string& value, double default_value);
}; };
#endif /*GNSS_SDR_STRING_CONVERTER_H_*/ #endif /*GNSS_SDR_STRING_CONVERTER_H_*/

15
src/core/receiver/gnss_block_factory.cc
View File

@@ -184,8 +184,8 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetObservables(std::shared
std::string default_implementation = "GPS_L1_CA_Observables"; std::string default_implementation = "GPS_L1_CA_Observables";
std::string implementation = configuration->property("Observables.implementation", default_implementation); std::string implementation = configuration->property("Observables.implementation", default_implementation);
LOG(INFO) << "Getting Observables with implementation " << implementation; LOG(INFO) << "Getting Observables with implementation " << implementation;
unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 12); unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 0);
unsigned int GPS_channels = configuration->property("Channels_GPS.count", 12); unsigned int GPS_channels = configuration->property("Channels_GPS.count", 0);
return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels, Galileo_channels + GPS_channels, queue); return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels, Galileo_channels + GPS_channels, queue);
} }
@@ -197,8 +197,8 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetPVT(std::shared_ptr<Con
std::string default_implementation = "Pass_Through"; std::string default_implementation = "Pass_Through";
std::string implementation = configuration->property("PVT.implementation", default_implementation); std::string implementation = configuration->property("PVT.implementation", default_implementation);
LOG(INFO) << "Getting PVT with implementation " << implementation; LOG(INFO) << "Getting PVT with implementation " << implementation;
unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 12); unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 0);
unsigned int GPS_channels = configuration->property("Channels_GPS.count", 12); unsigned int GPS_channels = configuration->property("Channels_GPS.count", 0);
return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels, 1, queue); return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels, 1, queue);
} }
@@ -280,7 +280,7 @@ std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFacto
unsigned int channel_absolute_id=0; unsigned int channel_absolute_id=0;
//**************** GPS CHANNELS ********************** //**************** GPS CHANNELS **********************
channel_count= configuration->property("Channels_GPS.count", 12); channel_count= configuration->property("Channels_GPS.count", 0);
LOG(INFO) << "Getting " << channel_count << " GPS channels"; LOG(INFO) << "Getting " << channel_count << " GPS channels";
@@ -304,7 +304,7 @@ std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFacto
} }
//**************** GALILEO CHANNELS ********************** //**************** GALILEO CHANNELS **********************
channel_count= configuration->property("Channels_Galileo.count", 12); channel_count= configuration->property("Channels_Galileo.count", 0);
LOG(INFO) << "Getting " << channel_count << " Galileo channels"; LOG(INFO) << "Getting " << channel_count << " Galileo channels";
@@ -325,9 +325,6 @@ std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFacto
acquisition_implementation, tracking, telemetry_decoder, channel_absolute_id, queue))); acquisition_implementation, tracking, telemetry_decoder, channel_absolute_id, queue)));
channel_absolute_id++; channel_absolute_id++;
} }
return channels; return channels;
} }

249
src/core/system_parameters/galileo_fnav_message.cc
View File

@@ -194,8 +194,8 @@ Galileo_Fnav_Message::Galileo_Fnav_Message()
//} //}
void Galileo_Fnav_Message::split_page(std::string page_string) void Galileo_Fnav_Message::split_page(std::string page_string)
{ {
std::string message_word = page_string.substr(0,214); std::string message_word = page_string.substr(0, 214);
std::string CRC_data = page_string.substr(214,24); std::string CRC_data = page_string.substr(214, 24);
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> Word_for_CRC_bits(message_word); std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> Word_for_CRC_bits(message_word);
std::bitset<24> checksum(CRC_data); std::bitset<24> checksum(CRC_data);
if (_CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true) if (_CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true)
@@ -210,7 +210,7 @@ void Galileo_Fnav_Message::split_page(std::string page_string)
} }
} }
bool Galileo_Fnav_Message::_CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits,boost::uint32_t checksum) bool Galileo_Fnav_Message::_CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, boost::uint32_t checksum)
{ {
CRC_Galileo_FNAV_type CRC_Galileo; CRC_Galileo_FNAV_type CRC_Galileo;
@@ -225,7 +225,7 @@ bool Galileo_Fnav_Message::_CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> b
std::vector<unsigned char> bytes; std::vector<unsigned char> bytes;
boost::to_block_range(frame_bits, std::back_inserter(bytes)); boost::to_block_range(frame_bits, std::back_inserter(bytes));
std::reverse(bytes.begin(),bytes.end()); std::reverse(bytes.begin(), bytes.end());
CRC_Galileo.process_bytes( bytes.data(), GALILEO_FNAV_DATA_FRAME_BYTES ); CRC_Galileo.process_bytes( bytes.data(), GALILEO_FNAV_DATA_FRAME_BYTES );
@@ -242,206 +242,206 @@ bool Galileo_Fnav_Message::_CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> b
void Galileo_Fnav_Message::decode_page(std::string data) void Galileo_Fnav_Message::decode_page(std::string data)
{ {
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> data_bits(data); std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> data_bits(data);
page_type = read_navigation_unsigned(data_bits,FNAV_PAGE_TYPE_bit); page_type = read_navigation_unsigned(data_bits, FNAV_PAGE_TYPE_bit);
switch(page_type) switch(page_type)
{ {
case 1: // SVID, Clock correction, SISA, Ionospheric correction, BGD, GST, Signal health and Data validity status case 1: // SVID, Clock correction, SISA, Ionospheric correction, BGD, GST, Signal health and Data validity status
FNAV_SV_ID_PRN_1=(int)read_navigation_unsigned(data_bits,FNAV_SV_ID_PRN_1_bit); FNAV_SV_ID_PRN_1 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SV_ID_PRN_1_bit));
FNAV_IODnav_1=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_1_bit); FNAV_IODnav_1 =static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_1_bit));
FNAV_t0c_1=(double)read_navigation_unsigned(data_bits,FNAV_t0c_1_bit); FNAV_t0c_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_t0c_1_bit));
FNAV_t0c_1 *= FNAV_t0c_1_LSB; FNAV_t0c_1 *= FNAV_t0c_1_LSB;
FNAV_af0_1=(double)read_navigation_signed(data_bits,FNAV_af0_1_bit); FNAV_af0_1 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af0_1_bit));
FNAV_af0_1 *= FNAV_af0_1_LSB; FNAV_af0_1 *= FNAV_af0_1_LSB;
FNAV_af1_1=(double)read_navigation_signed(data_bits,FNAV_af1_1_bit); FNAV_af1_1 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af1_1_bit));
FNAV_af1_1 *= FNAV_af1_1_LSB; FNAV_af1_1 *= FNAV_af1_1_LSB;
FNAV_af2_1=(double)read_navigation_signed(data_bits,FNAV_af2_1_bit); FNAV_af2_1 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af2_1_bit));
FNAV_af2_1 *= FNAV_af2_1_LSB; FNAV_af2_1 *= FNAV_af2_1_LSB;
FNAV_SISA_1=(double)read_navigation_unsigned(data_bits,FNAV_SISA_1_bit); FNAV_SISA_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_SISA_1_bit));
FNAV_ai0_1=(double)read_navigation_unsigned(data_bits,FNAV_ai0_1_bit); FNAV_ai0_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_ai0_1_bit));
FNAV_ai0_1 *= FNAV_ai0_1_LSB; FNAV_ai0_1 *= FNAV_ai0_1_LSB;
FNAV_ai1_1=(double)read_navigation_signed(data_bits,FNAV_ai1_1_bit); FNAV_ai1_1 = static_cast<double>(read_navigation_signed(data_bits, FNAV_ai1_1_bit));
FNAV_ai1_1 *= FNAV_ai1_1_LSB; FNAV_ai1_1 *= FNAV_ai1_1_LSB;
FNAV_ai2_1=(double)read_navigation_signed(data_bits,FNAV_ai2_1_bit); FNAV_ai2_1 = static_cast<double>(read_navigation_signed(data_bits, FNAV_ai2_1_bit));
FNAV_ai2_1 *= FNAV_ai2_1_LSB; FNAV_ai2_1 *= FNAV_ai2_1_LSB;
FNAV_region1_1=(bool)read_navigation_unsigned(data_bits,FNAV_region1_1_bit); FNAV_region1_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region1_1_bit));
FNAV_region2_1=(bool)read_navigation_unsigned(data_bits,FNAV_region2_1_bit); FNAV_region2_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region2_1_bit));
FNAV_region3_1=(bool)read_navigation_unsigned(data_bits,FNAV_region3_1_bit); FNAV_region3_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region3_1_bit));
FNAV_region4_1=(bool)read_navigation_unsigned(data_bits,FNAV_region4_1_bit); FNAV_region4_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region4_1_bit));
FNAV_region5_1=(bool)read_navigation_unsigned(data_bits,FNAV_region5_1_bit); FNAV_region5_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region5_1_bit));
FNAV_BGD_1=(double)read_navigation_signed(data_bits,FNAV_BGD_1_bit); FNAV_BGD_1 = static_cast<double>(read_navigation_signed(data_bits, FNAV_BGD_1_bit));
FNAV_BGD_1 *= FNAV_BGD_1_LSB; FNAV_BGD_1 *= FNAV_BGD_1_LSB;
FNAV_E5ahs_1=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_1_bit); FNAV_E5ahs_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5ahs_1_bit));
FNAV_WN_1=(double)read_navigation_unsigned(data_bits,FNAV_WN_1_bit); FNAV_WN_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN_1_bit));
FNAV_TOW_1=(double)read_navigation_unsigned(data_bits,FNAV_TOW_1_bit); FNAV_TOW_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_1_bit));
FNAV_E5advs_1=(double)read_navigation_unsigned(data_bits,FNAV_E5advs_1_bit); FNAV_E5advs_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5advs_1_bit));
flag_TOW_1=true; flag_TOW_1 = true;
flag_TOW_set=true; flag_TOW_set = true;
flag_iono_and_GST = true; //set to false externally flag_iono_and_GST = true; //set to false externally
break; break;
case 2: // Ephemeris (1/3) and GST case 2: // Ephemeris (1/3) and GST
FNAV_IODnav_2=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_2_bit); FNAV_IODnav_2 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_2_bit));
FNAV_M0_2=(double)read_navigation_unsigned(data_bits,FNAV_M0_2_bit); FNAV_M0_2 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_M0_2_bit));
FNAV_M0_2 *= FNAV_M0_2_LSB; FNAV_M0_2 *= FNAV_M0_2_LSB;
FNAV_omegadot_2=(double)read_navigation_signed(data_bits,FNAV_omegadot_2_bit); FNAV_omegadot_2 = static_cast<double>(read_navigation_signed(data_bits, FNAV_omegadot_2_bit));
FNAV_omegadot_2 *= FNAV_omegadot_2_LSB; FNAV_omegadot_2 *= FNAV_omegadot_2_LSB;
FNAV_e_2=(double)read_navigation_unsigned(data_bits,FNAV_e_2_bit); FNAV_e_2 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_e_2_bit));
FNAV_e_2 *= FNAV_e_2_LSB; FNAV_e_2 *= FNAV_e_2_LSB;
FNAV_a12_2=(double)read_navigation_unsigned(data_bits,FNAV_a12_2_bit); FNAV_a12_2 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_a12_2_bit));
FNAV_a12_2 *= FNAV_a12_2_LSB; FNAV_a12_2 *= FNAV_a12_2_LSB;
FNAV_omega0_2=(double)read_navigation_signed(data_bits,FNAV_omega0_2_bit); FNAV_omega0_2 = static_cast<double>(read_navigation_signed(data_bits, FNAV_omega0_2_bit));
FNAV_omega0_2 *= FNAV_omega0_2_LSB; FNAV_omega0_2 *= FNAV_omega0_2_LSB;
FNAV_idot_2=(double)read_navigation_signed(data_bits,FNAV_idot_2_bit); FNAV_idot_2 = static_cast<double>(read_navigation_signed(data_bits, FNAV_idot_2_bit));
FNAV_idot_2 *= FNAV_idot_2_LSB; FNAV_idot_2 *= FNAV_idot_2_LSB;
FNAV_WN_2=(double)read_navigation_unsigned(data_bits,FNAV_WN_2_bit); FNAV_WN_2 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN_2_bit));
FNAV_TOW_2=(double)read_navigation_unsigned(data_bits,FNAV_TOW_2_bit); FNAV_TOW_2 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_2_bit));
flag_TOW_2=true; flag_TOW_2 = true;
flag_TOW_set=true; flag_TOW_set = true;
flag_ephemeris_1=true; flag_ephemeris_1 = true;
break; break;
case 3: // Ephemeris (2/3) and GST case 3: // Ephemeris (2/3) and GST
FNAV_IODnav_3=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_3_bit); FNAV_IODnav_3 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_3_bit));
FNAV_i0_3=(double)read_navigation_signed(data_bits,FNAV_i0_3_bit); FNAV_i0_3 = static_cast<double>(read_navigation_signed(data_bits, FNAV_i0_3_bit));
FNAV_i0_3 *= FNAV_i0_3_LSB; FNAV_i0_3 *= FNAV_i0_3_LSB;
FNAV_w_3=(double)read_navigation_signed(data_bits,FNAV_w_3_bit); FNAV_w_3=static_cast<double>(read_navigation_signed(data_bits, FNAV_w_3_bit));
FNAV_w_3 *= FNAV_w_3_LSB; FNAV_w_3 *= FNAV_w_3_LSB;
FNAV_deltan_3=(double)read_navigation_unsigned(data_bits,FNAV_deltan_3_bit); FNAV_deltan_3 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_deltan_3_bit));
FNAV_deltan_3 *= FNAV_deltan_3_LSB; FNAV_deltan_3 *= FNAV_deltan_3_LSB;
FNAV_Cuc_3=(double)read_navigation_signed(data_bits,FNAV_Cuc_3_bit); FNAV_Cuc_3 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Cuc_3_bit));
FNAV_Cuc_3 *= FNAV_Cuc_3_LSB; FNAV_Cuc_3 *= FNAV_Cuc_3_LSB;
FNAV_Cus_3=(double)read_navigation_signed(data_bits,FNAV_Cus_3_bit); FNAV_Cus_3 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Cus_3_bit));
FNAV_Cus_3 *= FNAV_Cus_3_LSB; FNAV_Cus_3 *= FNAV_Cus_3_LSB;
FNAV_Crc_3=(double)read_navigation_signed(data_bits,FNAV_Crc_3_bit); FNAV_Crc_3 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Crc_3_bit));
FNAV_Crc_3 *= FNAV_Crc_3_LSB; FNAV_Crc_3 *= FNAV_Crc_3_LSB;
FNAV_Crs_3=(double)read_navigation_signed(data_bits,FNAV_Crs_3_bit); FNAV_Crs_3 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Crs_3_bit));
FNAV_Crs_3 *= FNAV_Crs_3_LSB; FNAV_Crs_3 *= FNAV_Crs_3_LSB;
FNAV_t0e_3=(double)read_navigation_unsigned(data_bits,FNAV_t0e_3_bit); FNAV_t0e_3 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_t0e_3_bit));
FNAV_t0e_3 *= FNAV_t0e_3_LSB; FNAV_t0e_3 *= FNAV_t0e_3_LSB;
FNAV_WN_3=(double)read_navigation_unsigned(data_bits,FNAV_WN_3_bit); FNAV_WN_3 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN_3_bit));
FNAV_TOW_3=(double)read_navigation_unsigned(data_bits,FNAV_TOW_3_bit); FNAV_TOW_3 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_3_bit));
flag_TOW_3=true; flag_TOW_3 = true;
flag_TOW_set=true; flag_TOW_set = true;
flag_ephemeris_2=true; flag_ephemeris_2 = true;
break; break;
case 4: // Ephemeris (3/3), GST-UTC conversion, GST-GPS conversion and TOW case 4: // Ephemeris (3/3), GST-UTC conversion, GST-GPS conversion and TOW
FNAV_IODnav_4=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_4_bit); FNAV_IODnav_4 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_4_bit));
FNAV_Cic_4=(double)read_navigation_unsigned(data_bits,FNAV_Cic_4_bit); FNAV_Cic_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_Cic_4_bit));
FNAV_Cic_4 *= FNAV_Cic_4_LSB; FNAV_Cic_4 *= FNAV_Cic_4_LSB;
FNAV_Cis_4=(double)read_navigation_unsigned(data_bits,FNAV_Cis_4_bit); FNAV_Cis_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_Cis_4_bit));
FNAV_Cis_4 *= FNAV_Cis_4_LSB; FNAV_Cis_4 *= FNAV_Cis_4_LSB;
FNAV_A0_4=(double)read_navigation_unsigned(data_bits,FNAV_A0_4_bit); FNAV_A0_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_A0_4_bit));
FNAV_A0_4 *= FNAV_A0_4_LSB; FNAV_A0_4 *= FNAV_A0_4_LSB;
FNAV_A1_4=(double)read_navigation_unsigned(data_bits,FNAV_A1_4_bit); FNAV_A1_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_A1_4_bit));
FNAV_A1_4 *= FNAV_A1_4_LSB; FNAV_A1_4 *= FNAV_A1_4_LSB;
FNAV_deltatls_4=(double)read_navigation_signed(data_bits,FNAV_deltatls_4_bit); FNAV_deltatls_4 = static_cast<double>(read_navigation_signed(data_bits, FNAV_deltatls_4_bit));
FNAV_t0t_4=(double)read_navigation_unsigned(data_bits,FNAV_t0t_4_bit); FNAV_t0t_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_t0t_4_bit));
FNAV_t0t_4 *= FNAV_t0t_4_LSB; FNAV_t0t_4 *= FNAV_t0t_4_LSB;
FNAV_WNot_4=(double)read_navigation_unsigned(data_bits,FNAV_WNot_4_bit); FNAV_WNot_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WNot_4_bit));
FNAV_WNlsf_4=(double)read_navigation_unsigned(data_bits,FNAV_WNlsf_4_bit); FNAV_WNlsf_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WNlsf_4_bit));
FNAV_DN_4=(double)read_navigation_unsigned(data_bits,FNAV_DN_4_bit); FNAV_DN_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_DN_4_bit));
FNAV_deltatlsf_4=(double)read_navigation_signed(data_bits,FNAV_deltatlsf_4_bit); FNAV_deltatlsf_4 = static_cast<double>(read_navigation_signed(data_bits, FNAV_deltatlsf_4_bit));
FNAV_t0g_4=(double)read_navigation_unsigned(data_bits,FNAV_t0g_4_bit); FNAV_t0g_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_t0g_4_bit));
FNAV_t0g_4 *= FNAV_t0g_4_LSB; FNAV_t0g_4 *= FNAV_t0g_4_LSB;
FNAV_A0g_4=(double)read_navigation_signed(data_bits,FNAV_A0g_4_bit); FNAV_A0g_4 = static_cast<double>(read_navigation_signed(data_bits, FNAV_A0g_4_bit));
FNAV_A0g_4 *= FNAV_A0g_4_LSB; FNAV_A0g_4 *= FNAV_A0g_4_LSB;
FNAV_A1g_4=(double)read_navigation_signed(data_bits,FNAV_A1g_4_bit); FNAV_A1g_4 = static_cast<double>(read_navigation_signed(data_bits, FNAV_A1g_4_bit));
FNAV_A1g_4 *= FNAV_A1g_4_LSB; FNAV_A1g_4 *= FNAV_A1g_4_LSB;
FNAV_WN0g_4=(double)read_navigation_unsigned(data_bits,FNAV_WN0g_4_bit); FNAV_WN0g_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN0g_4_bit));
FNAV_TOW_4=(double)read_navigation_unsigned(data_bits,FNAV_TOW_4_bit); FNAV_TOW_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_4_bit));
flag_TOW_4=true; flag_TOW_4 = true;
flag_TOW_set=true; flag_TOW_set = true;
flag_ephemeris_3=true; flag_ephemeris_3 = true;
flag_utc_model = true; //set to false externally flag_utc_model = true; //set to false externally
break; break;
case 5: // Almanac (SVID1 and SVID2(1/2)), Week Number and almanac reference time case 5: // Almanac (SVID1 and SVID2(1/2)), Week Number and almanac reference time
FNAV_IODa_5=(int)read_navigation_unsigned(data_bits,FNAV_IODa_5_bit); FNAV_IODa_5 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODa_5_bit));
FNAV_WNa_5=(double)read_navigation_unsigned(data_bits,FNAV_WNa_5_bit); FNAV_WNa_5 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WNa_5_bit));
FNAV_t0a_5=(double)read_navigation_unsigned(data_bits,FNAV_t0a_5_bit); FNAV_t0a_5 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_t0a_5_bit));
FNAV_t0a_5 *= FNAV_t0a_5_LSB; FNAV_t0a_5 *= FNAV_t0a_5_LSB;
FNAV_SVID1_5=(int)read_navigation_unsigned(data_bits,FNAV_SVID1_5_bit); FNAV_SVID1_5 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SVID1_5_bit));
FNAV_Deltaa12_1_5=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_1_5_bit); FNAV_Deltaa12_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Deltaa12_1_5_bit));
FNAV_Deltaa12_1_5 *= FNAV_Deltaa12_5_LSB; FNAV_Deltaa12_1_5 *= FNAV_Deltaa12_5_LSB;
FNAV_e_1_5=(double)read_navigation_unsigned(data_bits,FNAV_e_1_5_bit); FNAV_e_1_5 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_e_1_5_bit));
FNAV_e_1_5 *= FNAV_e_5_LSB; FNAV_e_1_5 *= FNAV_e_5_LSB;
FNAV_w_1_5=(double)read_navigation_signed(data_bits,FNAV_w_1_5_bit); FNAV_w_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_w_1_5_bit));
FNAV_w_1_5 *= FNAV_w_5_LSB; FNAV_w_1_5 *= FNAV_w_5_LSB;
FNAV_deltai_1_5=(double)read_navigation_signed(data_bits,FNAV_deltai_1_5_bit); FNAV_deltai_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_deltai_1_5_bit));
FNAV_deltai_1_5 *= FNAV_deltai_5_LSB; FNAV_deltai_1_5 *= FNAV_deltai_5_LSB;
FNAV_Omega0_1_5=(double)read_navigation_signed(data_bits,FNAV_Omega0_1_5_bit); FNAV_Omega0_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Omega0_1_5_bit));
FNAV_Omega0_1_5 *= FNAV_Omega0_5_LSB; FNAV_Omega0_1_5 *= FNAV_Omega0_5_LSB;
FNAV_Omegadot_1_5=(double)read_navigation_signed(data_bits,FNAV_Omegadot_1_5_bit); FNAV_Omegadot_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Omegadot_1_5_bit));
FNAV_Omegadot_1_5 *= FNAV_Omegadot_5_LSB; FNAV_Omegadot_1_5 *= FNAV_Omegadot_5_LSB;
FNAV_M0_1_5=(double)read_navigation_signed(data_bits,FNAV_M0_1_5_bit); FNAV_M0_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_M0_1_5_bit));
FNAV_M0_1_5 *= FNAV_M0_5_LSB; FNAV_M0_1_5 *= FNAV_M0_5_LSB;
FNAV_af0_1_5=(double)read_navigation_signed(data_bits,FNAV_af0_1_5_bit); FNAV_af0_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af0_1_5_bit));
FNAV_af0_1_5 *= FNAV_af0_5_LSB; FNAV_af0_1_5 *= FNAV_af0_5_LSB;
FNAV_af1_1_5=(double)read_navigation_signed(data_bits,FNAV_af1_1_5_bit); FNAV_af1_1_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af1_1_5_bit));
FNAV_af1_1_5 *= FNAV_af1_5_LSB; FNAV_af1_1_5 *= FNAV_af1_5_LSB;
FNAV_E5ahs_1_5=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_1_5_bit); FNAV_E5ahs_1_5 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5ahs_1_5_bit));
FNAV_SVID2_5=(int)read_navigation_unsigned(data_bits,FNAV_SVID2_5_bit); FNAV_SVID2_5 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SVID2_5_bit));
FNAV_Deltaa12_2_5=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_2_5_bit); FNAV_Deltaa12_2_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Deltaa12_2_5_bit));
FNAV_Deltaa12_2_5 *= FNAV_Deltaa12_5_LSB; FNAV_Deltaa12_2_5 *= FNAV_Deltaa12_5_LSB;
FNAV_e_2_5=(double)read_navigation_unsigned(data_bits,FNAV_e_2_5_bit); FNAV_e_2_5 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_e_2_5_bit));
FNAV_e_2_5 *= FNAV_e_5_LSB; FNAV_e_2_5 *= FNAV_e_5_LSB;
FNAV_w_2_5=(double)read_navigation_signed(data_bits,FNAV_w_2_5_bit); FNAV_w_2_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_w_2_5_bit));
FNAV_w_2_5 *= FNAV_w_5_LSB; FNAV_w_2_5 *= FNAV_w_5_LSB;
FNAV_deltai_2_5=(double)read_navigation_signed(data_bits,FNAV_deltai_2_5_bit); FNAV_deltai_2_5 = static_cast<double>(read_navigation_signed(data_bits, FNAV_deltai_2_5_bit));
FNAV_deltai_2_5 *= FNAV_deltai_5_LSB; FNAV_deltai_2_5 *= FNAV_deltai_5_LSB;
//TODO check this //TODO check this
// Omega0_2 must be decoded when the two pieces are joined // Omega0_2 must be decoded when the two pieces are joined
omega0_1=data.substr(210,4); omega0_1 = data.substr(210, 4);
//omega_flag=true; //omega_flag=true;
// //
//FNAV_Omega012_2_5=(double)read_navigation_signed(data_bits,FNAV_Omega012_2_5_bit); //FNAV_Omega012_2_5=static_cast<double>(read_navigation_signed(data_bits, FNAV_Omega012_2_5_bit);
flag_almanac_1=true; flag_almanac_1 = true;
break; break;
case 6: // Almanac (SVID2(2/2) and SVID3) case 6: // Almanac (SVID2(2/2) and SVID3)
FNAV_IODa_6=(int)read_navigation_unsigned(data_bits,FNAV_IODa_6_bit); FNAV_IODa_6 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODa_6_bit));
/* Don't worry about omega pieces. If page 5 has not been received, all_ephemeris /* Don't worry about omega pieces. If page 5 has not been received, all_ephemeris
* flag will be set to false and the data won't be recorded.*/ * flag will be set to false and the data won't be recorded.*/
std::string omega0_2 = data.substr(10,12); std::string omega0_2 = data.substr(10, 12);
std::string Omega0 = omega0_1 + omega0_2; std::string Omega0 = omega0_1 + omega0_2;
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0); std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0);
const std::vector<std::pair<int,int>> om_bit({{0,12}}); const std::vector<std::pair<int, int>> om_bit({{0, 12}});
FNAV_Omega0_2_6=(double)read_navigation_signed(omega_bits,om_bit); FNAV_Omega0_2_6 = static_cast<double>(read_navigation_signed(omega_bits, om_bit));
FNAV_Omega0_2_6 *= FNAV_Omega0_5_LSB; FNAV_Omega0_2_6 *= FNAV_Omega0_5_LSB;
// //
FNAV_Omegadot_2_6=(double)read_navigation_signed(data_bits,FNAV_Omegadot_2_6_bit); FNAV_Omegadot_2_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Omegadot_2_6_bit));
FNAV_Omegadot_2_6 *= FNAV_Omegadot_5_LSB; FNAV_Omegadot_2_6 *= FNAV_Omegadot_5_LSB;
FNAV_M0_2_6=(double)read_navigation_signed(data_bits,FNAV_M0_2_6_bit); FNAV_M0_2_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_M0_2_6_bit));
FNAV_M0_2_6 *= FNAV_M0_5_LSB; FNAV_M0_2_6 *= FNAV_M0_5_LSB;
FNAV_af0_2_6=(double)read_navigation_signed(data_bits,FNAV_af0_2_6_bit); FNAV_af0_2_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af0_2_6_bit));
FNAV_af0_2_6 *= FNAV_af0_5_LSB; FNAV_af0_2_6 *= FNAV_af0_5_LSB;
FNAV_af1_2_6=(double)read_navigation_signed(data_bits,FNAV_af1_2_6_bit); FNAV_af1_2_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af1_2_6_bit));
FNAV_af1_2_6 *= FNAV_af1_5_LSB; FNAV_af1_2_6 *= FNAV_af1_5_LSB;
FNAV_E5ahs_2_6=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_2_6_bit); FNAV_E5ahs_2_6 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5ahs_2_6_bit));
FNAV_SVID3_6=(int)read_navigation_unsigned(data_bits,FNAV_SVID3_6_bit); FNAV_SVID3_6 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SVID3_6_bit));
FNAV_Deltaa12_3_6=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_3_6_bit); FNAV_Deltaa12_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Deltaa12_3_6_bit));
FNAV_Deltaa12_3_6 *= FNAV_Deltaa12_5_LSB; FNAV_Deltaa12_3_6 *= FNAV_Deltaa12_5_LSB;
FNAV_e_3_6=(double)read_navigation_unsigned(data_bits,FNAV_e_3_6_bit); FNAV_e_3_6 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_e_3_6_bit));
FNAV_e_3_6 *= FNAV_e_5_LSB; FNAV_e_3_6 *= FNAV_e_5_LSB;
FNAV_w_3_6=(double)read_navigation_signed(data_bits,FNAV_w_3_6_bit); FNAV_w_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_w_3_6_bit));
FNAV_w_3_6 *= FNAV_w_5_LSB; FNAV_w_3_6 *= FNAV_w_5_LSB;
FNAV_deltai_3_6=(double)read_navigation_signed(data_bits,FNAV_deltai_3_6_bit); FNAV_deltai_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_deltai_3_6_bit));
FNAV_deltai_3_6 *= FNAV_deltai_5_LSB; FNAV_deltai_3_6 *= FNAV_deltai_5_LSB;
FNAV_Omega0_3_6=(double)read_navigation_signed(data_bits,FNAV_Omega0_3_6_bit); FNAV_Omega0_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Omega0_3_6_bit));
FNAV_Omega0_3_6 *= FNAV_Omega0_5_LSB; FNAV_Omega0_3_6 *= FNAV_Omega0_5_LSB;
FNAV_Omegadot_3_6=(double)read_navigation_signed(data_bits,FNAV_Omegadot_3_6_bit); FNAV_Omegadot_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_Omegadot_3_6_bit));
FNAV_Omegadot_3_6 *= FNAV_Omegadot_5_LSB; FNAV_Omegadot_3_6 *= FNAV_Omegadot_5_LSB;
FNAV_M0_3_6=(double)read_navigation_signed(data_bits,FNAV_M0_3_6_bit); FNAV_M0_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_M0_3_6_bit));
FNAV_M0_3_6 *= FNAV_M0_5_LSB; FNAV_M0_3_6 *= FNAV_M0_5_LSB;
FNAV_af0_3_6=(double)read_navigation_signed(data_bits,FNAV_af0_3_6_bit); FNAV_af0_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af0_3_6_bit));
FNAV_af0_3_6 *= FNAV_af0_5_LSB; FNAV_af0_3_6 *= FNAV_af0_5_LSB;
FNAV_af1_3_6=(double)read_navigation_signed(data_bits,FNAV_af1_3_6_bit); FNAV_af1_3_6 = static_cast<double>(read_navigation_signed(data_bits, FNAV_af1_3_6_bit));
FNAV_af1_3_6 *= FNAV_af1_5_LSB; FNAV_af1_3_6 *= FNAV_af1_5_LSB;
FNAV_E5ahs_3_6=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_3_6_bit); FNAV_E5ahs_3_6 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5ahs_3_6_bit));
flag_almanac_2=true; flag_almanac_2 = true;
break; break;
} }
@@ -528,6 +528,7 @@ signed long int Galileo_Fnav_Message::read_navigation_signed(std::bitset<GALILEO
return value; return value;
} }
bool Galileo_Fnav_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class bool Galileo_Fnav_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class
{ {
if ((flag_ephemeris_1 == true) and (flag_ephemeris_2 == true) and (flag_ephemeris_3 == true) and (flag_iono_and_GST == true)) if ((flag_ephemeris_1 == true) and (flag_ephemeris_2 == true) and (flag_ephemeris_3 == true) and (flag_iono_and_GST == true))
@@ -552,6 +553,8 @@ bool Galileo_Fnav_Message::have_new_ephemeris() //Check if we have a new ephemer
else else
return false; return false;
} }
bool Galileo_Fnav_Message::have_new_iono_and_GST() //Check if we have a new iono data set stored in the galileo navigation class bool Galileo_Fnav_Message::have_new_iono_and_GST() //Check if we have a new iono data set stored in the galileo navigation class
{ {
if ((flag_iono_and_GST == true) and (flag_utc_model == true)) //the condition on flag_utc_model is added to have a time stamp for iono if ((flag_iono_and_GST == true) and (flag_utc_model == true)) //the condition on flag_utc_model is added to have a time stamp for iono
@@ -562,6 +565,8 @@ bool Galileo_Fnav_Message::have_new_iono_and_GST() //Check if we have a new iono
else else
return false; return false;
} }
bool Galileo_Fnav_Message::have_new_utc_model() // Check if we have a new utc data set stored in the galileo navigation class bool Galileo_Fnav_Message::have_new_utc_model() // Check if we have a new utc data set stored in the galileo navigation class
{ {
if (flag_utc_model == true) if (flag_utc_model == true)
@@ -572,6 +577,8 @@ bool Galileo_Fnav_Message::have_new_utc_model() // Check if we have a new utc da
else else
return false; return false;
} }
bool Galileo_Fnav_Message::have_new_almanac() //Check if we have a new almanac data set stored in the galileo navigation class bool Galileo_Fnav_Message::have_new_almanac() //Check if we have a new almanac data set stored in the galileo navigation class
{ {
if ((flag_almanac_1 == true) and (flag_almanac_2 == true)) if ((flag_almanac_1 == true) and (flag_almanac_2 == true))
@@ -586,6 +593,8 @@ bool Galileo_Fnav_Message::have_new_almanac() //Check if we have a new almanac d
return false; return false;
} }
Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris() Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris()
{ {
Galileo_Ephemeris ephemeris; Galileo_Ephemeris ephemeris;
@@ -622,6 +631,8 @@ Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris()
return ephemeris; return ephemeris;
} }
Galileo_Iono Galileo_Fnav_Message::get_iono() Galileo_Iono Galileo_Fnav_Message::get_iono()
{ {
Galileo_Iono iono; Galileo_Iono iono;
@@ -644,6 +655,8 @@ Galileo_Iono Galileo_Fnav_Message::get_iono()
return iono; return iono;
} }
Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model() Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model()
{ {
Galileo_Utc_Model utc_model; Galileo_Utc_Model utc_model;
@@ -664,6 +677,8 @@ Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model()
return utc_model; return utc_model;
} }
Galileo_Almanac Galileo_Fnav_Message::get_almanac() Galileo_Almanac Galileo_Fnav_Message::get_almanac()
{ {
Galileo_Almanac almanac; Galileo_Almanac almanac;

222
src/core/system_parameters/galileo_navigation_message.cc
View File

@@ -66,11 +66,11 @@ void Galileo_Navigation_Message::reset()
flag_TOW_5 = 0; flag_TOW_5 = 0;
flag_TOW_set = false; flag_TOW_set = false;
flag_GGTO= false; flag_GGTO = false;
flag_GGTO_1= false; flag_GGTO_1 = false;
flag_GGTO_2= false; flag_GGTO_2 = false;
flag_GGTO_3= false; flag_GGTO_3 = false;
flag_GGTO_4= false; flag_GGTO_4 = false;
IOD_ephemeris = 0; IOD_ephemeris = 0;
/*Word type 1: Ephemeris (1/4)*/ /*Word type 1: Ephemeris (1/4)*/
@@ -420,7 +420,7 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
// CRC correct: Decode word // CRC correct: Decode word
std::string page_number_bits = Data_k.substr (0,6); std::string page_number_bits = Data_k.substr (0,6);
std::bitset<GALILEO_PAGE_TYPE_BITS> page_type_bits (page_number_bits); // from string to bitset std::bitset<GALILEO_PAGE_TYPE_BITS> page_type_bits (page_number_bits); // from string to bitset
Page_type = (int)read_page_type_unsigned(page_type_bits, type); Page_type = static_cast<int>(read_page_type_unsigned(page_type_bits, type));
Page_type_time_stamp = Page_type; Page_type_time_stamp = Page_type;
std::string Data_jk_ephemeris = Data_k + Data_j; std::string Data_jk_ephemeris = Data_k + Data_j;
page_jk_decoder(Data_jk_ephemeris.c_str()); page_jk_decoder(Data_jk_ephemeris.c_str());
@@ -677,24 +677,24 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
std::bitset<GALILEO_DATA_JK_BITS> data_jk_bits (data_jk_string); std::bitset<GALILEO_DATA_JK_BITS> data_jk_bits (data_jk_string);
//DLOG(INFO) << "Data_jk_bits (bitset) "<< endl << data_jk_bits << endl; //DLOG(INFO) << "Data_jk_bits (bitset) "<< endl << data_jk_bits << endl;
page_number = (int)read_navigation_unsigned(data_jk_bits, PAGE_TYPE_bit); page_number = static_cast<int>(read_navigation_unsigned(data_jk_bits, PAGE_TYPE_bit));
LOG(INFO) << "Page number = " << page_number; LOG(INFO) << "Page number = " << page_number;
switch (page_number) switch (page_number)
{ {
case 1: /*Word type 1: Ephemeris (1/4)*/ case 1: /*Word type 1: Ephemeris (1/4)*/
IOD_nav_1 = (int)read_navigation_unsigned(data_jk_bits, IOD_nav_1_bit); IOD_nav_1 = static_cast<int>(read_navigation_unsigned(data_jk_bits, IOD_nav_1_bit));
DLOG(INFO) << "IOD_nav_1= " << IOD_nav_1; DLOG(INFO) << "IOD_nav_1= " << IOD_nav_1;
t0e_1 = (double)read_navigation_unsigned(data_jk_bits, T0E_1_bit); t0e_1 = static_cast<double>(read_navigation_unsigned(data_jk_bits, T0E_1_bit));
t0e_1 = t0e_1 * t0e_1_LSB; t0e_1 = t0e_1 * t0e_1_LSB;
DLOG(INFO) << "t0e_1= " << t0e_1; DLOG(INFO) << "t0e_1= " << t0e_1;
M0_1 = (double)read_navigation_signed(data_jk_bits, M0_1_bit); M0_1 = static_cast<double>(read_navigation_signed(data_jk_bits, M0_1_bit));
M0_1 = M0_1 * M0_1_LSB; M0_1 = M0_1 * M0_1_LSB;
DLOG(INFO) << "M0_1= " << M0_1; DLOG(INFO) << "M0_1= " << M0_1;
e_1 = (double)read_navigation_unsigned(data_jk_bits, e_1_bit); e_1 = static_cast<double>(read_navigation_unsigned(data_jk_bits, e_1_bit));
e_1 = e_1 * e_1_LSB; e_1 = e_1 * e_1_LSB;
DLOG(INFO) << "e_1= " << e_1; DLOG(INFO) << "e_1= " << e_1;
A_1 = (double)read_navigation_unsigned(data_jk_bits, A_1_bit); A_1 = static_cast<double>(read_navigation_unsigned(data_jk_bits, A_1_bit));
A_1 = A_1 * A_1_LSB_gal; A_1 = A_1 * A_1_LSB_gal;
DLOG(INFO) << "A_1= " << A_1; DLOG(INFO) << "A_1= " << A_1;
flag_ephemeris_1 = true; flag_ephemeris_1 = true;
@@ -702,18 +702,18 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 2: /*Word type 2: Ephemeris (2/4)*/ case 2: /*Word type 2: Ephemeris (2/4)*/
IOD_nav_2 = (int)read_navigation_unsigned(data_jk_bits, IOD_nav_2_bit); IOD_nav_2 = static_cast<int>(read_navigation_unsigned(data_jk_bits, IOD_nav_2_bit));
DLOG(INFO) << "IOD_nav_2= " << IOD_nav_2; DLOG(INFO) << "IOD_nav_2= " << IOD_nav_2;
OMEGA_0_2 = (double)read_navigation_signed(data_jk_bits, OMEGA_0_2_bit); OMEGA_0_2 = static_cast<double>(read_navigation_signed(data_jk_bits, OMEGA_0_2_bit));
OMEGA_0_2 = OMEGA_0_2 * OMEGA_0_2_LSB; OMEGA_0_2 = OMEGA_0_2 * OMEGA_0_2_LSB;
DLOG(INFO) << "OMEGA_0_2= " << OMEGA_0_2 ; DLOG(INFO) << "OMEGA_0_2= " << OMEGA_0_2 ;
i_0_2 = (double)read_navigation_signed(data_jk_bits, i_0_2_bit); i_0_2 = static_cast<double>(read_navigation_signed(data_jk_bits, i_0_2_bit));
i_0_2 = i_0_2 * i_0_2_LSB; i_0_2 = i_0_2 * i_0_2_LSB;
DLOG(INFO) << "i_0_2= " << i_0_2 ; DLOG(INFO) << "i_0_2= " << i_0_2 ;
omega_2 = (double)read_navigation_signed(data_jk_bits, omega_2_bit); omega_2 = static_cast<double>(read_navigation_signed(data_jk_bits, omega_2_bit));
omega_2 = omega_2 * omega_2_LSB; omega_2 = omega_2 * omega_2_LSB;
DLOG(INFO) << "omega_2= " << omega_2; DLOG(INFO) << "omega_2= " << omega_2;
iDot_2 = (double)read_navigation_signed(data_jk_bits, iDot_2_bit); iDot_2 = static_cast<double>(read_navigation_signed(data_jk_bits, iDot_2_bit));
iDot_2 = iDot_2 * iDot_2_LSB; iDot_2 = iDot_2 * iDot_2_LSB;
DLOG(INFO) << "iDot_2= " << iDot_2; DLOG(INFO) << "iDot_2= " << iDot_2;
flag_ephemeris_2 = true; flag_ephemeris_2 = true;
@@ -721,57 +721,57 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 3: /*Word type 3: Ephemeris (3/4) and SISA*/ case 3: /*Word type 3: Ephemeris (3/4) and SISA*/
IOD_nav_3 = (int)read_navigation_unsigned(data_jk_bits, IOD_nav_3_bit); IOD_nav_3 = static_cast<int>(read_navigation_unsigned(data_jk_bits, IOD_nav_3_bit));
DLOG(INFO) << "IOD_nav_3= " << IOD_nav_3 ; DLOG(INFO) << "IOD_nav_3= " << IOD_nav_3 ;
OMEGA_dot_3 = (double)read_navigation_signed(data_jk_bits, OMEGA_dot_3_bit); OMEGA_dot_3 = static_cast<double>(read_navigation_signed(data_jk_bits, OMEGA_dot_3_bit));
OMEGA_dot_3 = OMEGA_dot_3 * OMEGA_dot_3_LSB; OMEGA_dot_3 = OMEGA_dot_3 * OMEGA_dot_3_LSB;
DLOG(INFO) <<"OMEGA_dot_3= " << OMEGA_dot_3 ; DLOG(INFO) <<"OMEGA_dot_3= " << OMEGA_dot_3 ;
delta_n_3 = (double)read_navigation_signed(data_jk_bits, delta_n_3_bit); delta_n_3 = static_cast<double>(read_navigation_signed(data_jk_bits, delta_n_3_bit));
delta_n_3 = delta_n_3 * delta_n_3_LSB; delta_n_3 = delta_n_3 * delta_n_3_LSB;
DLOG(INFO) << "delta_n_3= " << delta_n_3 ; DLOG(INFO) << "delta_n_3= " << delta_n_3 ;
C_uc_3 = (double)read_navigation_signed(data_jk_bits, C_uc_3_bit); C_uc_3 = static_cast<double>(read_navigation_signed(data_jk_bits, C_uc_3_bit));
C_uc_3 = C_uc_3 * C_uc_3_LSB; C_uc_3 = C_uc_3 * C_uc_3_LSB;
DLOG(INFO) << "C_uc_3= " << C_uc_3; DLOG(INFO) << "C_uc_3= " << C_uc_3;
C_us_3 = (double)read_navigation_signed(data_jk_bits, C_us_3_bit); C_us_3 = static_cast<double>(read_navigation_signed(data_jk_bits, C_us_3_bit));
C_us_3 = C_us_3 * C_us_3_LSB; C_us_3 = C_us_3 * C_us_3_LSB;
DLOG(INFO) << "C_us_3= " << C_us_3; DLOG(INFO) << "C_us_3= " << C_us_3;
C_rc_3 = (double)read_navigation_signed(data_jk_bits, C_rc_3_bit); C_rc_3 = static_cast<double>(read_navigation_signed(data_jk_bits, C_rc_3_bit));
C_rc_3 = C_rc_3 * C_rc_3_LSB; C_rc_3 = C_rc_3 * C_rc_3_LSB;
DLOG(INFO) << "C_rc_3= " << C_rc_3; DLOG(INFO) << "C_rc_3= " << C_rc_3;
C_rs_3 = (double)read_navigation_signed(data_jk_bits, C_rs_3_bit); C_rs_3 = static_cast<double>(read_navigation_signed(data_jk_bits, C_rs_3_bit));
C_rs_3 = C_rs_3 * C_rs_3_LSB; C_rs_3 = C_rs_3 * C_rs_3_LSB;
DLOG(INFO) << "C_rs_3= " << C_rs_3; DLOG(INFO) << "C_rs_3= " << C_rs_3;
SISA_3 = (double)read_navigation_unsigned(data_jk_bits, SISA_3_bit); SISA_3 = static_cast<double>(read_navigation_unsigned(data_jk_bits, SISA_3_bit));
DLOG(INFO) << "SISA_3= " << SISA_3; DLOG(INFO) << "SISA_3= " << SISA_3;
flag_ephemeris_3 = true; flag_ephemeris_3 = true;
DLOG(INFO) << "flag_tow_set" << flag_TOW_set; DLOG(INFO) << "flag_tow_set" << flag_TOW_set;
break; break;
case 4: /* Word type 4: Ephemeris (4/4) and Clock correction parameters*/ case 4: /* Word type 4: Ephemeris (4/4) and Clock correction parameters*/
IOD_nav_4 = (int)read_navigation_unsigned(data_jk_bits, IOD_nav_4_bit); IOD_nav_4 = static_cast<int>(read_navigation_unsigned(data_jk_bits, IOD_nav_4_bit));
DLOG(INFO) << "IOD_nav_4= " << IOD_nav_4 ; DLOG(INFO) << "IOD_nav_4= " << IOD_nav_4 ;
SV_ID_PRN_4 = (int)read_navigation_unsigned(data_jk_bits, SV_ID_PRN_4_bit); SV_ID_PRN_4 = static_cast<int>(read_navigation_unsigned(data_jk_bits, SV_ID_PRN_4_bit));
DLOG(INFO) << "SV_ID_PRN_4= " << SV_ID_PRN_4 ; DLOG(INFO) << "SV_ID_PRN_4= " << SV_ID_PRN_4 ;
C_ic_4 = (double)read_navigation_signed(data_jk_bits, C_ic_4_bit); C_ic_4 = static_cast<double>(read_navigation_signed(data_jk_bits, C_ic_4_bit));
C_ic_4 = C_ic_4 * C_ic_4_LSB; C_ic_4 = C_ic_4 * C_ic_4_LSB;
DLOG(INFO) << "C_ic_4= " << C_ic_4; DLOG(INFO) << "C_ic_4= " << C_ic_4;
C_is_4 = (double)read_navigation_signed(data_jk_bits, C_is_4_bit); C_is_4 = static_cast<double>(read_navigation_signed(data_jk_bits, C_is_4_bit));
C_is_4 = C_is_4 * C_is_4_LSB; C_is_4 = C_is_4 * C_is_4_LSB;
DLOG(INFO) << "C_is_4= " << C_is_4; DLOG(INFO) << "C_is_4= " << C_is_4;
/*Clock correction parameters*/ /*Clock correction parameters*/
t0c_4 = (double)read_navigation_unsigned(data_jk_bits, t0c_4_bit); t0c_4 = static_cast<double>(read_navigation_unsigned(data_jk_bits, t0c_4_bit));
t0c_4 = t0c_4 * t0c_4_LSB; t0c_4 = t0c_4 * t0c_4_LSB;
DLOG(INFO) << "t0c_4= " << t0c_4; DLOG(INFO) << "t0c_4= " << t0c_4;
af0_4 = (double)read_navigation_signed(data_jk_bits, af0_4_bit); af0_4 = static_cast<double>(read_navigation_signed(data_jk_bits, af0_4_bit));
af0_4 = af0_4 * af0_4_LSB; af0_4 = af0_4 * af0_4_LSB;
DLOG(INFO) << "af0_4 = " << af0_4; DLOG(INFO) << "af0_4 = " << af0_4;
af1_4 = (double)read_navigation_signed(data_jk_bits, af1_4_bit); af1_4 = static_cast<double>(read_navigation_signed(data_jk_bits, af1_4_bit));
af1_4 = af1_4 * af1_4_LSB; af1_4 = af1_4 * af1_4_LSB;
DLOG(INFO) << "af1_4 = " << af1_4; DLOG(INFO) << "af1_4 = " << af1_4;
af2_4 = (double)read_navigation_signed(data_jk_bits, af2_4_bit); af2_4 = static_cast<double>(read_navigation_signed(data_jk_bits, af2_4_bit));
af2_4 = af2_4 * af2_4_LSB; af2_4 = af2_4 * af2_4_LSB;
DLOG(INFO) << "af2_4 = " << af2_4; DLOG(INFO) << "af2_4 = " << af2_4;
spare_4 = (double)read_navigation_unsigned(data_jk_bits, spare_4_bit); spare_4 = static_cast<double>(read_navigation_unsigned(data_jk_bits, spare_4_bit));
DLOG(INFO) << "spare_4 = " << spare_4; DLOG(INFO) << "spare_4 = " << spare_4;
flag_ephemeris_4 = true; flag_ephemeris_4 = true;
DLOG(INFO) << "flag_tow_set" << flag_TOW_set; DLOG(INFO) << "flag_tow_set" << flag_TOW_set;
@@ -780,47 +780,47 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
case 5: /*Word type 5: Ionospheric correction, BGD, signal health and data validity status and GST*/ case 5: /*Word type 5: Ionospheric correction, BGD, signal health and data validity status and GST*/
/*Ionospheric correction*/ /*Ionospheric correction*/
/*Az*/ /*Az*/
ai0_5 = (double)read_navigation_unsigned(data_jk_bits, ai0_5_bit); ai0_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, ai0_5_bit));
ai0_5 = ai0_5 * ai0_5_LSB; ai0_5 = ai0_5 * ai0_5_LSB;
DLOG(INFO) << "ai0_5= " << ai0_5; DLOG(INFO) << "ai0_5= " << ai0_5;
ai1_5 = (double)read_navigation_signed(data_jk_bits, ai1_5_bit); ai1_5 = static_cast<double>(read_navigation_signed(data_jk_bits, ai1_5_bit));
ai1_5 = ai1_5 * ai1_5_LSB; ai1_5 = ai1_5 * ai1_5_LSB;
DLOG(INFO) << "ai1_5= " << ai1_5; DLOG(INFO) << "ai1_5= " << ai1_5;
ai2_5 = (double)read_navigation_signed(data_jk_bits, ai2_5_bit); ai2_5 = static_cast<double>(read_navigation_signed(data_jk_bits, ai2_5_bit));
ai2_5 = ai2_5 * ai2_5_LSB; ai2_5 = ai2_5 * ai2_5_LSB;
DLOG(INFO) << "ai2_5= " << ai2_5; DLOG(INFO) << "ai2_5= " << ai2_5;
/*Ionospheric disturbance flag*/ /*Ionospheric disturbance flag*/
Region1_flag_5 = (bool)read_navigation_bool(data_jk_bits, Region1_5_bit); Region1_flag_5 = static_cast<bool>(read_navigation_bool(data_jk_bits, Region1_5_bit));
DLOG(INFO) << "Region1_flag_5= " << Region1_flag_5; DLOG(INFO) << "Region1_flag_5= " << Region1_flag_5;
Region2_flag_5 = (bool)read_navigation_bool(data_jk_bits, Region2_5_bit); Region2_flag_5 = static_cast<bool>(read_navigation_bool(data_jk_bits, Region2_5_bit));
DLOG(INFO) << "Region2_flag_5= " << Region2_flag_5; DLOG(INFO) << "Region2_flag_5= " << Region2_flag_5;
Region3_flag_5 = (bool)read_navigation_bool(data_jk_bits, Region3_5_bit); Region3_flag_5 = static_cast<bool>(read_navigation_bool(data_jk_bits, Region3_5_bit));
DLOG(INFO) << "Region3_flag_5= " << Region3_flag_5; DLOG(INFO) << "Region3_flag_5= " << Region3_flag_5;
Region4_flag_5 = (bool)read_navigation_bool(data_jk_bits, Region4_5_bit); Region4_flag_5 = static_cast<bool>(read_navigation_bool(data_jk_bits, Region4_5_bit));
DLOG(INFO) << "Region4_flag_5= " << Region4_flag_5; DLOG(INFO) << "Region4_flag_5= " << Region4_flag_5;
Region5_flag_5 = (bool)read_navigation_bool(data_jk_bits, Region5_5_bit); Region5_flag_5 = static_cast<bool>(read_navigation_bool(data_jk_bits, Region5_5_bit));
DLOG(INFO) << "Region5_flag_5= " << Region5_flag_5; DLOG(INFO) << "Region5_flag_5= " << Region5_flag_5;
BGD_E1E5a_5 = (double)read_navigation_signed(data_jk_bits, BGD_E1E5a_5_bit); BGD_E1E5a_5 = static_cast<double>(read_navigation_signed(data_jk_bits, BGD_E1E5a_5_bit));
BGD_E1E5a_5 = BGD_E1E5a_5 * BGD_E1E5a_5_LSB; BGD_E1E5a_5 = BGD_E1E5a_5 * BGD_E1E5a_5_LSB;
DLOG(INFO) << "BGD_E1E5a_5= " << BGD_E1E5a_5; DLOG(INFO) << "BGD_E1E5a_5= " << BGD_E1E5a_5;
BGD_E1E5b_5 = (double)read_navigation_signed(data_jk_bits, BGD_E1E5b_5_bit); BGD_E1E5b_5 = static_cast<double>(read_navigation_signed(data_jk_bits, BGD_E1E5b_5_bit));
BGD_E1E5b_5 = BGD_E1E5b_5 * BGD_E1E5b_5_LSB; BGD_E1E5b_5 = BGD_E1E5b_5 * BGD_E1E5b_5_LSB;
DLOG(INFO) << "BGD_E1E5b_5= " << BGD_E1E5b_5; DLOG(INFO) << "BGD_E1E5b_5= " << BGD_E1E5b_5;
E5b_HS_5 = (double)read_navigation_unsigned(data_jk_bits, E5b_HS_5_bit); E5b_HS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_HS_5_bit));
DLOG(INFO) << "E5b_HS_5= " << E5b_HS_5; DLOG(INFO) << "E5b_HS_5= " << E5b_HS_5;
E1B_HS_5 = (double)read_navigation_unsigned(data_jk_bits, E1B_HS_5_bit); E1B_HS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_5_bit));
DLOG(INFO) << "E1B_HS_5= " << E1B_HS_5; DLOG(INFO) << "E1B_HS_5= " << E1B_HS_5;
E5b_DVS_5 = (double)read_navigation_unsigned(data_jk_bits, E5b_DVS_5_bit); E5b_DVS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_DVS_5_bit));
DLOG(INFO) << "E5b_DVS_5= " << E5b_DVS_5; DLOG(INFO) << "E5b_DVS_5= " << E5b_DVS_5;
E1B_DVS_5 = (double)read_navigation_unsigned(data_jk_bits, E1B_DVS_5_bit); E1B_DVS_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_DVS_5_bit));
DLOG(INFO) << "E1B_DVS_5= " << E1B_DVS_5; DLOG(INFO) << "E1B_DVS_5= " << E1B_DVS_5;
/*GST*/ /*GST*/
WN_5 = (double)read_navigation_unsigned(data_jk_bits, WN_5_bit); WN_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_5_bit));
DLOG(INFO) << "WN_5= " << WN_5; DLOG(INFO) << "WN_5= " << WN_5;
TOW_5 = (double)read_navigation_unsigned(data_jk_bits, TOW_5_bit); TOW_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, TOW_5_bit));
DLOG(INFO) << "TOW_5= " << TOW_5; DLOG(INFO) << "TOW_5= " << TOW_5;
flag_TOW_5 = true; //set to false externally flag_TOW_5 = true; //set to false externally
spare_5 = (double)read_navigation_unsigned(data_jk_bits, spare_5_bit); spare_5 = static_cast<double>(read_navigation_unsigned(data_jk_bits, spare_5_bit));
DLOG(INFO) << "spare_5= " << spare_5; DLOG(INFO) << "spare_5= " << spare_5;
flag_iono_and_GST = true; //set to false externally flag_iono_and_GST = true; //set to false externally
flag_TOW_set = true; //set to false externally flag_TOW_set = true; //set to false externally
@@ -828,26 +828,26 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 6: /*Word type 6: GST-UTC conversion parameters*/ case 6: /*Word type 6: GST-UTC conversion parameters*/
A0_6 = (double)read_navigation_signed(data_jk_bits, A0_6_bit); A0_6 = static_cast<double>(read_navigation_signed(data_jk_bits, A0_6_bit));
A0_6 = A0_6 * A0_6_LSB; A0_6 = A0_6 * A0_6_LSB;
DLOG(INFO) << "A0_6= " << A0_6; DLOG(INFO) << "A0_6= " << A0_6;
A1_6 = (double)read_navigation_signed(data_jk_bits, A1_6_bit); A1_6 = static_cast<double>(read_navigation_signed(data_jk_bits, A1_6_bit));
A1_6 = A1_6 * A1_6_LSB; A1_6 = A1_6 * A1_6_LSB;
DLOG(INFO) << "A1_6= " << A1_6; DLOG(INFO) << "A1_6= " << A1_6;
Delta_tLS_6 = (double)read_navigation_signed(data_jk_bits, Delta_tLS_6_bit); Delta_tLS_6 = static_cast<double>(read_navigation_signed(data_jk_bits, Delta_tLS_6_bit));
DLOG(INFO) << "Delta_tLS_6= " << Delta_tLS_6; DLOG(INFO) << "Delta_tLS_6= " << Delta_tLS_6;
t0t_6 = (double)read_navigation_unsigned(data_jk_bits, t0t_6_bit); t0t_6 = static_cast<double>(read_navigation_unsigned(data_jk_bits, t0t_6_bit));
t0t_6 = t0t_6 * t0t_6_LSB; t0t_6 = t0t_6 * t0t_6_LSB;
DLOG(INFO) << "t0t_6= " << t0t_6; DLOG(INFO) << "t0t_6= " << t0t_6;
WNot_6 = (double)read_navigation_unsigned(data_jk_bits, WNot_6_bit); WNot_6 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WNot_6_bit));
DLOG(INFO) << "WNot_6= " << WNot_6; DLOG(INFO) << "WNot_6= " << WNot_6;
WN_LSF_6 = (double)read_navigation_unsigned(data_jk_bits, WN_LSF_6_bit); WN_LSF_6 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_LSF_6_bit));
DLOG(INFO) << "WN_LSF_6= " << WN_LSF_6; DLOG(INFO) << "WN_LSF_6= " << WN_LSF_6;
DN_6 = (double)read_navigation_unsigned(data_jk_bits, DN_6_bit); DN_6 = static_cast<double>(read_navigation_unsigned(data_jk_bits, DN_6_bit));
DLOG(INFO) << "DN_6= " << DN_6; DLOG(INFO) << "DN_6= " << DN_6;
Delta_tLSF_6 = (double)read_navigation_signed(data_jk_bits, Delta_tLSF_6_bit); Delta_tLSF_6 = static_cast<double>(read_navigation_signed(data_jk_bits, Delta_tLSF_6_bit));
DLOG(INFO) << "Delta_tLSF_6= " << Delta_tLSF_6; DLOG(INFO) << "Delta_tLSF_6= " << Delta_tLSF_6;
TOW_6 = (double)read_navigation_unsigned(data_jk_bits, TOW_6_bit); TOW_6 = static_cast<double>(read_navigation_unsigned(data_jk_bits, TOW_6_bit));
DLOG(INFO) << "TOW_6= " << TOW_6; DLOG(INFO) << "TOW_6= " << TOW_6;
flag_TOW_6 = true; //set to false externally flag_TOW_6 = true; //set to false externally
flag_utc_model = true; //set to false externally flag_utc_model = true; //set to false externally
@@ -856,34 +856,34 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 7: /*Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number*/ case 7: /*Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number*/
IOD_a_7 = (double)read_navigation_unsigned(data_jk_bits, IOD_a_7_bit); IOD_a_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, IOD_a_7_bit));
DLOG(INFO) << "IOD_a_7= " << IOD_a_7; DLOG(INFO) << "IOD_a_7= " << IOD_a_7;
WN_a_7 = (double)read_navigation_unsigned(data_jk_bits, WN_a_7_bit); WN_a_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_a_7_bit));
DLOG(INFO) << "WN_a_7= " << WN_a_7; DLOG(INFO) << "WN_a_7= " << WN_a_7;
t0a_7 = (double)read_navigation_unsigned(data_jk_bits, t0a_7_bit); t0a_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, t0a_7_bit));
t0a_7 = t0a_7 * t0a_7_LSB; t0a_7 = t0a_7 * t0a_7_LSB;
DLOG(INFO) << "t0a_7= " << t0a_7; DLOG(INFO) << "t0a_7= " << t0a_7;
SVID1_7 = (double)read_navigation_unsigned(data_jk_bits, SVID1_7_bit); SVID1_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, SVID1_7_bit));
DLOG(INFO) << "SVID1_7= " << SVID1_7; DLOG(INFO) << "SVID1_7= " << SVID1_7;
DELTA_A_7 = (double)read_navigation_signed(data_jk_bits, DELTA_A_7_bit); DELTA_A_7 = static_cast<double>(read_navigation_signed(data_jk_bits, DELTA_A_7_bit));
DELTA_A_7 = DELTA_A_7 * DELTA_A_7_LSB; DELTA_A_7 = DELTA_A_7 * DELTA_A_7_LSB;
DLOG(INFO) << "DELTA_A_7= " << DELTA_A_7; DLOG(INFO) << "DELTA_A_7= " << DELTA_A_7;
e_7 = (double)read_navigation_unsigned(data_jk_bits, e_7_bit); e_7 = static_cast<double>(read_navigation_unsigned(data_jk_bits, e_7_bit));
e_7 = e_7 * e_7_LSB; e_7 = e_7 * e_7_LSB;
DLOG(INFO) << "e_7= " << e_7; DLOG(INFO) << "e_7= " << e_7;
omega_7 = (double)read_navigation_signed(data_jk_bits, omega_7_bit); omega_7 = static_cast<double>(read_navigation_signed(data_jk_bits, omega_7_bit));
omega_7 = omega_7 * omega_7_LSB; omega_7 = omega_7 * omega_7_LSB;
DLOG(INFO) << "omega_7= " << omega_7; DLOG(INFO) << "omega_7= " << omega_7;
delta_i_7 = (double)read_navigation_signed(data_jk_bits, delta_i_7_bit); delta_i_7 = static_cast<double>(read_navigation_signed(data_jk_bits, delta_i_7_bit));
delta_i_7 = delta_i_7 * delta_i_7_LSB; delta_i_7 = delta_i_7 * delta_i_7_LSB;
DLOG(INFO) << "delta_i_7= " << delta_i_7; DLOG(INFO) << "delta_i_7= " << delta_i_7;
Omega0_7 = (double)read_navigation_signed(data_jk_bits, Omega0_7_bit); Omega0_7 = static_cast<double>(read_navigation_signed(data_jk_bits, Omega0_7_bit));
Omega0_7 = Omega0_7 * Omega0_7_LSB; Omega0_7 = Omega0_7 * Omega0_7_LSB;
DLOG(INFO) << "Omega0_7= " << Omega0_7; DLOG(INFO) << "Omega0_7= " << Omega0_7;
Omega_dot_7 = (double)read_navigation_signed(data_jk_bits, Omega_dot_7_bit); Omega_dot_7 = static_cast<double>(read_navigation_signed(data_jk_bits, Omega_dot_7_bit));
Omega_dot_7 = Omega_dot_7 * Omega_dot_7_LSB; Omega_dot_7 = Omega_dot_7 * Omega_dot_7_LSB;
DLOG(INFO) << "Omega_dot_7= " << Omega_dot_7; DLOG(INFO) << "Omega_dot_7= " << Omega_dot_7;
M0_7 = (double)read_navigation_signed(data_jk_bits, M0_7_bit); M0_7 = static_cast<double>(read_navigation_signed(data_jk_bits, M0_7_bit));
M0_7 = M0_7 * M0_7_LSB; M0_7 = M0_7 * M0_7_LSB;
DLOG(INFO) << "M0_7= " << M0_7; DLOG(INFO) << "M0_7= " << M0_7;
flag_almanac_1 = true; flag_almanac_1 = true;
@@ -891,36 +891,36 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 8: /*Word type 8: Almanac for SVID1 (2/2) and SVID2 (1/2)*/ case 8: /*Word type 8: Almanac for SVID1 (2/2) and SVID2 (1/2)*/
IOD_a_8 = (double)read_navigation_signed(data_jk_bits, IOD_a_8_bit); IOD_a_8 = static_cast<double>(read_navigation_signed(data_jk_bits, IOD_a_8_bit));
DLOG(INFO) << "IOD_a_8= " << IOD_a_8; DLOG(INFO) << "IOD_a_8= " << IOD_a_8;
af0_8 = (double)read_navigation_signed(data_jk_bits, af0_8_bit); af0_8 = static_cast<double>(read_navigation_signed(data_jk_bits, af0_8_bit));
af0_8 = af0_8 * af0_8_LSB; af0_8 = af0_8 * af0_8_LSB;
DLOG(INFO) << "af0_8= " << af0_8; DLOG(INFO) << "af0_8= " << af0_8;
af1_8 = (double)read_navigation_signed(data_jk_bits, af1_8_bit); af1_8 = static_cast<double>(read_navigation_signed(data_jk_bits, af1_8_bit));
af1_8 = af1_8 * af1_8_LSB; af1_8 = af1_8 * af1_8_LSB;
DLOG(INFO) << "af1_8= " << af1_8; DLOG(INFO) << "af1_8= " << af1_8;
E5b_HS_8 = (double)read_navigation_unsigned(data_jk_bits, E5b_HS_8_bit); E5b_HS_8 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_HS_8_bit));
DLOG(INFO) << "E5b_HS_8= " << E5b_HS_8; DLOG(INFO) << "E5b_HS_8= " << E5b_HS_8;
E1B_HS_8 = (double)read_navigation_unsigned(data_jk_bits, E1B_HS_8_bit); E1B_HS_8 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_8_bit));
DLOG(INFO) << "E1B_HS_8= " << E1B_HS_8; DLOG(INFO) << "E1B_HS_8= " << E1B_HS_8;
SVID2_8 = (double)read_navigation_unsigned(data_jk_bits, SVID2_8_bit); SVID2_8 = static_cast<double>(read_navigation_unsigned(data_jk_bits, SVID2_8_bit));
DLOG(INFO) << "SVID2_8= " << SVID2_8; DLOG(INFO) << "SVID2_8= " << SVID2_8;
DELTA_A_8 = (double)read_navigation_signed(data_jk_bits, DELTA_A_8_bit); DELTA_A_8 = static_cast<double>(read_navigation_signed(data_jk_bits, DELTA_A_8_bit));
DELTA_A_8 = DELTA_A_8 * DELTA_A_8_LSB; DELTA_A_8 = DELTA_A_8 * DELTA_A_8_LSB;
DLOG(INFO) << "DELTA_A_8= " << DELTA_A_8; DLOG(INFO) << "DELTA_A_8= " << DELTA_A_8;
e_8 = (double)read_navigation_unsigned(data_jk_bits, e_8_bit); e_8 = static_cast<double>(read_navigation_unsigned(data_jk_bits, e_8_bit));
e_8 = e_8 * e_8_LSB; e_8 = e_8 * e_8_LSB;
DLOG(INFO) << "e_8= " << e_8; DLOG(INFO) << "e_8= " << e_8;
omega_8 = (double)read_navigation_signed(data_jk_bits, omega_8_bit); omega_8 = static_cast<double>(read_navigation_signed(data_jk_bits, omega_8_bit));
omega_8 = omega_8 * omega_8_LSB; omega_8 = omega_8 * omega_8_LSB;
DLOG(INFO) << "omega_8= " << omega_8; DLOG(INFO) << "omega_8= " << omega_8;
delta_i_8 = (double)read_navigation_signed(data_jk_bits, delta_i_8_bit); delta_i_8 = static_cast<double>(read_navigation_signed(data_jk_bits, delta_i_8_bit));
delta_i_8 = delta_i_8 * delta_i_8_LSB; delta_i_8 = delta_i_8 * delta_i_8_LSB;
DLOG(INFO) << "delta_i_8= " << delta_i_8; DLOG(INFO) << "delta_i_8= " << delta_i_8;
Omega0_8 = (double)read_navigation_signed(data_jk_bits, Omega0_8_bit); Omega0_8 = static_cast<double>(read_navigation_signed(data_jk_bits, Omega0_8_bit));
Omega0_8 = Omega0_8 * Omega0_8_LSB; Omega0_8 = Omega0_8 * Omega0_8_LSB;
DLOG(INFO) << "Omega0_8= " << Omega0_8; DLOG(INFO) << "Omega0_8= " << Omega0_8;
Omega_dot_8 = (double)read_navigation_signed(data_jk_bits, Omega_dot_8_bit); Omega_dot_8 = static_cast<double>(read_navigation_signed(data_jk_bits, Omega_dot_8_bit));
Omega_dot_8 = Omega_dot_8 * Omega_dot_8_LSB; Omega_dot_8 = Omega_dot_8 * Omega_dot_8_LSB;
DLOG(INFO) << "Omega_dot_8= " << Omega_dot_8; DLOG(INFO) << "Omega_dot_8= " << Omega_dot_8;
flag_almanac_2 = true; flag_almanac_2 = true;
@@ -928,38 +928,38 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 9: /*Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)*/ case 9: /*Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)*/
IOD_a_9 = (double)read_navigation_unsigned(data_jk_bits, IOD_a_9_bit); IOD_a_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, IOD_a_9_bit));
DLOG(INFO) << "IOD_a_9= " << IOD_a_9; DLOG(INFO) << "IOD_a_9= " << IOD_a_9;
WN_a_9 = (double)read_navigation_unsigned(data_jk_bits, WN_a_9_bit); WN_a_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_a_9_bit));
DLOG(INFO) << "WN_a_9= " << WN_a_9; DLOG(INFO) << "WN_a_9= " << WN_a_9;
t0a_9 = (double)read_navigation_unsigned(data_jk_bits, t0a_9_bit); t0a_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, t0a_9_bit));
t0a_9 = t0a_9 * t0a_9_LSB; t0a_9 = t0a_9 * t0a_9_LSB;
DLOG(INFO) << "t0a_9= " << t0a_9; DLOG(INFO) << "t0a_9= " << t0a_9;
M0_9 = (double)read_navigation_signed(data_jk_bits, M0_9_bit); M0_9 = static_cast<double>(read_navigation_signed(data_jk_bits, M0_9_bit));
M0_9 = M0_9 * M0_9_LSB; M0_9 = M0_9 * M0_9_LSB;
DLOG(INFO) << "M0_9= " << M0_9; DLOG(INFO) << "M0_9= " << M0_9;
af0_9 = (double)read_navigation_signed(data_jk_bits, af0_9_bit); af0_9 = static_cast<double>(read_navigation_signed(data_jk_bits, af0_9_bit));
af0_9 = af0_9 * af0_9_LSB; af0_9 = af0_9 * af0_9_LSB;
DLOG(INFO) << "af0_9= " << af0_9; DLOG(INFO) << "af0_9= " << af0_9;
af1_9 = (double)read_navigation_signed(data_jk_bits, af1_9_bit); af1_9 = static_cast<double>(read_navigation_signed(data_jk_bits, af1_9_bit));
af1_9 = af1_9 * af1_9_LSB; af1_9 = af1_9 * af1_9_LSB;
DLOG(INFO) << "af1_9= " << af1_9; DLOG(INFO) << "af1_9= " << af1_9;
E1B_HS_9 = (double)read_navigation_unsigned(data_jk_bits, E1B_HS_9_bit); E1B_HS_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_9_bit));
DLOG(INFO) << "E1B_HS_9= " << E1B_HS_9; DLOG(INFO) << "E1B_HS_9= " << E1B_HS_9;
E1B_HS_9 = (double)read_navigation_unsigned(data_jk_bits, E1B_HS_9_bit); E1B_HS_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_9_bit));
DLOG(INFO) << "E1B_HS_9= " << E1B_HS_9; DLOG(INFO) << "E1B_HS_9= " << E1B_HS_9;
SVID3_9 = (double)read_navigation_unsigned(data_jk_bits,SVID3_9_bit); SVID3_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits,SVID3_9_bit));
DLOG(INFO) << "SVID3_9= " << SVID3_9; DLOG(INFO) << "SVID3_9= " << SVID3_9;
DELTA_A_9 = (double)read_navigation_signed(data_jk_bits, DELTA_A_9_bit); DELTA_A_9 = static_cast<double>(read_navigation_signed(data_jk_bits, DELTA_A_9_bit));
DELTA_A_9 = DELTA_A_9 * DELTA_A_9_LSB; DELTA_A_9 = DELTA_A_9 * DELTA_A_9_LSB;
DLOG(INFO) << "DELTA_A_9= " << DELTA_A_9; DLOG(INFO) << "DELTA_A_9= " << DELTA_A_9;
e_9 = (double)read_navigation_unsigned(data_jk_bits, e_9_bit); e_9 = static_cast<double>(read_navigation_unsigned(data_jk_bits, e_9_bit));
e_9 = e_9 * e_9_LSB; e_9 = e_9 * e_9_LSB;
DLOG(INFO) << "e_9= " << e_9; DLOG(INFO) << "e_9= " << e_9;
omega_9 = (double)read_navigation_signed(data_jk_bits, omega_9_bit); omega_9 = static_cast<double>(read_navigation_signed(data_jk_bits, omega_9_bit));
omega_9 = omega_9 * omega_9_LSB; omega_9 = omega_9 * omega_9_LSB;
DLOG(INFO) << "omega_9= " << omega_9; DLOG(INFO) << "omega_9= " << omega_9;
delta_i_9 = (double)read_navigation_signed(data_jk_bits, delta_i_9_bit); delta_i_9 = static_cast<double>(read_navigation_signed(data_jk_bits, delta_i_9_bit));
delta_i_9 = delta_i_9 * delta_i_9_LSB; delta_i_9 = delta_i_9 * delta_i_9_LSB;
DLOG(INFO) << "delta_i_9= " << delta_i_9; DLOG(INFO) << "delta_i_9= " << delta_i_9;
flag_almanac_3 = true; flag_almanac_3 = true;
@@ -967,40 +967,40 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 10: /*Word type 10: Almanac for SVID3 (2/2) and GST-GPS conversion parameters*/ case 10: /*Word type 10: Almanac for SVID3 (2/2) and GST-GPS conversion parameters*/
IOD_a_10 = (double)read_navigation_unsigned(data_jk_bits, IOD_a_10_bit); IOD_a_10 = static_cast<double>(read_navigation_unsigned(data_jk_bits, IOD_a_10_bit));
DLOG(INFO) << "IOD_a_10= " << IOD_a_10; DLOG(INFO) << "IOD_a_10= " << IOD_a_10;
Omega0_10 = (double)read_navigation_signed(data_jk_bits, Omega0_10_bit); Omega0_10 = static_cast<double>(read_navigation_signed(data_jk_bits, Omega0_10_bit));
Omega0_10 = Omega0_10 * Omega0_10_LSB; Omega0_10 = Omega0_10 * Omega0_10_LSB;
DLOG(INFO) << "Omega0_10= " << Omega0_10; DLOG(INFO) << "Omega0_10= " << Omega0_10;
Omega_dot_10 = (double)read_navigation_signed(data_jk_bits, Omega_dot_10_bit); Omega_dot_10 = static_cast<double>(read_navigation_signed(data_jk_bits, Omega_dot_10_bit));
Omega_dot_10 = Omega_dot_10 * Omega_dot_10_LSB; Omega_dot_10 = Omega_dot_10 * Omega_dot_10_LSB;
DLOG(INFO) << "Omega_dot_10= " << Omega_dot_10 ; DLOG(INFO) << "Omega_dot_10= " << Omega_dot_10 ;
M0_10 = (double)read_navigation_signed(data_jk_bits, M0_10_bit); M0_10 = static_cast<double>(read_navigation_signed(data_jk_bits, M0_10_bit));
M0_10 = M0_10 * M0_10_LSB; M0_10 = M0_10 * M0_10_LSB;
DLOG(INFO) << "M0_10= " << M0_10; DLOG(INFO) << "M0_10= " << M0_10;
af0_10 = (double)read_navigation_signed(data_jk_bits, af0_10_bit); af0_10 = static_cast<double>(read_navigation_signed(data_jk_bits, af0_10_bit));
af0_10 = af0_10 * af0_10_LSB; af0_10 = af0_10 * af0_10_LSB;
DLOG(INFO) << "af0_10= " << af0_10; DLOG(INFO) << "af0_10= " << af0_10;
af1_10 = (double)read_navigation_signed(data_jk_bits, af1_10_bit); af1_10 = static_cast<double>(read_navigation_signed(data_jk_bits, af1_10_bit));
af1_10 = af1_10 * af1_10_LSB; af1_10 = af1_10 * af1_10_LSB;
DLOG(INFO) << "af1_10= " << af1_10; DLOG(INFO) << "af1_10= " << af1_10;
E5b_HS_10 = (double)read_navigation_unsigned(data_jk_bits, E5b_HS_10_bit); E5b_HS_10 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E5b_HS_10_bit));
DLOG(INFO) << "E5b_HS_10= " << E5b_HS_10; DLOG(INFO) << "E5b_HS_10= " << E5b_HS_10;
E1B_HS_10 = (double)read_navigation_unsigned(data_jk_bits, E1B_HS_10_bit); E1B_HS_10 = static_cast<double>(read_navigation_unsigned(data_jk_bits, E1B_HS_10_bit));
DLOG(INFO) << "E1B_HS_10= " << E1B_HS_10; DLOG(INFO) << "E1B_HS_10= " << E1B_HS_10;
A_0G_10 = (double)read_navigation_signed(data_jk_bits, A_0G_10_bit); A_0G_10 = static_cast<double>(read_navigation_signed(data_jk_bits, A_0G_10_bit));
A_0G_10 = A_0G_10 * A_0G_10_LSB; A_0G_10 = A_0G_10 * A_0G_10_LSB;
flag_GGTO_1=true; flag_GGTO_1=true;
DLOG(INFO) << "A_0G_10= " << A_0G_10; DLOG(INFO) << "A_0G_10= " << A_0G_10;
A_1G_10 = (double)read_navigation_signed(data_jk_bits, A_1G_10_bit); A_1G_10 = static_cast<double>(read_navigation_signed(data_jk_bits, A_1G_10_bit));
A_1G_10 = A_1G_10 * A_1G_10_LSB; A_1G_10 = A_1G_10 * A_1G_10_LSB;
flag_GGTO_2=true; flag_GGTO_2=true;
DLOG(INFO) << "A_1G_10= " << A_1G_10; DLOG(INFO) << "A_1G_10= " << A_1G_10;
t_0G_10 = (double)read_navigation_unsigned(data_jk_bits, t_0G_10_bit); t_0G_10 = static_cast<double>(read_navigation_unsigned(data_jk_bits, t_0G_10_bit));
t_0G_10 = t_0G_10 * t_0G_10_LSB; t_0G_10 = t_0G_10 * t_0G_10_LSB;
flag_GGTO_3=true; flag_GGTO_3=true;
DLOG(INFO) << "t_0G_10= " << t_0G_10; DLOG(INFO) << "t_0G_10= " << t_0G_10;
WN_0G_10 = (double)read_navigation_unsigned(data_jk_bits, WN_0G_10_bit); WN_0G_10 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_0G_10_bit));
flag_GGTO_4=true; flag_GGTO_4=true;
DLOG(INFO) << "WN_0G_10= " << WN_0G_10; DLOG(INFO) << "WN_0G_10= " << WN_0G_10;
flag_almanac_4 = true; flag_almanac_4 = true;
@@ -1008,11 +1008,11 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
break; break;
case 0: /*Word type 0: I/NAV Spare Word*/ case 0: /*Word type 0: I/NAV Spare Word*/
Time_0 = (double)read_navigation_unsigned(data_jk_bits, Time_0_bit); Time_0 = static_cast<double>(read_navigation_unsigned(data_jk_bits, Time_0_bit));
DLOG(INFO) << "Time_0= " << Time_0; DLOG(INFO) << "Time_0= " << Time_0;
WN_0 = (double)read_navigation_unsigned(data_jk_bits, WN_0_bit); WN_0 = static_cast<double>(read_navigation_unsigned(data_jk_bits, WN_0_bit));
DLOG(INFO) << "WN_0= " << WN_0; DLOG(INFO) << "WN_0= " << WN_0;
TOW_0 = (double)read_navigation_unsigned(data_jk_bits, TOW_0_bit); TOW_0 = static_cast<double>(read_navigation_unsigned(data_jk_bits, TOW_0_bit));
DLOG(INFO) << "TOW_0= " << TOW_0; DLOG(INFO) << "TOW_0= " << TOW_0;
DLOG(INFO) << "flag_tow_set" << flag_TOW_set; DLOG(INFO) << "flag_tow_set" << flag_TOW_set;
break; break;

6
src/core/system_parameters/galileo_utc_model.cc
View File

@@ -67,7 +67,7 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int WN)
* the GST/Utc relationship is given by * the GST/Utc relationship is given by
*/ */
//std::cout<<"GST->UTC case a"<<std::endl; //std::cout<<"GST->UTC case a"<<std::endl;
Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)((WN % 256) - WNot_6)); Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * static_cast<double>((WN % 256) - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400); t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
} }
else else
@@ -78,7 +78,7 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int WN)
* the effective time is computed according to the following equations: * the effective time is computed according to the following equations:
*/ */
//std::cout<<"GST->UTC case b"<<std::endl; //std::cout<<"GST->UTC case b"<<std::endl;
Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)((WN % 256) - WNot_6)); Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * static_cast<double>((WN % 256) - WNot_6));
double W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200; double W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200;
t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6); t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
//implement something to handle a leap second event! //implement something to handle a leap second event!
@@ -94,7 +94,7 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int WN)
* the following equation: * the following equation:
*/ */
//std::cout<<"GST->UTC case c"<<std::endl; //std::cout<<"GST->UTC case c"<<std::endl;
Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)((WN % 256) - WNot_6)); Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * static_cast<double>((WN % 256) - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400); t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
} }

18
src/core/system_parameters/gnss_satellite.cc
View File

@@ -5,7 +5,7 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -15,7 +15,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -41,7 +41,7 @@ Gnss_Satellite::Gnss_Satellite()
Gnss_Satellite::Gnss_Satellite(std::string system_, unsigned int PRN_) Gnss_Satellite::Gnss_Satellite(const std::string& system_, unsigned int PRN_)
{ {
Gnss_Satellite::reset(); Gnss_Satellite::reset();
Gnss_Satellite::set_system(system_); Gnss_Satellite::set_system(system_);
@@ -61,12 +61,12 @@ Gnss_Satellite::~Gnss_Satellite()
void Gnss_Satellite::reset() void Gnss_Satellite::reset()
{ {
system_set = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}; system_set = {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"};
satelliteSystem["GPS"] = "G"; satelliteSystem["GPS"] = "G";
satelliteSystem["GLONASS"] = "R"; satelliteSystem["GLONASS"] = "R";
satelliteSystem["SBAS"] = "S"; satelliteSystem["SBAS"] = "S";
satelliteSystem["Galileo"] = "E"; satelliteSystem["Galileo"] = "E";
satelliteSystem["Compass"] = "C"; satelliteSystem["Beidou"] = "C";
PRN = 0; PRN = 0;
system = std::string(""); system = std::string("");
block = std::string(""); block = std::string("");
@@ -115,7 +115,7 @@ Gnss_Satellite& Gnss_Satellite::operator=(const Gnss_Satellite &rhs) {
}*/ }*/
void Gnss_Satellite::set_system(std::string system_) void Gnss_Satellite::set_system(const std::string& system_)
{ {
// Set the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"} // Set the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}
std::set<std::string>::iterator it = system_set.find(system_); std::set<std::string>::iterator it = system_set.find(system_);
@@ -126,7 +126,7 @@ void Gnss_Satellite::set_system(std::string system_)
} }
else else
{ {
DLOG(INFO) << "System " << system_ << " is not defined {GPS, GLONASS, SBAS, Galileo, Compass}. Initialization?"; DLOG(INFO) << "System " << system_ << " is not defined {GPS, GLONASS, SBAS, Galileo, Beidou}. Initialization?";
system = std::string(""); system = std::string("");
} }
} }
@@ -217,7 +217,7 @@ unsigned int Gnss_Satellite::get_PRN() const
std::string Gnss_Satellite::get_system() const std::string Gnss_Satellite::get_system() const
{ {
// Get the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"} // Get the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"}
std::string system_; std::string system_;
system_ = system; system_ = system;
return system_; return system_;
@@ -246,7 +246,7 @@ std::string Gnss_Satellite::get_block() const
void Gnss_Satellite::set_block(std::string system_, unsigned int PRN_ ) void Gnss_Satellite::set_block(const std::string& system_, unsigned int PRN_ )
{ {
if (system_.compare("GPS") == 0) if (system_.compare("GPS") == 0)
{ {

12
src/core/system_parameters/gnss_satellite.h
View File

@@ -5,7 +5,7 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -15,7 +15,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -48,10 +48,10 @@ class Gnss_Satellite
{ {
public: public:
Gnss_Satellite(); //!< Default Constructor. Gnss_Satellite(); //!< Default Constructor.
Gnss_Satellite(std::string system_, unsigned int PRN_); //!< Concrete GNSS satellite Constructor. Gnss_Satellite(const std::string& system_, unsigned int PRN_); //!< Concrete GNSS satellite Constructor.
~Gnss_Satellite(); //!< Default Destructor. ~Gnss_Satellite(); //!< Default Destructor.
unsigned int get_PRN() const; //!< Gets satellite's PRN unsigned int get_PRN() const; //!< Gets satellite's PRN
std::string get_system() const; //!< Gets the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"} std::string get_system() const; //!< Gets the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"}
std::string get_system_short() const; //!< Gets the satellite system {"G", "R", "SBAS", "E", "C"} std::string get_system_short() const; //!< Gets the satellite system {"G", "R", "SBAS", "E", "C"}
std::string get_block() const; //!< Gets the satellite block. If GPS, returns {"IIA", "IIR", "IIR-M", "IIF"} std::string get_block() const; //!< Gets the satellite block. If GPS, returns {"IIA", "IIR", "IIR-M", "IIF"}
friend bool operator== (const Gnss_Satellite &, const Gnss_Satellite &); //!< operator== for comparison friend bool operator== (const Gnss_Satellite &, const Gnss_Satellite &); //!< operator== for comparison
@@ -63,9 +63,9 @@ private:
std::map<std::string,std::string> satelliteSystem; std::map<std::string,std::string> satelliteSystem;
std::string block; std::string block;
signed int rf_link; signed int rf_link;
void set_system(std::string system); // Sets the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}. void set_system(const std::string& system); // Sets the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"}.
void set_PRN(unsigned int PRN); // Sets satellite's PRN void set_PRN(unsigned int PRN); // Sets satellite's PRN
void set_block(std::string system_, unsigned int PRN_ ); void set_block(const std::string& system_, unsigned int PRN_ );
std::set<std::string> system_set; // = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}; std::set<std::string> system_set; // = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"};
void reset(); void reset();

7
src/core/system_parameters/gnss_signal.cc
View File

@@ -6,7 +6,7 @@
* Javier Arribas, 2012. jarribas(at)cttc.es * Javier Arribas, 2012. jarribas(at)cttc.es
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -16,7 +16,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -28,6 +28,7 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#include "gnss_signal.h" #include "gnss_signal.h"
Gnss_Signal::Gnss_Signal() Gnss_Signal::Gnss_Signal()
@@ -36,7 +37,7 @@ Gnss_Signal::Gnss_Signal()
} }
Gnss_Signal::Gnss_Signal(Gnss_Satellite satellite_,std::string signal_) Gnss_Signal::Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_)
{ {
this->satellite = satellite_; this->satellite = satellite_;
this->signal = signal_; this->signal = signal_;

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

@@ -6,7 +6,7 @@
* Javier Arribas, 2012. jarribas(at)cttc.es * Javier Arribas, 2012. jarribas(at)cttc.es
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -16,7 +16,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -47,7 +47,7 @@ private:
std::string signal; std::string signal;
public: public:
Gnss_Signal(); Gnss_Signal();
Gnss_Signal(Gnss_Satellite satellite_, std::string signal_); Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_);
~Gnss_Signal(); ~Gnss_Signal();
std::string get_signal() const; //!< Get the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"} std::string get_signal() const; //!< Get the satellite system {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"}
Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal

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

@@ -7,7 +7,7 @@
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -17,7 +17,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -303,11 +303,11 @@ double Gps_Navigation_Message::check_t(double time)
corrTime = time; corrTime = time;
if (time > half_week) if (time > half_week)
{ {
corrTime = time - 2*half_week; corrTime = time - 2 * half_week;
} }
else if (time < -half_week) else if (time < -half_week)
{ {
corrTime = time + 2*half_week; corrTime = time + 2 * half_week;
} }
return corrTime; return corrTime;
} }
@@ -349,13 +349,13 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
// Find satellite's position ---------------------------------------------- // Find satellite's position ----------------------------------------------
// Restore semi-major axis // Restore semi-major axis
a = d_sqrt_A*d_sqrt_A; a = d_sqrt_A * d_sqrt_A;
// Time from ephemeris reference epoch // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe); tk = check_t(transmitTime - d_Toe);
// Computed mean motion // Computed mean motion
n0 = sqrt(GM / (a*a*a)); n0 = sqrt(GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; n = n0 + d_Delta_n;
@@ -364,7 +364,7 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
M = d_M_0 + n * tk; M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2*GPS_PI), (2*GPS_PI)); M = fmod((M + 2 * GPS_PI), (2 * GPS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; E = M;
@@ -374,7 +374,7 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
{ {
E_old = E; E_old = E;
E = M + d_e_eccentricity * sin(E); E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2*GPS_PI); dE = fmod(E - E_old, 2 * GPS_PI);
if (fabs(dE) < 1e-12) if (fabs(dE) < 1e-12)
{ {
//Necessary precision is reached, exit from the loop //Necessary precision is reached, exit from the loop
@@ -394,22 +394,22 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
phi = nu + d_OMEGA; phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2*GPS_PI)); phi = fmod((phi), (2 * GPS_PI));
// Correct argument of latitude // Correct argument of latitude
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi); u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi);
// Correct radius // Correct radius
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi); r = a * (1 - d_e_eccentricity * cos(E)) + d_Crc * cos(2 * phi) + d_Crs * sin(2 * phi);
// Correct inclination // Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi); i = d_i_0 + d_IDOT * tk + d_Cic * cos(2 * phi) + d_Cis * sin(2 * phi);
// Compute the angle between the ascending node and the Greenwich meridian // Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe; Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT) * tk - OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2*GPS_PI), (2*GPS_PI)); Omega = fmod((Omega + 2 * GPS_PI), (2 * GPS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates // --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@@ -438,18 +438,18 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
// UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE
std::bitset<GPS_SUBFRAME_BITS> subframe_bits; std::bitset<GPS_SUBFRAME_BITS> subframe_bits;
std::bitset<GPS_WORD_BITS+2> word_bits; std::bitset<GPS_WORD_BITS + 2> word_bits;
for (int i=0; i<10; i++) for (int i=0; i<10; i++)
{ {
memcpy(&gps_word, &subframe[i*4], sizeof(char)*4); memcpy(&gps_word, &subframe[i * 4], sizeof(char) * 4);
word_bits = std::bitset<(GPS_WORD_BITS+2)>(gps_word); word_bits = std::bitset<(GPS_WORD_BITS + 2) > (gps_word);
for (int j=0; j<GPS_WORD_BITS; j++) for (int j = 0; j < GPS_WORD_BITS; j++)
{ {
subframe_bits[GPS_WORD_BITS*(9-i) + j] = word_bits[j]; subframe_bits[GPS_WORD_BITS * (9 - i) + j] = word_bits[j];
} }
} }
subframe_ID = (int)read_navigation_unsigned(subframe_bits, SUBFRAME_ID); subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, SUBFRAME_ID));
// Decode all 5 sub-frames // Decode all 5 sub-frames
switch (subframe_ID) switch (subframe_ID)
@@ -463,98 +463,98 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
// subframe and we need the TOW of the first subframe in this data block // subframe and we need the TOW of the first subframe in this data block
// (the variable subframe at this point contains bits of the last subframe). // (the variable subframe at this point contains bits of the last subframe).
//TOW = bin2dec(subframe(31:47)) * 6 - 30; //TOW = bin2dec(subframe(31:47)) * 6 - 30;
d_TOW_SF1 = (double)read_navigation_unsigned(subframe_bits, TOW); d_TOW_SF1 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
//we are in the first subframe (the transmitted TOW is the start time of the next subframe) ! //we are in the first subframe (the transmitted TOW is the start time of the next subframe) !
d_TOW_SF1 = d_TOW_SF1*6; d_TOW_SF1 = d_TOW_SF1 * 6;
d_TOW = d_TOW_SF1 - 6; // Set transmission time d_TOW = d_TOW_SF1 - 6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
i_GPS_week = (int)read_navigation_unsigned(subframe_bits, GPS_WEEK); i_GPS_week = static_cast<int>(read_navigation_unsigned(subframe_bits, GPS_WEEK));
i_SV_accuracy = (int)read_navigation_unsigned(subframe_bits, SV_ACCURACY); // (20.3.3.3.1.3) i_SV_accuracy = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_ACCURACY)); // (20.3.3.3.1.3)
i_SV_health = (int)read_navigation_unsigned(subframe_bits, SV_HEALTH); i_SV_health = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_HEALTH));
b_L2_P_data_flag = read_navigation_bool(subframe_bits, L2_P_DATA_FLAG); // b_L2_P_data_flag = read_navigation_bool(subframe_bits, L2_P_DATA_FLAG); //
i_code_on_L2 = (int)read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2); i_code_on_L2 = static_cast<int>(read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2));
d_TGD = (double)read_navigation_signed(subframe_bits, T_GD); d_TGD = static_cast<double>(read_navigation_signed(subframe_bits, T_GD));
d_TGD = d_TGD*T_GD_LSB; d_TGD = d_TGD * T_GD_LSB;
d_IODC = (double)read_navigation_unsigned(subframe_bits, IODC); d_IODC = static_cast<double>(read_navigation_unsigned(subframe_bits, IODC));
d_Toc = (double)read_navigation_unsigned(subframe_bits, T_OC); d_Toc = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OC));
d_Toc = d_Toc*T_OC_LSB; d_Toc = d_Toc * T_OC_LSB;
d_A_f0 = (double)read_navigation_signed(subframe_bits, A_F0); d_A_f0 = static_cast<double>(read_navigation_signed(subframe_bits, A_F0));
d_A_f0 = d_A_f0*A_F0_LSB; d_A_f0 = d_A_f0 * A_F0_LSB;
d_A_f1 = (double)read_navigation_signed(subframe_bits, A_F1); d_A_f1 = static_cast<double>(read_navigation_signed(subframe_bits, A_F1));
d_A_f1 = d_A_f1*A_F1_LSB; d_A_f1 = d_A_f1 * A_F1_LSB;
d_A_f2 = (double)read_navigation_signed(subframe_bits, A_F2); d_A_f2 = static_cast<double>(read_navigation_signed(subframe_bits, A_F2));
d_A_f2 = d_A_f2*A_F2_LSB; d_A_f2 = d_A_f2 * A_F2_LSB;
break; break;
case 2: //--- It is subframe 2 ------------------- case 2: //--- It is subframe 2 -------------------
d_TOW_SF2 = (double)read_navigation_unsigned(subframe_bits, TOW); d_TOW_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF2 = d_TOW_SF2*6; d_TOW_SF2 = d_TOW_SF2 * 6;
d_TOW = d_TOW_SF2 - 6; // Set transmission time d_TOW = d_TOW_SF2 - 6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
d_IODE_SF2 = (double)read_navigation_unsigned(subframe_bits, IODE_SF2); d_IODE_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, IODE_SF2));
d_Crs = (double)read_navigation_signed(subframe_bits, C_RS); d_Crs = static_cast<double>(read_navigation_signed(subframe_bits, C_RS));
d_Crs = d_Crs * C_RS_LSB; d_Crs = d_Crs * C_RS_LSB;
d_Delta_n = (double)read_navigation_signed(subframe_bits, DELTA_N); d_Delta_n = static_cast<double>(read_navigation_signed(subframe_bits, DELTA_N));
d_Delta_n = d_Delta_n * DELTA_N_LSB; d_Delta_n = d_Delta_n * DELTA_N_LSB;
d_M_0 = (double)read_navigation_signed(subframe_bits, M_0); d_M_0 = static_cast<double>(read_navigation_signed(subframe_bits, M_0));
d_M_0 = d_M_0 * M_0_LSB; d_M_0 = d_M_0 * M_0_LSB;
d_Cuc = (double)read_navigation_signed(subframe_bits, C_UC); d_Cuc = static_cast<double>(read_navigation_signed(subframe_bits, C_UC));
d_Cuc = d_Cuc * C_UC_LSB; d_Cuc = d_Cuc * C_UC_LSB;
d_e_eccentricity = (double)read_navigation_unsigned(subframe_bits, E); d_e_eccentricity = static_cast<double>(read_navigation_unsigned(subframe_bits, E));
d_e_eccentricity = d_e_eccentricity * E_LSB; d_e_eccentricity = d_e_eccentricity * E_LSB;
d_Cus = (double)read_navigation_signed(subframe_bits, C_US); d_Cus = static_cast<double>(read_navigation_signed(subframe_bits, C_US));
d_Cus = d_Cus * C_US_LSB; d_Cus = d_Cus * C_US_LSB;
d_sqrt_A = (double)read_navigation_unsigned(subframe_bits, SQRT_A); d_sqrt_A = static_cast<double>(read_navigation_unsigned(subframe_bits, SQRT_A));
d_sqrt_A = d_sqrt_A * SQRT_A_LSB; d_sqrt_A = d_sqrt_A * SQRT_A_LSB;
d_Toe = (double)read_navigation_unsigned(subframe_bits, T_OE); d_Toe = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OE));
d_Toe = d_Toe * T_OE_LSB; d_Toe = d_Toe * T_OE_LSB;
b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG); b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG);
i_AODO = (int)read_navigation_unsigned(subframe_bits, AODO); i_AODO = static_cast<int>(read_navigation_unsigned(subframe_bits, AODO));
i_AODO = i_AODO * AODO_LSB; i_AODO = i_AODO * AODO_LSB;
break; break;
case 3: // --- It is subframe 3 ------------------------------------- case 3: // --- It is subframe 3 -------------------------------------
d_TOW_SF3 = (double)read_navigation_unsigned(subframe_bits, TOW); d_TOW_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF3 = d_TOW_SF3*6; d_TOW_SF3 = d_TOW_SF3 * 6;
d_TOW = d_TOW_SF3 - 6; // Set transmission time d_TOW = d_TOW_SF3 - 6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
d_Cic = (double)read_navigation_signed(subframe_bits, C_IC); d_Cic = static_cast<double>(read_navigation_signed(subframe_bits, C_IC));
d_Cic = d_Cic * C_IC_LSB; d_Cic = d_Cic * C_IC_LSB;
d_OMEGA0 = (double)read_navigation_signed(subframe_bits, OMEGA_0); d_OMEGA0 = static_cast<double>(read_navigation_signed(subframe_bits, OMEGA_0));
d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB; d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB;
d_Cis = (double)read_navigation_signed(subframe_bits, C_IS); d_Cis = static_cast<double>(read_navigation_signed(subframe_bits, C_IS));
d_Cis = d_Cis * C_IS_LSB; d_Cis = d_Cis * C_IS_LSB;
d_i_0 = (double)read_navigation_signed(subframe_bits, I_0); d_i_0 = static_cast<double>(read_navigation_signed(subframe_bits, I_0));
d_i_0 = d_i_0 * I_0_LSB; d_i_0 = d_i_0 * I_0_LSB;
d_Crc = (double)read_navigation_signed(subframe_bits, C_RC); d_Crc = static_cast<double>(read_navigation_signed(subframe_bits, C_RC));
d_Crc = d_Crc * C_RC_LSB; d_Crc = d_Crc * C_RC_LSB;
d_OMEGA = (double)read_navigation_signed(subframe_bits, OMEGA); d_OMEGA = static_cast<double>(read_navigation_signed(subframe_bits, OMEGA));
d_OMEGA = d_OMEGA * OMEGA_LSB; d_OMEGA = d_OMEGA * OMEGA_LSB;
d_OMEGA_DOT = (double)read_navigation_signed(subframe_bits, OMEGA_DOT); d_OMEGA_DOT = static_cast<double>(read_navigation_signed(subframe_bits, OMEGA_DOT));
d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB; d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB;
d_IODE_SF3 = (double)read_navigation_unsigned(subframe_bits, IODE_SF3); d_IODE_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, IODE_SF3));
d_IDOT = (double)read_navigation_signed(subframe_bits, I_DOT); d_IDOT = static_cast<double>(read_navigation_signed(subframe_bits, I_DOT));
d_IDOT = d_IDOT*I_DOT_LSB; d_IDOT = d_IDOT * I_DOT_LSB;
break; break;
case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32) case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32)
d_TOW_SF4 = (double)read_navigation_unsigned(subframe_bits, TOW); d_TOW_SF4 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF4 = d_TOW_SF4*6; d_TOW_SF4 = d_TOW_SF4 * 6;
d_TOW = d_TOW_SF4 - 6; // Set transmission time d_TOW = d_TOW_SF4 - 6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID); SV_data_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_DATA_ID));
SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE); SV_page = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_PAGE));
if (SV_page == 13) if (SV_page == 13)
{ {
@@ -564,33 +564,33 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
if (SV_page == 18) if (SV_page == 18)
{ {
// Page 18 - Ionospheric and UTC data // Page 18 - Ionospheric and UTC data
d_alpha0 = (double)read_navigation_signed(subframe_bits, ALPHA_0); d_alpha0 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_0));
d_alpha0 = d_alpha0 * ALPHA_0_LSB; d_alpha0 = d_alpha0 * ALPHA_0_LSB;
d_alpha1 = (double)read_navigation_signed(subframe_bits, ALPHA_1); d_alpha1 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_1));
d_alpha1 = d_alpha1 * ALPHA_1_LSB; d_alpha1 = d_alpha1 * ALPHA_1_LSB;
d_alpha2 = (double)read_navigation_signed(subframe_bits, ALPHA_2); d_alpha2 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_2));
d_alpha2 = d_alpha2 * ALPHA_2_LSB; d_alpha2 = d_alpha2 * ALPHA_2_LSB;
d_alpha3 = (double)read_navigation_signed(subframe_bits, ALPHA_3); d_alpha3 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_3));
d_alpha3 = d_alpha3 * ALPHA_3_LSB; d_alpha3 = d_alpha3 * ALPHA_3_LSB;
d_beta0 = (double)read_navigation_signed(subframe_bits, BETA_0); d_beta0 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_0));
d_beta0 = d_beta0 * BETA_0_LSB; d_beta0 = d_beta0 * BETA_0_LSB;
d_beta1 = (double)read_navigation_signed(subframe_bits, BETA_1); d_beta1 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_1));
d_beta1 = d_beta1 * BETA_1_LSB; d_beta1 = d_beta1 * BETA_1_LSB;
d_beta2 = (double)read_navigation_signed(subframe_bits, BETA_2); d_beta2 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_2));
d_beta2 = d_beta2 * BETA_2_LSB; d_beta2 = d_beta2 * BETA_2_LSB;
d_beta3 = (double)read_navigation_signed(subframe_bits, BETA_3); d_beta3 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_3));
d_beta3 = d_beta3 * BETA_3_LSB; d_beta3 = d_beta3 * BETA_3_LSB;
d_A1 = (double)read_navigation_signed(subframe_bits, A_1); d_A1 = static_cast<double>(read_navigation_signed(subframe_bits, A_1));
d_A1 = d_A1 * A_1_LSB; d_A1 = d_A1 * A_1_LSB;
d_A0 = (double)read_navigation_signed(subframe_bits, A_0); d_A0 = static_cast<double>(read_navigation_signed(subframe_bits, A_0));
d_A0 = d_A0 * A_0_LSB; d_A0 = d_A0 * A_0_LSB;
d_t_OT = (double)read_navigation_unsigned(subframe_bits, T_OT); d_t_OT = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OT));
d_t_OT = d_t_OT * T_OT_LSB; d_t_OT = d_t_OT * T_OT_LSB;
i_WN_T = (int)read_navigation_unsigned(subframe_bits, WN_T); i_WN_T = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_T));
d_DeltaT_LS = (double)read_navigation_signed(subframe_bits, DELTAT_LS); d_DeltaT_LS = static_cast<double>(read_navigation_signed(subframe_bits, DELTAT_LS));
i_WN_LSF = (int)read_navigation_unsigned(subframe_bits, WN_LSF); i_WN_LSF = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_LSF));
i_DN = (int)read_navigation_unsigned(subframe_bits, DN); // Right-justified ? i_DN = static_cast<int>(read_navigation_unsigned(subframe_bits, DN)); // Right-justified ?
d_DeltaT_LSF = (double)read_navigation_signed(subframe_bits, DELTAT_LSF); d_DeltaT_LSF = static_cast<double>(read_navigation_signed(subframe_bits, DELTAT_LSF));
flag_iono_valid = true; flag_iono_valid = true;
flag_utc_model_valid = true; flag_utc_model_valid = true;
} }
@@ -599,60 +599,60 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
{ {
// Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32) // Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32)
//! \TODO Read Anti-Spoofing, SV config //! \TODO Read Anti-Spoofing, SV config
almanacHealth[25] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV25); almanacHealth[25] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV25));
almanacHealth[26] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV26); almanacHealth[26] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV26));
almanacHealth[27] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV27); almanacHealth[27] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV27));
almanacHealth[28] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV28); almanacHealth[28] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV28));
almanacHealth[29] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV29); almanacHealth[29] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV29));
almanacHealth[30] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV30); almanacHealth[30] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV30));
almanacHealth[31] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV31); almanacHealth[31] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV31));
almanacHealth[32] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV32); almanacHealth[32] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV32));
} }
break; break;
case 5://--- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time. case 5://--- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time.
d_TOW_SF5 = (double)read_navigation_unsigned(subframe_bits, TOW); d_TOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF5 = d_TOW_SF5*6; d_TOW_SF5 = d_TOW_SF5 * 6;
d_TOW = d_TOW_SF5 - 6; // Set transmission time d_TOW = d_TOW_SF5 - 6; // Set transmission time
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID); SV_data_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_DATA_ID));
SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE); SV_page = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_PAGE));
if (SV_page < 25) if (SV_page < 25)
{ {
//! \TODO read almanac //! \TODO read almanac
} }
if (SV_page == 25) if (SV_page == 25)
{ {
d_Toa = (double)read_navigation_unsigned(subframe_bits, T_OA); d_Toa = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OA));
d_Toa = d_Toa * T_OA_LSB; d_Toa = d_Toa * T_OA_LSB;
i_WN_A = (int)read_navigation_unsigned(subframe_bits, WN_A); i_WN_A = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_A));
almanacHealth[1] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV1); almanacHealth[1] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV1));
almanacHealth[2] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV2); almanacHealth[2] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV2));
almanacHealth[3] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV3); almanacHealth[3] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV3));
almanacHealth[4] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV4); almanacHealth[4] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV4));
almanacHealth[5] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV5); almanacHealth[5] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV5));
almanacHealth[6] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV6); almanacHealth[6] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV6));
almanacHealth[7] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV7); almanacHealth[7] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV7));
almanacHealth[8] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV8); almanacHealth[8] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV8));
almanacHealth[9] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV9); almanacHealth[9] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV9));
almanacHealth[10] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV10); almanacHealth[10] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV10));
almanacHealth[11] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV11); almanacHealth[11] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV11));
almanacHealth[12] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV12); almanacHealth[12] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV12));
almanacHealth[13] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV13); almanacHealth[13] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV13));
almanacHealth[14] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV14); almanacHealth[14] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV14));
almanacHealth[15] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV15); almanacHealth[15] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV15));
almanacHealth[16] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV16); almanacHealth[16] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV16));
almanacHealth[17] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV17); almanacHealth[17] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV17));
almanacHealth[18] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV18); almanacHealth[18] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV18));
almanacHealth[19] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV19); almanacHealth[19] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV19));
almanacHealth[20] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV20); almanacHealth[20] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV20));
almanacHealth[21] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV21); almanacHealth[21] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV21));
almanacHealth[22] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV22); almanacHealth[22] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV22));
almanacHealth[23] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV23); almanacHealth[23] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV23));
almanacHealth[24] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV24); almanacHealth[24] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV24));
} }
break; break;
@@ -666,11 +666,11 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
double Gps_Navigation_Message::utc_time(double gpstime_corrected) double Gps_Navigation_Message::utc_time(const double gpstime_corrected) const
{ {
double t_utc; double t_utc;
double t_utc_daytime; double t_utc_daytime;
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T)); double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<double>((i_GPS_week - i_WN_T)));
// Determine if the effectivity time of the leap second event is in the past // Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week;
@@ -710,7 +710,7 @@ double Gps_Navigation_Message::utc_time(double gpstime_corrected)
} }
if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600) if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{ {
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800*(double)(i_GPS_week - i_WN_T)); Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<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);
} }
} }
@@ -722,7 +722,7 @@ double Gps_Navigation_Message::utc_time(double gpstime_corrected)
* WNLSF and DN values, is in the "past" (relative to the user's current time), * WNLSF and DN values, is in the "past" (relative to the user's current time),
* and the user<65>s current time does not fall in the time span as given above * and the user<65>s current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/ * in 20.3.3.5.2.4b,*/
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T)); Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<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);
} }

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

@@ -15,7 +15,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -231,7 +231,7 @@ public:
* \brief Computes the Coordinated Universal Time (UTC) and * \brief Computes the Coordinated Universal Time (UTC) and
* returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4) * returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4)
*/ */
double utc_time(double gpstime_corrected); double utc_time(const double gpstime_corrected) const;
bool satellite_validation(); bool satellite_validation();

6
src/core/system_parameters/gps_utc_model.cc
View File

@@ -47,7 +47,7 @@ double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
{ {
double t_utc; double t_utc;
double t_utc_daytime; double t_utc_daytime;
double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T)); double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<double>(i_GPS_week - i_WN_T));
// Determine if the effectivity time of the leap second event is in the past // Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week;
@@ -87,7 +87,7 @@ double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
} }
if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600) if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600)
{ {
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800*(double)(i_GPS_week - i_WN_T)); Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<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);
} }
} }
@@ -99,7 +99,7 @@ double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
* WNLSF and DN values, is in the "past" (relative to the user's current time), * WNLSF and DN values, is in the "past" (relative to the user's current time),
* and the user<65>s current time does not fall in the time span as given above * and the user<65>s current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/ * in 20.3.3.5.2.4b,*/
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T)); Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast<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);
} }

8
src/core/system_parameters/sbas_telemetry_data.h
View File

@@ -15,7 +15,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify * GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or * the Free Software Foundation, either version 3 of the License, or
* at your option) any later version. * (at your option) any later version.
* *
* GNSS-SDR is distributed in the hope that it will be useful, * GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -64,14 +64,14 @@ public:
{ {
rx_time.relate(time_relation); rx_time.relate(time_relation);
} }
Sbas_Time get_rx_time_obj(){ return rx_time; } Sbas_Time get_rx_time_obj() const { return rx_time; }
int get_prn() { return i_prn; } int get_prn() const { return i_prn; }
std::vector<unsigned char> get_msg() const { return d_msg; } std::vector<unsigned char> get_msg() const { return d_msg; }
int get_preamble() int get_preamble()
{ {
return d_msg[0]; return d_msg[0];
} }
int get_msg_type() int get_msg_type() const
{ {
return d_msg[1] >> 2; return d_msg[1] >> 2;
} }

2
src/main/main.cc
View File

@@ -178,7 +178,7 @@ int main(int argc, char** argv)
gettimeofday(&tv, NULL); gettimeofday(&tv, NULL);
long long int end = tv.tv_sec * 1000000 + tv.tv_usec; long long int end = tv.tv_sec * 1000000 + tv.tv_usec;
std::cout << "Total GNSS-SDR run time " std::cout << "Total GNSS-SDR run time "
<< ((double)(end - begin))/1000000.0 << (static_cast<double>(end - begin)) / 1000000.0
<< " [seconds]" << std::endl; << " [seconds]" << std::endl;
google::ShutDownCommandLineFlags(); google::ShutDownCommandLineFlags();

8
src/tests/arithmetic/conjugate_test.cc
View File

@@ -116,8 +116,8 @@ TEST(Conjugate_Test, ArmadilloComplexImplementation)
TEST(Conjugate_Test, VolkComplexImplementation) TEST(Conjugate_Test, VolkComplexImplementation)
{ {
std::complex<float>* input = new std::complex<float>[FLAGS_size_conjugate_test]; std::complex<float>* input = static_cast<std::complex<float>*>(volk_malloc(FLAGS_size_conjugate_test * sizeof(std::complex<float>), volk_get_alignment()));
std::complex<float>* output = new std::complex<float>[FLAGS_size_conjugate_test]; std::complex<float>* output = static_cast<std::complex<float>*>(volk_malloc(FLAGS_size_conjugate_test * sizeof(std::complex<float>), volk_get_alignment()));
memset(input, 0, sizeof(std::complex<float>) * FLAGS_size_conjugate_test); memset(input, 0, sizeof(std::complex<float>) * FLAGS_size_conjugate_test);
struct timeval tv; struct timeval tv;
@@ -132,6 +132,6 @@ TEST(Conjugate_Test, VolkComplexImplementation)
<< "-length complex float vector using VOLK finished in " << (end - begin) << "-length complex float vector using VOLK finished in " << (end - begin)
<< " microseconds" << std::endl; << " microseconds" << std::endl;
ASSERT_LE(0, end - begin); ASSERT_LE(0, end - begin);
delete [] input; volk_free(input);
delete [] output; volk_free(output);
} }

10
src/tests/arithmetic/magnitude_squared_test.cc
View File

@@ -116,14 +116,14 @@ TEST(MagnitudeSquared_Test, ArmadilloComplexImplementation)
TEST(MagnitudeSquared_Test, VolkComplexImplementation) TEST(MagnitudeSquared_Test, VolkComplexImplementation)
{ {
std::complex<float>* input = new std::complex<float>[FLAGS_size_magnitude_test]; std::complex<float>* input = static_cast<std::complex<float>*>(volk_malloc(FLAGS_size_magnitude_test * sizeof(std::complex<float>), volk_get_alignment()));
memset(input, 0, sizeof(std::complex<float>) * FLAGS_size_magnitude_test); memset(input, 0, sizeof(std::complex<float>) * FLAGS_size_magnitude_test);
float* output = new float[FLAGS_size_magnitude_test]; float* output = static_cast<float*>(volk_malloc(FLAGS_size_magnitude_test * sizeof(float), volk_get_alignment()));
struct timeval tv; struct timeval tv;
gettimeofday(&tv, NULL); gettimeofday(&tv, NULL);
long long int begin = tv.tv_sec * 1000000 + tv.tv_usec; long long int begin = tv.tv_sec * 1000000 + tv.tv_usec;
volk_32fc_magnitude_squared_32f(output, input, (unsigned int)FLAGS_size_magnitude_test); volk_32fc_magnitude_squared_32f(output, input, static_cast<unsigned int>(FLAGS_size_magnitude_test));
gettimeofday(&tv, NULL); gettimeofday(&tv, NULL);
long long int end = tv.tv_sec * 1000000 + tv.tv_usec; long long int end = tv.tv_sec * 1000000 + tv.tv_usec;
@@ -131,8 +131,8 @@ TEST(MagnitudeSquared_Test, VolkComplexImplementation)
<< "-length vector using VOLK computed in " << (end - begin) << "-length vector using VOLK computed in " << (end - begin)
<< " microseconds" << std::endl; << " microseconds" << std::endl;
ASSERT_LE(0, end - begin); ASSERT_LE(0, end - begin);
delete [] input; volk_free(input);
delete [] output; volk_free(output);
} }
// volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size); // volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);

13
src/tests/arithmetic/multiply_test.cc
View File

@@ -181,8 +181,8 @@ TEST(Multiply_Test, ArmadilloComplexImplementation)
TEST(Multiply_Test, VolkComplexImplementation) TEST(Multiply_Test, VolkComplexImplementation)
{ {
std::complex<float>* input = new std::complex<float>[FLAGS_size_multiply_test]; std::complex<float>* input = static_cast<std::complex<float>*>(volk_malloc(FLAGS_size_multiply_test * sizeof(std::complex<float>), volk_get_alignment()));
std::complex<float>* output = new std::complex<float>[FLAGS_size_multiply_test]; std::complex<float>* output = static_cast<std::complex<float>*>(volk_malloc(FLAGS_size_multiply_test * sizeof(std::complex<float>), volk_get_alignment()));
memset(input, 0, sizeof(std::complex<float>) * FLAGS_size_multiply_test); memset(input, 0, sizeof(std::complex<float>) * FLAGS_size_multiply_test);
struct timeval tv; struct timeval tv;
@@ -198,8 +198,9 @@ TEST(Multiply_Test, VolkComplexImplementation)
<< " microseconds" << std::endl; << " microseconds" << std::endl;
ASSERT_LE(0, end - begin); ASSERT_LE(0, end - begin);
float* mag = new float [FLAGS_size_multiply_test]; float* mag = static_cast<float*>(volk_malloc(FLAGS_size_multiply_test * sizeof(float), volk_get_alignment()));
volk_32fc_magnitude_32f(mag, output, FLAGS_size_multiply_test); volk_32fc_magnitude_32f(mag, output, FLAGS_size_multiply_test);
float* result = new float(0); float* result = new float(0);
volk_32f_accumulator_s32f(result, mag, FLAGS_size_multiply_test); volk_32f_accumulator_s32f(result, mag, FLAGS_size_multiply_test);
// Comparing floating-point numbers is tricky. // Comparing floating-point numbers is tricky.
@@ -207,8 +208,8 @@ TEST(Multiply_Test, VolkComplexImplementation)
// See http://code.google.com/p/googletest/wiki/AdvancedGuide#Floating-Point_Comparison // See http://code.google.com/p/googletest/wiki/AdvancedGuide#Floating-Point_Comparison
float expected = 0; float expected = 0;
ASSERT_FLOAT_EQ(expected, result[0]); ASSERT_FLOAT_EQ(expected, result[0]);
delete [] input; volk_free(input);
delete [] output; volk_free(output);
delete [] mag; volk_free(mag);
} }

24
src/tests/gnss_block/galileo_e5a_pcps_acquisition_gsoc2014_gensource_test.cc
View File

@@ -183,19 +183,11 @@ void GalileoE5aPcpsAcquisitionGSoC2014GensourceTest::config_1()
config = std::make_shared<InMemoryConfiguration>(); config = std::make_shared<InMemoryConfiguration>();
config->set_property("Channel.signal",signal); config->set_property("Channel.signal",signal);
config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in)); config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(fs_in));
int a = config->property("GNSS-SDR.internal_fs_hz",10);
std::cout << "fs "<< a <<std::endl;
config->set_property("SignalSource.fs_hz", std::to_string(fs_in)); config->set_property("SignalSource.fs_hz", std::to_string(fs_in));
config->set_property("SignalSource.item_type", "gr_complex"); config->set_property("SignalSource.item_type", "gr_complex");
config->set_property("SignalSource.num_satellites", "1"); config->set_property("SignalSource.num_satellites", "1");
config->set_property("SignalSource.system_0", "E"); config->set_property("SignalSource.system_0", "E");
config->set_property("SignalSource.signal_0", "5X"); config->set_property("SignalSource.signal_0", "5X");
config->set_property("SignalSource.PRN_0", "11"); config->set_property("SignalSource.PRN_0", "11");
@@ -325,12 +317,8 @@ void GalileoE5aPcpsAcquisitionGSoC2014GensourceTest::config_3()
expected_delay_sec3 = 77; expected_delay_sec3 = 77;
expected_doppler_hz3 = 5000; expected_doppler_hz3 = 5000;
max_doppler_error_hz = 2/(3*integration_time_ms*1e-3); max_doppler_error_hz = 2/(3*integration_time_ms*1e-3);
max_delay_error_chips = 0.50; max_delay_error_chips = 0.50;
//max_doppler_error_hz = 1000; //max_doppler_error_hz = 1000;
//max_delay_error_chips = 1; //max_delay_error_chips = 1;
@@ -497,9 +485,9 @@ void GalileoE5aPcpsAcquisitionGSoC2014GensourceTest::process_message()
realization_counter++; realization_counter++;
std::cout << correct_estimation_counter << "correct estimation counter" << std::endl; //std::cout << correct_estimation_counter << "correct estimation counter" << std::endl;
std::cout << "Progress: " << round((float)realization_counter/num_of_realizations*100) << "% \r" << std::flush; std::cout << "Progress: " << round((float)realization_counter/num_of_realizations*100) << "% \r" << std::flush;
std::cout << message << "message" <<std::endl; //std::cout << message << "message" <<std::endl;
if (realization_counter == num_of_realizations) if (realization_counter == num_of_realizations)
{ {
mse_delay /= num_of_realizations; mse_delay /= num_of_realizations;
@@ -611,7 +599,7 @@ TEST_F(GalileoE5aPcpsAcquisitionGSoC2014GensourceTest, ValidationOfSIM)
config_1(); config_1();
//int nsamples = floor(fs_in*integration_time_ms*1e-3); //int nsamples = floor(fs_in*integration_time_ms*1e-3);
acquisition = std::make_shared<GalileoE5aNoncoherentIQAcquisitionCaf>(config.get(), "Acquisition_Galileo", 1, 1, queue); acquisition = std::make_shared<GalileoE5aNoncoherentIQAcquisitionCaf>(config.get(), "Acquisition", 1, 1, queue);
//unsigned int skiphead_sps = 28000+32000; // 32 Msps //unsigned int skiphead_sps = 28000+32000; // 32 Msps
// unsigned int skiphead_sps = 0; // unsigned int skiphead_sps = 0;
@@ -722,9 +710,9 @@ TEST_F(GalileoE5aPcpsAcquisitionGSoC2014GensourceTest, ValidationOfSIM)
top_block->run(); // Start threads and wait top_block->run(); // Start threads and wait
}) << "Failure running the top_block."<< std::endl; }) << "Failure running the top_block."<< std::endl;
std::cout << gnss_synchro.Acq_delay_samples << "acq delay" <<std::endl; //std::cout << gnss_synchro.Acq_delay_samples << "acq delay" <<std::endl;
std::cout << gnss_synchro.Acq_doppler_hz << "acq doppler" <<std::endl; //std::cout << gnss_synchro.Acq_doppler_hz << "acq doppler" <<std::endl;
std::cout << gnss_synchro.Acq_samplestamp_samples << "acq samples" <<std::endl; //std::cout << gnss_synchro.Acq_samplestamp_samples << "acq samples" <<std::endl;
// if (i == 0) // if (i == 0)
// { // {
// EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS."; // EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS.";

7
src/tests/gnss_block/galileo_e5a_tracking_test.cc
View File

@@ -85,6 +85,7 @@ void GalileoE5aTrackingTest::init()
gnss_synchro.System = 'E'; gnss_synchro.System = 'E';
std::string signal = "5Q"; std::string signal = "5Q";
signal.copy(gnss_synchro.Signal, 2, 0); signal.copy(gnss_synchro.Signal, 2, 0);
gnss_synchro.PRN = 11;
config->set_property("GNSS-SDR.internal_fs_hz", "32000000"); config->set_property("GNSS-SDR.internal_fs_hz", "32000000");
config->set_property("Tracking_Galileo.item_type", "gr_complex"); config->set_property("Tracking_Galileo.item_type", "gr_complex");
@@ -92,12 +93,12 @@ void GalileoE5aTrackingTest::init()
config->set_property("Tracking_Galileo.dump_filename", "../data/e5a_tracking_ch_"); config->set_property("Tracking_Galileo.dump_filename", "../data/e5a_tracking_ch_");
config->set_property("Tracking_Galileo.implementation", "Galileo_E5a_DLL_PLL_Tracking"); config->set_property("Tracking_Galileo.implementation", "Galileo_E5a_DLL_PLL_Tracking");
config->set_property("Tracking_Galileo.early_late_space_chips", "0.5"); config->set_property("Tracking_Galileo.early_late_space_chips", "0.5");
config->set_property("Tracking_Galileo.order", "2");
config->set_property("Tracking_Galileo.pll_bw_hz_init","20.0"); config->set_property("Tracking_Galileo.pll_bw_hz_init","20.0");
config->set_property("Tracking_Galileo.ti_ms", "1");
config->set_property("Tracking_Galileo.dll_bw_hz_init","2.0");
config->set_property("Tracking_Galileo.pll_bw_hz", "5"); config->set_property("Tracking_Galileo.pll_bw_hz", "5");
config->set_property("Tracking_Galileo.dll_bw_hz_init","2.0");
config->set_property("Tracking_Galileo.dll_bw_hz", "2"); config->set_property("Tracking_Galileo.dll_bw_hz", "2");
config->set_property("Tracking_Galileo.ti_ms", "1");
} }
TEST_F(GalileoE5aTrackingTest, ValidationOfResults) TEST_F(GalileoE5aTrackingTest, ValidationOfResults)