mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-03-14 07:28:17 +00:00
Merge with next
This commit is contained in:
commit
77b635e8c0
@ -44,13 +44,32 @@ option(ENABLE_GPERFTOOLS "Enable linking to Gperftools libraries (tcmalloc and p
|
||||
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_API_COMPAT 0)
|
||||
set(VERSION_INFO_MINOR_VERSION 4)
|
||||
set(VERSION_INFO_API_COMPAT 0)
|
||||
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})
|
||||
|
||||
|
||||
|
||||
########################################################################
|
||||
# Environment setup
|
||||
########################################################################
|
||||
@ -73,7 +92,6 @@ if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
|
||||
else(ARCH_64BITS)
|
||||
set(ARCH_ "(32 bits)")
|
||||
endif(ARCH_64BITS)
|
||||
|
||||
if(EXISTS "/etc/lsb-release")
|
||||
execute_process(COMMAND cat /etc/lsb-release
|
||||
COMMAND grep DISTRIB_ID
|
||||
|
@ -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;
|
||||
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)
|
||||
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
|
||||
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
|
||||
arma::mat F = arma::zeros(3,3);
|
||||
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,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(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,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,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,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,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,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
|
||||
F(2,2) = sin((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));
|
||||
|
||||
// 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);
|
||||
@ -384,7 +384,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
|
||||
|
||||
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_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
|
||||
@ -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_height_m = d_avg_height_m + d_hist_height_m.at(i);
|
||||
}
|
||||
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
|
||||
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
|
||||
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
|
||||
d_avg_latitude_d = d_avg_latitude_d / static_cast<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 / static_cast<double>(d_averaging_depth);
|
||||
b_valid_position = true;
|
||||
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};
|
||||
|
||||
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 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 oldh = 0;
|
||||
@ -526,8 +526,8 @@ void galileo_e1_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_sele
|
||||
{
|
||||
oldh = h;
|
||||
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
|
||||
phi = atan(Z / ((sqrt(X*X + Y*Y) * (1 - (2 - f[elipsoid_selection]) * f[elipsoid_selection] *N / (N + h) ))));
|
||||
h = sqrt(X*X + Y*Y) / cos(phi) - N;
|
||||
phi = atan(Z / ((sqrt(X * X + Y * Y) * (1 - (2 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) ))));
|
||||
h = sqrt(X * X + Y * Y) / cos(phi) - N;
|
||||
iterations = iterations + 1;
|
||||
if (iterations > 100)
|
||||
{
|
||||
@ -582,7 +582,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
|
||||
}
|
||||
|
||||
// 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
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
if (r > 1.0E-20)
|
||||
@ -621,7 +621,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
|
||||
return;
|
||||
}
|
||||
|
||||
*h = r - a*(1 - sinphi*sinphi/finv);
|
||||
*h = r - a * (1 - sinphi * sinphi/finv);
|
||||
|
||||
// iterate
|
||||
double cosphi;
|
||||
@ -636,18 +636,18 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
|
||||
cosphi = cos(*dphi);
|
||||
|
||||
// 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
|
||||
dP = P - (N_phi + (*h)) * cosphi;
|
||||
dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
|
||||
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
|
||||
|
||||
// update height and latitude
|
||||
*h = *h + (sinphi*dZ + cosphi*dP);
|
||||
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
|
||||
*h = *h + (sinphi * dZ + cosphi * dP);
|
||||
*dphi = *dphi + (cosphi * dZ - sinphi * dP)/(N_phi + (*h));
|
||||
|
||||
// test for convergence
|
||||
if ((dP*dP + dZ*dZ) < tolsq)
|
||||
if ((dP * dP + dZ * dZ) < tolsq)
|
||||
{
|
||||
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);
|
||||
|
||||
F(0,0) = -sl;
|
||||
F(0,1) = -sb*cl;
|
||||
F(0,2) = cb*cl;
|
||||
F(0,1) = -sb * cl;
|
||||
F(0,2) = cb * cl;
|
||||
|
||||
F(1,0) = cl;
|
||||
F(1,1) = -sb*sl;
|
||||
F(1,2) = cb*sl;
|
||||
F(1,1) = -sb * sl;
|
||||
F(1,2) = cb * sl;
|
||||
|
||||
F(2,0) = 0;
|
||||
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 hor_dis;
|
||||
hor_dis = sqrt(E*E + N*N);
|
||||
hor_dis = sqrt(E * E + N * N);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
*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)
|
||||
@ -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);
|
||||
|
||||
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 tksea = t_kel - tlapse * htkel_km;
|
||||
double tkelh = tksea + tlapse * hhum_km;
|
||||
|
@ -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
|
||||
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
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
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_height_m = d_avg_height_m + d_hist_height_m.at(i);
|
||||
}
|
||||
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
|
||||
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
|
||||
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
|
||||
d_avg_latitude_d = d_avg_latitude_d / static_cast<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 / static_cast<double>(d_averaging_depth);
|
||||
b_valid_position = true;
|
||||
return true; //indicates that the returned position is valid
|
||||
}
|
||||
|
@ -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;
|
||||
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)
|
||||
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_height_m = d_avg_height_m + d_hist_height_m.at(i);
|
||||
}
|
||||
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
|
||||
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
|
||||
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
|
||||
d_avg_latitude_d = d_avg_latitude_d / static_cast<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 / static_cast<double>(d_averaging_depth);
|
||||
b_valid_position = true;
|
||||
return true; //indicates that the returned position is valid
|
||||
}
|
||||
|
@ -222,8 +222,8 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
|
||||
north = true;
|
||||
}
|
||||
|
||||
int deg = (int)lat;
|
||||
double mins = lat - (double)deg;
|
||||
int deg = static_cast<int>(lat);
|
||||
double mins = lat - static_cast<double>(deg);
|
||||
mins *= 60.0 ;
|
||||
std::ostringstream out_string;
|
||||
out_string.setf(std::ios::fixed, std::ios::floatfield);
|
||||
@ -259,8 +259,8 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
|
||||
{
|
||||
east = true;
|
||||
}
|
||||
int deg = (int)longitude;
|
||||
double mins = longitude - (double)deg;
|
||||
int deg = static_cast<int>(longitude);
|
||||
double mins = longitude - static_cast<double>(deg);
|
||||
mins *= 60.0 ;
|
||||
std::ostringstream out_string;
|
||||
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 << mins;
|
||||
|
||||
if (east==true)
|
||||
if (east == true)
|
||||
{
|
||||
out_string << ",E";
|
||||
}
|
||||
@ -361,7 +361,7 @@ std::string Nmea_Printer::get_GPRMC()
|
||||
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)
|
||||
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.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << std::hex << (int)checksum;
|
||||
sentence_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str << "\r\n";
|
||||
@ -507,7 +507,7 @@ std::string Nmea_Printer::get_GPGSA()
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << std::hex << (int)checksum;
|
||||
sentence_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
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)
|
||||
// Each frame contains up to 4 satellites
|
||||
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
|
||||
int current_satellite = 0;
|
||||
@ -566,17 +566,17 @@ std::string Nmea_Printer::get_GPGSV()
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
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.width(3);
|
||||
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.width(2);
|
||||
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++;
|
||||
|
||||
@ -592,7 +592,7 @@ std::string Nmea_Printer::get_GPGSV()
|
||||
frame_str << "*";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str << std::hex << (int)checksum;
|
||||
frame_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
frame_str << "\r\n";
|
||||
@ -712,7 +712,7 @@ std::string Nmea_Printer::get_GPGGA()
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << std::hex <<(int)checksum;
|
||||
sentence_str << std::hex << static_cast<int>(checksum);
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str << "\r\n";
|
||||
|
@ -143,7 +143,7 @@ Rinex_Printer::Rinex_Printer()
|
||||
if ( FLAGS_RINEX_version.compare("3.01") == 0 )
|
||||
{
|
||||
version = 3;
|
||||
stringVersion = "3.02";
|
||||
stringVersion = "3.01";
|
||||
}
|
||||
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 )
|
||||
{
|
||||
version = 2;
|
||||
stringVersion = "2.11";
|
||||
stringVersion = "2.10";
|
||||
}
|
||||
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)
|
||||
{
|
||||
@ -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;
|
||||
stringVersion = "3.02";
|
||||
@ -501,7 +501,7 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Galileo_Iono iono, Gal
|
||||
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;
|
||||
@ -680,7 +680,7 @@ void Rinex_Printer::rinex_nav_header(std::ofstream& out, Gps_Iono iono, Gps_Utc_
|
||||
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;
|
||||
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, std::map<int,Gps_Ephemeris> eph_map)
|
||||
void Rinex_Printer::log_rinex_nav(std::ofstream& out, const std::map<int,Gps_Ephemeris>& eph_map)
|
||||
{
|
||||
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();
|
||||
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 += 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, ' ');
|
||||
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 += 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)
|
||||
{
|
||||
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 += 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 += 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 += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_TGD, 18, 2);
|
||||
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, ' ');
|
||||
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 )
|
||||
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.satelliteBlock[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[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) )
|
||||
if ((gps_ephemeris_iter->second.satelliteBlock.at(gps_ephemeris_iter->second.i_satellite_PRN).compare("IIR") == 0) ||
|
||||
(gps_ephemeris_iter->second.satelliteBlock.at(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("IIF") == 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 )
|
||||
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, std::map<int, Galileo_Ephemeris> eph_map)
|
||||
void Rinex_Printer::log_rinex_nav(std::ofstream& out, const std::map<int, Galileo_Ephemeris>& eph_map)
|
||||
{
|
||||
std::string line;
|
||||
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
|
||||
std::map<int,Galileo_Ephemeris>::const_iterator galileo_ephemeris_iter;
|
||||
line.clear();
|
||||
for(galileo_ephemeris_iter = eph_map.begin();
|
||||
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);
|
||||
line += Rinex_Printer::doub2for(static_cast<double>(data_source_INAV), 18, 2);
|
||||
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 Galileo_week_continuous_number = GST_week + 1024.0 + num_GST_rollovers * 4096.0;
|
||||
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;
|
||||
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;
|
||||
@ -1639,7 +1636,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, doub
|
||||
// -------- TIME OF FIRST OBS
|
||||
line.clear();
|
||||
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 month (timestring, 4, 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;
|
||||
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;
|
||||
version = 3;
|
||||
@ -2101,7 +2098,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris gps_eph,
|
||||
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.
|
||||
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');
|
||||
//Number of satellites observed in current epoch
|
||||
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();
|
||||
pseudoranges_iter != pseudoranges.end();
|
||||
pseudoranges_iter++)
|
||||
@ -2167,8 +2164,8 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
|
||||
pseudoranges_iter++)
|
||||
{
|
||||
line += satelliteSystem["GPS"];
|
||||
if ((int)pseudoranges_iter->first < 10) line += std::string(1, '0');
|
||||
line += boost::lexical_cast<std::string>((int)pseudoranges_iter->first);
|
||||
if (static_cast<int>(pseudoranges_iter->first) < 10) line += std::string(1, '0');
|
||||
line += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
|
||||
}
|
||||
// Receiver clock offset (optional)
|
||||
//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
|
||||
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();
|
||||
pseudoranges_iter != pseudoranges.end();
|
||||
pseudoranges_iter++)
|
||||
@ -2263,8 +2260,8 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double
|
||||
std::string lineObs;
|
||||
lineObs.clear();
|
||||
lineObs += satelliteSystem["GPS"];
|
||||
if ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0');
|
||||
lineObs += boost::lexical_cast<std::string>((int)pseudoranges_iter->first);
|
||||
if (static_cast<int>(pseudoranges_iter->first) < 10) lineObs += std::string(1, '0');
|
||||
lineObs += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
|
||||
//lineObs += std::string(2, ' ');
|
||||
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.
|
||||
// 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
|
||||
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();
|
||||
pseudoranges_iter != pseudoranges.end();
|
||||
pseudoranges_iter++)
|
||||
@ -2364,8 +2361,8 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, dou
|
||||
std::string lineObs;
|
||||
lineObs.clear();
|
||||
lineObs += satelliteSystem["Galileo"];
|
||||
if ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0');
|
||||
lineObs += boost::lexical_cast<std::string>((int)pseudoranges_iter->first);
|
||||
if (static_cast<int>(pseudoranges_iter->first) < 10) lineObs += std::string(1, '0');
|
||||
lineObs += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
|
||||
//lineObs += std::string(2, ' ');
|
||||
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;
|
||||
|
||||
@ -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
|
||||
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();
|
||||
pseudoranges_iter != pseudoranges.end();
|
||||
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);
|
||||
if(s.compare("G") == 0) lineObs += satelliteSystem["GPS"];
|
||||
if(s.compare("E") == 0) lineObs += satelliteSystem["Galileo"];
|
||||
if ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0');
|
||||
lineObs += boost::lexical_cast<std::string>((int)pseudoranges_iter->first);
|
||||
if (static_cast<int>(pseudoranges_iter->first) < 10) lineObs += std::string(1, '0');
|
||||
lineObs += boost::lexical_cast<std::string>(static_cast<int>(pseudoranges_iter->first));
|
||||
lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14);
|
||||
|
||||
//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
|
||||
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;
|
||||
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
|
||||
//: 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);
|
||||
boost::posix_time::time_duration t = boost::posix_time::millisec((utc_t + 604800*(double)(nav_msg.i_GPS_week))*1000);
|
||||
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 * static_cast<double>(nav_msg.i_GPS_week)) * 1000);
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
|
||||
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.
|
||||
// (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)
|
||||
// --??? No time correction here, since it will be done in the RINEX processor
|
||||
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);
|
||||
const double gps_t = obs_time;
|
||||
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);
|
||||
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.
|
||||
// (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
|
||||
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);
|
||||
return p_time;
|
||||
}
|
||||
|
@ -93,32 +93,32 @@ public:
|
||||
/*!
|
||||
* \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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
@ -128,47 +128,47 @@ public:
|
||||
/*!
|
||||
* \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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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
|
||||
*/
|
||||
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.
|
||||
@ -178,12 +178,12 @@ public:
|
||||
/*!
|
||||
* \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> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH
|
||||
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:
|
||||
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
|
||||
*/
|
||||
int signalStrength(double snr);
|
||||
int signalStrength(const double snr);
|
||||
|
||||
/* Creates RINEX file names according to the naming convention
|
||||
*
|
||||
@ -225,7 +225,7 @@ private:
|
||||
/*
|
||||
* 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.
|
||||
|
@ -277,7 +277,7 @@ std::string Rtcm_Printer::print_M1005_test ()
|
||||
{
|
||||
std::bitset<152> m1005 = get_M1005_test();
|
||||
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);
|
||||
unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits;
|
||||
std::string b(zeros_to_fill, '0');
|
||||
@ -313,11 +313,11 @@ void Rtcm_Printer::print_M1001 ()
|
||||
{
|
||||
std::bitset<122> m1001 = get_M1001();
|
||||
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);
|
||||
unsigned int zeros_to_fill = 8*msg_length_bytes - msg_length_bits;
|
||||
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_without_crc = preamble.to_string() +
|
||||
reserved_field.to_string() +
|
||||
|
@ -268,7 +268,7 @@ float GalileoE1Pcps8msAmbiguousAcquisition::calculate_threshold(float pfa)
|
||||
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
|
||||
|
||||
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 lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
|
@ -277,9 +277,9 @@ float GalileoE1PcpsAmbiguousAcquisition::calculate_threshold(float pfa)
|
||||
|
||||
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
|
||||
|
||||
unsigned int ncells = vector_length_*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
double val = pow(1.0-pfa,exponent);
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
double val = pow(1.0 - pfa,exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
float threshold = (float)quantile(mydist,val);
|
||||
|
@ -64,7 +64,7 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition
|
||||
|
||||
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 "
|
||||
<< "Galileo code length (4 ms). coherent_integration_time = "
|
||||
<< sampled_ms_ << " ms will be used.";
|
||||
@ -82,7 +82,7 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition
|
||||
/ (Galileo_E1_CODE_CHIP_RATE_HZ
|
||||
/ 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;
|
||||
|
||||
|
@ -305,7 +305,7 @@ float GalileoE1PcpsQuickSyncAmbiguousAcquisition::calculate_threshold(float pfa)
|
||||
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
|
||||
|
||||
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 lambda = double(code_length_/folding_factor_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
|
@ -268,10 +268,10 @@ float GalileoE1PcpsTongAmbiguousAcquisition::calculate_threshold(float pfa)
|
||||
frequency_bins++;
|
||||
}
|
||||
|
||||
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
|
||||
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
|
||||
|
||||
unsigned int ncells = vector_length_*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
double val = pow(1.0-pfa,exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
|
@ -293,9 +293,9 @@ float GalileoE5aNoncoherentIQAcquisitionCaf::calculate_threshold(float pfa)
|
||||
{
|
||||
frequency_bins++;
|
||||
}
|
||||
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa;
|
||||
unsigned int ncells = vector_length_*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
double val = pow(1.0 - pfa, exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
|
@ -254,8 +254,8 @@ float GpsL1CaPcpsAcquisition::calculate_threshold(float pfa)
|
||||
frequency_bins++;
|
||||
}
|
||||
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa;
|
||||
unsigned int ncells = vector_length_*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
double val = pow(1.0 - pfa, exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
|
@ -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()
|
||||
{
|
||||
|
@ -250,11 +250,11 @@ float GpsL1CaPcpsMultithreadAcquisition::calculate_threshold(float pfa)
|
||||
frequency_bins++;
|
||||
}
|
||||
|
||||
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
|
||||
DLOG(INFO) << "Channel "<< channel_ << " Pfa = " << pfa;
|
||||
|
||||
unsigned int ncells = vector_length_*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
double val = pow(1.0-pfa,exponent);
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
double val = pow(1.0 - pfa, exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
float threshold = (float)quantile(mydist,val);
|
||||
|
@ -249,11 +249,11 @@ float GpsL1CaPcpsOpenClAcquisition::calculate_threshold(float pfa)
|
||||
frequency_bins++;
|
||||
}
|
||||
|
||||
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
|
||||
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
|
||||
|
||||
unsigned int ncells = vector_length_*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
double val = pow(1.0-pfa,exponent);
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
double val = pow(1.0 - pfa, exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
float threshold = (float)quantile(mydist,val);
|
||||
|
@ -286,10 +286,10 @@ float GpsL1CaPcpsQuickSyncAcquisition::calculate_threshold(float pfa)
|
||||
frequency_bins++;
|
||||
}
|
||||
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa;
|
||||
unsigned int ncells = (code_length_/folding_factor_)*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
unsigned int ncells = (code_length_ / folding_factor_) * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
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);
|
||||
float threshold = (float)quantile(mydist,val);
|
||||
|
||||
|
@ -239,14 +239,14 @@ float GpsL1CaPcpsTongAcquisition::calculate_threshold(float pfa)
|
||||
frequency_bins++;
|
||||
}
|
||||
|
||||
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
|
||||
DLOG(INFO) << "Channel "<< channel_ <<" Pfa = "<< pfa;
|
||||
|
||||
unsigned int ncells = vector_length_*frequency_bins;
|
||||
double exponent = 1/(double)ncells;
|
||||
double val = pow(1.0-pfa,exponent);
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1 / static_cast<double>(ncells);
|
||||
double val = pow(1.0 - pfa,exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
float threshold = (float)quantile(mydist,val);
|
||||
float threshold = (float)quantile(mydist, val);
|
||||
|
||||
return threshold;
|
||||
}
|
||||
|
@ -107,35 +107,27 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
|
||||
d_both_signal_components = both_signal_components_;
|
||||
d_CAF_window_hz = CAF_window_hz_;
|
||||
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_inbuffer, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_fft_code_I_A, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_magnitudeIA, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
d_inbuffer = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_fft_code_I_A = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitudeIA = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
if (d_both_signal_components == true)
|
||||
{
|
||||
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){};
|
||||
d_fft_code_Q_A = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitudeQA = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
}
|
||||
// IF COHERENT INTEGRATION TIME > 1
|
||||
if (d_sampled_ms > 1)
|
||||
{
|
||||
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){};
|
||||
d_fft_code_I_B = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitudeIB = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
if (d_both_signal_components == true)
|
||||
{
|
||||
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){};
|
||||
d_fft_code_Q_B = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
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
|
||||
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++)
|
||||
{
|
||||
free(d_grid_doppler_wipeoffs[i]);
|
||||
volk_free(d_grid_doppler_wipeoffs[i]);
|
||||
}
|
||||
delete[] d_grid_doppler_wipeoffs;
|
||||
}
|
||||
|
||||
free(d_fft_code_I_A);
|
||||
free(d_magnitudeIA);
|
||||
volk_free(d_inbuffer);
|
||||
volk_free(d_fft_code_I_A);
|
||||
volk_free(d_magnitudeIA);
|
||||
if (d_both_signal_components == true)
|
||||
{
|
||||
free(d_fft_code_Q_A);
|
||||
free(d_magnitudeQA);
|
||||
volk_free(d_fft_code_Q_A);
|
||||
volk_free(d_magnitudeQA);
|
||||
}
|
||||
// IF INTEGRATION TIME > 1
|
||||
if (d_sampled_ms > 1)
|
||||
{
|
||||
free(d_fft_code_I_B);
|
||||
free(d_magnitudeIB);
|
||||
volk_free(d_fft_code_I_B);
|
||||
volk_free(d_magnitudeIB);
|
||||
if (d_both_signal_components == true)
|
||||
{
|
||||
free(d_fft_code_Q_B);
|
||||
free(d_magnitudeQB);
|
||||
volk_free(d_fft_code_Q_B);
|
||||
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_ifft;
|
||||
|
||||
|
||||
if (d_dump)
|
||||
{
|
||||
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
|
||||
|
||||
//Conjugate the local code
|
||||
if (is_unaligned())
|
||||
{
|
||||
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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(d_fft_code_I_A,d_fft_if->get_outbuf(),d_fft_size);
|
||||
|
||||
// SAME FOR PILOT SIGNAL
|
||||
if (d_both_signal_components == true)
|
||||
{
|
||||
// Three replicas of pilot primary code. CODE A: (1,1,1)
|
||||
memcpy(d_fft_if->get_inbuf(), codeQ, sizeof(gr_complex)*d_fft_size);
|
||||
{
|
||||
// Three replicas of pilot primary code. CODE A: (1,1,1)
|
||||
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
|
||||
if (is_unaligned())
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
//Conjugate the local code
|
||||
volk_32fc_conjugate_32fc(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
|
||||
// Note: max integration time allowed = 3ms (dealt in adapter)
|
||||
if (d_sampled_ms > 1)
|
||||
{
|
||||
// DATA CODE B: First replica is inverted (0,1,1)
|
||||
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[0],
|
||||
&codeI[0], gr_complex(-1,0),
|
||||
d_samples_per_code);
|
||||
d_fft_if->execute(); // We need the FFT of local code
|
||||
{
|
||||
// DATA CODE B: First replica is inverted (0,1,1)
|
||||
volk_32fc_s32fc_multiply_32fc(&(d_fft_if->get_inbuf())[0],
|
||||
&codeI[0], gr_complex(-1,0),
|
||||
d_samples_per_code);
|
||||
|
||||
//Conjugate the 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
|
||||
d_fft_if->execute(); // We need the FFT of local code
|
||||
|
||||
//Conjugate the local code
|
||||
if (is_unaligned())
|
||||
{
|
||||
volk_32fc_conjugate_32fc_u(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size);
|
||||
}
|
||||
else
|
||||
{
|
||||
volk_32fc_conjugate_32fc_a(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size);
|
||||
}
|
||||
}
|
||||
}
|
||||
//Conjugate the local code
|
||||
volk_32fc_conjugate_32fc(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(&(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
|
||||
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()
|
||||
@ -273,8 +249,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
|
||||
|
||||
// Count the number of bins
|
||||
d_num_doppler_bins = 0;
|
||||
for (int doppler = (int)(-d_doppler_max);
|
||||
doppler <= (int)d_doppler_max;
|
||||
for (int doppler = static_cast<int>(-d_doppler_max);
|
||||
doppler <= static_cast<int>(d_doppler_max);
|
||||
doppler += d_doppler_step)
|
||||
{
|
||||
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];
|
||||
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_fft_size * sizeof(gr_complex)) == 0){};
|
||||
|
||||
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
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);
|
||||
}
|
||||
@ -297,11 +271,11 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
|
||||
// if (d_CAF_filter)
|
||||
if (d_CAF_window_hz > 0)
|
||||
{
|
||||
if (posix_memalign((void**)&d_CAF_vector, 16, d_num_doppler_bins * sizeof(float)) == 0){};
|
||||
if (posix_memalign((void**)&d_CAF_vector_I, 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()));
|
||||
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 (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
|
||||
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];
|
||||
}
|
||||
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 (d_buffer_count >= d_fft_size-d_gr_stream_buffer)
|
||||
if (d_buffer_count >= d_fft_size - d_gr_stream_buffer)
|
||||
{
|
||||
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_QA = 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_mag = 0.0;
|
||||
d_well_count++;
|
||||
@ -408,18 +382,18 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitudeIA, d_inbuffer, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitudeIA, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_inbuffer, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitudeIA, d_fft_size);
|
||||
d_input_power /= static_cast<float>(d_fft_size);
|
||||
|
||||
// 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=-(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);
|
||||
|
||||
// 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
|
||||
// Multiply carrier wiped--off, Fourier transformed incoming signal
|
||||
// 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);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
|
||||
// Search maximum
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitudeIA, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_IA, d_magnitudeIA, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitudeIA, d_ifft->get_outbuf(), 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
|
||||
magt_IA = d_magnitudeIA[indext_IA] / (fft_normalization_factor * fft_normalization_factor);
|
||||
|
||||
if (d_both_signal_components == true)
|
||||
{
|
||||
// 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_ifft->execute();
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitudeQA, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_QA, d_magnitudeQA, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitudeQA, d_ifft->get_outbuf(), 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);
|
||||
}
|
||||
if (d_sampled_ms > 1) // If Integration time > 1 code
|
||||
{
|
||||
// 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_ifft->execute();
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitudeIB, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_IB, d_magnitudeIB, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitudeIB, d_ifft->get_outbuf(), 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);
|
||||
|
||||
if (d_both_signal_components == true)
|
||||
{
|
||||
// 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_ifft->execute();
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitudeQB, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_QB, d_magnitudeQB, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitudeQB, d_ifft->get_outbuf(), 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);
|
||||
}
|
||||
}
|
||||
@ -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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
@ -552,7 +526,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
||||
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);
|
||||
}
|
||||
|
||||
@ -569,8 +543,8 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
||||
// restarted between consecutive dwells in multidwell operation.
|
||||
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_doppler_hz = (double)doppler;
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
|
||||
|
||||
// 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.
|
||||
if (d_CAF_window_hz > 0)
|
||||
{
|
||||
int CAF_bins_half;
|
||||
float* accum;
|
||||
// double* accum;
|
||||
if (posix_memalign((void**)&accum, 16, sizeof(float)) == 0){};
|
||||
CAF_bins_half = d_CAF_window_hz/(2*d_doppler_step);
|
||||
float weighting_factor;
|
||||
weighting_factor = 0.5/(float)CAF_bins_half;
|
||||
// weighting_factor = 0;
|
||||
// std::cout << "weighting_factor " << weighting_factor << std::endl;
|
||||
// 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);
|
||||
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*(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]
|
||||
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);
|
||||
for (int i = 0; i < CAF_bins_half+doppler_index+1; 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]
|
||||
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++)
|
||||
{
|
||||
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);
|
||||
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*(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;
|
||||
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);
|
||||
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*(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;
|
||||
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++)
|
||||
{
|
||||
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));
|
||||
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] /= 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;
|
||||
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));
|
||||
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] /= 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;
|
||||
d_CAF_vector[doppler_index] += accum[0];
|
||||
}
|
||||
}
|
||||
int CAF_bins_half;
|
||||
float* accum = static_cast<float*>(volk_malloc(sizeof(float), volk_get_alignment()));
|
||||
CAF_bins_half = d_CAF_window_hz / (2 * d_doppler_step);
|
||||
float weighting_factor;
|
||||
weighting_factor = 0.5 / static_cast<float>(CAF_bins_half);
|
||||
// weighting_factor = 0;
|
||||
// std::cout << "weighting_factor " << weighting_factor << std::endl;
|
||||
// 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);
|
||||
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] /= 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]
|
||||
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);
|
||||
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] /= 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]
|
||||
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++)
|
||||
{
|
||||
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);
|
||||
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] /= 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;
|
||||
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);
|
||||
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] /= 2*CAF_bins_half+1;
|
||||
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++)
|
||||
{
|
||||
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));
|
||||
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] /= 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;
|
||||
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));
|
||||
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] /= 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;
|
||||
d_CAF_vector[doppler_index] += accum[0];
|
||||
}
|
||||
}
|
||||
|
||||
// Recompute the maximum doppler peak
|
||||
volk_32f_index_max_16u_a(&indext, d_CAF_vector, d_num_doppler_bins);
|
||||
doppler=-(int)d_doppler_max+d_doppler_step*indext;
|
||||
d_gnss_synchro->Acq_doppler_hz = (double)doppler;
|
||||
// Dump if required, appended at the end of the file
|
||||
if (d_dump)
|
||||
{
|
||||
std::stringstream filename;
|
||||
std::streamsize n = sizeof(float) * (d_num_doppler_bins); // noncomplex file write
|
||||
filename.str("");
|
||||
filename << "../data/test_statistics_E5a_sat_"
|
||||
<< d_gnss_synchro->PRN << "_CAF.dat";
|
||||
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.close();
|
||||
}
|
||||
// Recompute the maximum doppler peak
|
||||
volk_32f_index_max_16u(&indext, d_CAF_vector, d_num_doppler_bins);
|
||||
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * indext;
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
// Dump if required, appended at the end of the file
|
||||
if (d_dump)
|
||||
{
|
||||
std::stringstream filename;
|
||||
std::streamsize n = sizeof(float) * (d_num_doppler_bins); // noncomplex file write
|
||||
filename.str("");
|
||||
filename << "../data/test_statistics_E5a_sat_"
|
||||
<< d_gnss_synchro->PRN << "_CAF.dat";
|
||||
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.close();
|
||||
}
|
||||
volk_free(accum);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
consume_each(d_fft_size-d_buffer_count);
|
||||
consume_each(d_fft_size - d_buffer_count);
|
||||
d_buffer_count = 0;
|
||||
break;
|
||||
}
|
||||
@ -745,7 +718,6 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
|
||||
d_active = false;
|
||||
d_state = 0;
|
||||
|
||||
|
||||
acquisition_message = 1;
|
||||
d_channel_internal_queue->push(acquisition_message);
|
||||
d_sample_counter += ninput_items[0]; // sample counter
|
||||
|
@ -79,10 +79,9 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
|
||||
d_input_power = 0.0;
|
||||
d_num_doppler_bins = 0;
|
||||
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_fft_code_A, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_fft_code_B, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
d_fft_code_A = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_fft_code_B = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
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++)
|
||||
{
|
||||
free(d_grid_doppler_wipeoffs[i]);
|
||||
volk_free(d_grid_doppler_wipeoffs[i]);
|
||||
}
|
||||
delete[] d_grid_doppler_wipeoffs;
|
||||
}
|
||||
|
||||
free(d_fft_code_A);
|
||||
free(d_fft_code_B);
|
||||
free(d_magnitude);
|
||||
volk_free(d_fft_code_A);
|
||||
volk_free(d_fft_code_B);
|
||||
volk_free(d_magnitude);
|
||||
|
||||
delete d_ifft;
|
||||
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)
|
||||
{
|
||||
// code A: two replicas of a primary code
|
||||
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size);
|
||||
// code A: two replicas of a primary code
|
||||
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex)*d_fft_size);
|
||||
|
||||
d_fft_if->execute(); // We need the FFT of local code
|
||||
|
||||
//Conjugate the local code
|
||||
if (is_unaligned())
|
||||
{
|
||||
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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(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.
|
||||
volk_32fc_s32fc_multiply_32fc_a(&(d_fft_if->get_inbuf())[d_samples_per_code],
|
||||
&code[d_samples_per_code], gr_complex(-1,0),
|
||||
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),
|
||||
d_samples_per_code);
|
||||
|
||||
d_fft_if->execute(); // We need the FFT of local code
|
||||
|
||||
//Conjugate the local code
|
||||
if (is_unaligned())
|
||||
{
|
||||
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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(d_fft_code_B, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
|
||||
void galileo_pcps_8ms_acquisition_cc::init()
|
||||
@ -164,8 +149,8 @@ void galileo_pcps_8ms_acquisition_cc::init()
|
||||
|
||||
// Count the number of bins
|
||||
d_num_doppler_bins = 0;
|
||||
for (int doppler = (int)(-d_doppler_max);
|
||||
doppler <= (int)d_doppler_max;
|
||||
for (int doppler = static_cast<int>(-d_doppler_max);
|
||||
doppler <= static_cast<int>(d_doppler_max);
|
||||
doppler += d_doppler_step)
|
||||
{
|
||||
d_num_doppler_bins++;
|
||||
@ -173,14 +158,11 @@ void galileo_pcps_8ms_acquisition_cc::init()
|
||||
|
||||
// Create the carrier Doppler wipeoff signals
|
||||
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_fft_size * sizeof(gr_complex)) == 0){};
|
||||
|
||||
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);
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -226,7 +208,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
||||
float magt_A = 0.0;
|
||||
float magt_B = 0.0;
|
||||
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_mag = 0.0;
|
||||
|
||||
@ -241,18 +223,18 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= static_cast<float>(d_fft_size);
|
||||
|
||||
// 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=-(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);
|
||||
|
||||
// 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
|
||||
// with the local FFT'd code A 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_A, d_fft_size);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
|
||||
// Search maximum
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_A, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), 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
|
||||
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
|
||||
// with the local FFT'd code B 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_B, d_fft_size);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
|
||||
// Search maximum
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_B, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), 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
|
||||
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)
|
||||
{
|
||||
d_mag = magt;
|
||||
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = (double)doppler;
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
|
||||
}
|
||||
|
||||
|
@ -86,9 +86,8 @@ pcps_acquisition_cc::pcps_acquisition_cc(
|
||||
d_num_doppler_bins = 0;
|
||||
d_bit_transition_flag = bit_transition_flag;
|
||||
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
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++)
|
||||
{
|
||||
free(d_grid_doppler_wipeoffs[i]);
|
||||
volk_free(d_grid_doppler_wipeoffs[i]);
|
||||
}
|
||||
delete[] d_grid_doppler_wipeoffs;
|
||||
}
|
||||
|
||||
free(d_fft_codes);
|
||||
free(d_magnitude);
|
||||
volk_free(d_fft_codes);
|
||||
volk_free(d_magnitude);
|
||||
|
||||
delete d_ifft;
|
||||
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)
|
||||
{
|
||||
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
|
||||
|
||||
//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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
|
||||
void pcps_acquisition_cc::init()
|
||||
@ -151,8 +140,8 @@ void pcps_acquisition_cc::init()
|
||||
|
||||
// Count the number of bins
|
||||
d_num_doppler_bins = 0;
|
||||
for (int doppler = (int)(-d_doppler_max);
|
||||
doppler <= (int)d_doppler_max;
|
||||
for (int doppler = static_cast<int>(-d_doppler_max);
|
||||
doppler <= static_cast<int>(d_doppler_max);
|
||||
doppler += d_doppler_step)
|
||||
{
|
||||
d_num_doppler_bins++;
|
||||
@ -160,14 +149,12 @@ void pcps_acquisition_cc::init()
|
||||
|
||||
// Create the carrier Doppler wipeoff signals
|
||||
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++)
|
||||
{
|
||||
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16,
|
||||
d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
|
||||
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);
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -219,7 +206,7 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
unsigned int indext = 0;
|
||||
float magt = 0.0;
|
||||
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_mag = 0.0;
|
||||
|
||||
@ -234,18 +221,18 @@ int pcps_acquisition_cc::general_work(int noutput_items,
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= static_cast<float>(d_fft_size);
|
||||
|
||||
// 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=-(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);
|
||||
|
||||
// 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
|
||||
// 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);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
|
||||
// Search maximum
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), 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
|
||||
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.
|
||||
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_doppler_hz = (double)doppler;
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
|
||||
|
||||
// 5- Compute the test statistics and compare to the threshold
|
||||
|
@ -46,96 +46,94 @@
|
||||
using google::LogMessage;
|
||||
|
||||
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,
|
||||
long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump,
|
||||
std::string dump_filename)
|
||||
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,
|
||||
std::string dump_filename)
|
||||
{
|
||||
|
||||
return pcps_acquisition_fine_doppler_cc_sptr(
|
||||
new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq,
|
||||
fs_in, samples_per_ms, queue, dump, dump_filename));
|
||||
return pcps_acquisition_fine_doppler_cc_sptr(
|
||||
new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min, freq,
|
||||
fs_in, samples_per_ms, queue, dump, dump_filename));
|
||||
}
|
||||
|
||||
|
||||
|
||||
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,
|
||||
long fs_in, int samples_per_ms, boost::shared_ptr<gr::msg_queue> queue, bool dump,
|
||||
std::string dump_filename) :
|
||||
gr::block("pcps_acquisition_fine_doppler_cc",
|
||||
gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(0, 0, sizeof(gr_complex)))
|
||||
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,
|
||||
std::string dump_filename) :
|
||||
gr::block("pcps_acquisition_fine_doppler_cc",
|
||||
gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(0, 0, sizeof(gr_complex)))
|
||||
{
|
||||
d_sample_counter = 0; // SAMPLE COUNTER
|
||||
d_active = false;
|
||||
d_queue = queue;
|
||||
d_freq = freq;
|
||||
d_fs_in = fs_in;
|
||||
d_samples_per_ms = samples_per_ms;
|
||||
d_sampled_ms = sampled_ms;
|
||||
d_config_doppler_max = doppler_max;
|
||||
d_config_doppler_min=doppler_min;
|
||||
d_fft_size = d_sampled_ms * d_samples_per_ms;
|
||||
// HS Acquisition
|
||||
d_max_dwells= max_dwells;
|
||||
d_gnuradio_forecast_samples=d_fft_size;
|
||||
d_input_power = 0.0;
|
||||
d_state=0;
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_carrier, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
d_sample_counter = 0; // SAMPLE COUNTER
|
||||
d_active = false;
|
||||
d_queue = queue;
|
||||
d_freq = freq;
|
||||
d_fs_in = fs_in;
|
||||
d_samples_per_ms = samples_per_ms;
|
||||
d_sampled_ms = sampled_ms;
|
||||
d_config_doppler_max = doppler_max;
|
||||
d_config_doppler_min = doppler_min;
|
||||
d_fft_size = d_sampled_ms * d_samples_per_ms;
|
||||
// HS Acquisition
|
||||
d_max_dwells = max_dwells;
|
||||
d_gnuradio_forecast_samples = d_fft_size;
|
||||
d_input_power = 0.0;
|
||||
d_state = 0;
|
||||
d_carrier = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
|
||||
// Direct FFT
|
||||
d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
|
||||
|
||||
// Inverse FFT
|
||||
d_ifft = new gr::fft::fft_complex(d_fft_size, false);
|
||||
// Inverse FFT
|
||||
d_ifft = new gr::fft::fft_complex(d_fft_size, false);
|
||||
|
||||
// For dumping samples into a file
|
||||
d_dump = dump;
|
||||
d_dump_filename = dump_filename;
|
||||
// For dumping samples into a file
|
||||
d_dump = dump;
|
||||
d_dump_filename = dump_filename;
|
||||
}
|
||||
|
||||
void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step)
|
||||
{
|
||||
d_doppler_step = doppler_step;
|
||||
// Create the search grid array
|
||||
d_doppler_step = doppler_step;
|
||||
// Create the search grid array
|
||||
|
||||
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_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++)
|
||||
{
|
||||
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()
|
||||
{
|
||||
for (int i=0;i<d_num_doppler_points;i++)
|
||||
{
|
||||
delete[] d_grid_data[i];
|
||||
delete[] d_grid_doppler_wipeoffs[i];
|
||||
}
|
||||
delete d_grid_data;
|
||||
delete d_grid_doppler_wipeoffs;
|
||||
for (int i = 0; i < d_num_doppler_points; i++)
|
||||
{
|
||||
volk_free(d_grid_data[i]);
|
||||
delete[] d_grid_doppler_wipeoffs[i];
|
||||
}
|
||||
delete d_grid_data;
|
||||
delete d_grid_doppler_wipeoffs;
|
||||
}
|
||||
|
||||
pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
|
||||
{
|
||||
free(d_carrier);
|
||||
free(d_fft_codes);
|
||||
delete d_ifft;
|
||||
delete d_fft_if;
|
||||
if (d_dump)
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
free_grid_memory();
|
||||
volk_free(d_carrier);
|
||||
volk_free(d_fft_codes);
|
||||
volk_free(d_magnitude);
|
||||
delete d_ifft;
|
||||
delete d_fft_if;
|
||||
if (d_dump)
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
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)
|
||||
{
|
||||
|
||||
memcpy(d_fft_if->get_inbuf(),code,sizeof(gr_complex)*d_fft_size);
|
||||
d_fft_if->execute(); // We need the FFT of local code
|
||||
//Conjugate the local code
|
||||
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(),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
|
||||
//Conjugate the local code
|
||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
|
||||
void pcps_acquisition_fine_doppler_cc::init()
|
||||
{
|
||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||
d_gnss_synchro->Acq_samplestamp_samples = 0;
|
||||
d_input_power = 0.0;
|
||||
d_state=0;
|
||||
d_gnss_synchro->Acq_delay_samples = 0.0;
|
||||
d_gnss_synchro->Acq_doppler_hz = 0.0;
|
||||
d_gnss_synchro->Acq_samplestamp_samples = 0;
|
||||
d_input_power = 0.0;
|
||||
d_state = 0;
|
||||
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
d_well_count=0;
|
||||
for (int i=0;i<d_num_doppler_points;i++)
|
||||
{
|
||||
for (unsigned int j=0;j<d_fft_size;j++)
|
||||
{
|
||||
d_grid_data[i][j]=0.0;
|
||||
}
|
||||
}
|
||||
d_well_count = 0;
|
||||
for (int i=0; i<d_num_doppler_points; i++)
|
||||
{
|
||||
for (unsigned int j=0; j < d_fft_size; j++)
|
||||
{
|
||||
d_grid_data[i][j] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
|
||||
{
|
||||
// create the carrier Doppler wipeoff signals
|
||||
int doppler_hz;
|
||||
// create the carrier Doppler wipeoff signals
|
||||
int doppler_hz;
|
||||
float phase_step_rad;
|
||||
d_grid_doppler_wipeoffs=new gr_complex*[d_num_doppler_points];
|
||||
for (int doppler_index=0;doppler_index<d_num_doppler_points;doppler_index++)
|
||||
{
|
||||
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_points];
|
||||
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 search steps
|
||||
// compute the carrier doppler wipe-off signal and store it
|
||||
phase_step_rad = (float)GPS_TWO_PI*doppler_hz / (float)d_fs_in;
|
||||
|
||||
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);
|
||||
}
|
||||
doppler_hz = d_config_doppler_min + d_doppler_step*doppler_index;
|
||||
// doppler search steps
|
||||
// compute the carrier doppler wipe-off signal and store it
|
||||
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];
|
||||
fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size,0, phase_step_rad);
|
||||
}
|
||||
}
|
||||
|
||||
double pcps_acquisition_fine_doppler_cc::search_maximum()
|
||||
{
|
||||
float magt = 0.0;
|
||||
float fft_normalization_factor;
|
||||
int index_doppler = 0;
|
||||
unsigned int tmp_intex_t;
|
||||
unsigned int index_time = 0;
|
||||
float magt = 0.0;
|
||||
float fft_normalization_factor;
|
||||
int index_doppler = 0;
|
||||
unsigned int tmp_intex_t;
|
||||
unsigned int index_time = 0;
|
||||
|
||||
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);
|
||||
if (d_grid_data[i][tmp_intex_t] > magt)
|
||||
{
|
||||
magt = d_grid_data[i][tmp_intex_t];
|
||||
//std::cout<<magt<<std::endl;
|
||||
index_doppler = i;
|
||||
index_time = tmp_intex_t;
|
||||
}
|
||||
}
|
||||
for (int i=0;i<d_num_doppler_points;i++)
|
||||
{
|
||||
volk_32f_index_max_16u(&tmp_intex_t, d_grid_data[i], d_fft_size);
|
||||
if (d_grid_data[i][tmp_intex_t] > magt)
|
||||
{
|
||||
magt = d_grid_data[i][tmp_intex_t];
|
||||
//std::cout<<magt<<std::endl;
|
||||
index_doppler = i;
|
||||
index_time = tmp_intex_t;
|
||||
}
|
||||
}
|
||||
|
||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
||||
fft_normalization_factor = (float)d_fft_size * (float)d_fft_size;;
|
||||
magt = magt / (fft_normalization_factor * fft_normalization_factor);
|
||||
// Normalize the maximum value to correct the scale factor introduced by FFTW
|
||||
fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
|
||||
magt = magt / (fft_normalization_factor * fft_normalization_factor);
|
||||
|
||||
// 5- Compute the test statistics and compare to the threshold
|
||||
d_test_statistics = magt/(d_input_power*std::sqrt(d_well_count));
|
||||
// 5- Compute the test statistics and compare to the threshold
|
||||
d_test_statistics = magt/(d_input_power*std::sqrt(d_well_count));
|
||||
|
||||
// 4- record the maximum peak and the associated synchronization parameters
|
||||
d_gnss_synchro->Acq_delay_samples = (double)index_time;
|
||||
d_gnss_synchro->Acq_doppler_hz = (double)(index_doppler*d_doppler_step+d_config_doppler_min);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
|
||||
// 4- record the maximum peak and the associated synchronization parameters
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
|
||||
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;
|
||||
|
||||
// Record results to file if required
|
||||
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_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
|
||||
d_dump_file.open(filename.str().c_str(), std::ios::out
|
||||
| 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.close();
|
||||
}
|
||||
// Record results to file if required
|
||||
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_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
|
||||
d_dump_file.open(filename.str().c_str(), std::ios::out
|
||||
| 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.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)
|
||||
{
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
// 1- Compute the input signal power estimation
|
||||
float power;
|
||||
power=0;
|
||||
if (is_unaligned())
|
||||
{
|
||||
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;
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
// Compute the input signal power estimation
|
||||
float power = 0;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&power, d_magnitude, d_fft_size);
|
||||
power /= static_cast<float>(d_fft_size);
|
||||
return power;
|
||||
|
||||
}
|
||||
|
||||
int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
|
||||
{
|
||||
// initialize acquisition algorithm
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
// initialize acquisition algorithm
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
|
||||
DLOG(INFO) << "Channel: " << d_channel
|
||||
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
|
||||
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
|
||||
<< d_threshold << ", doppler_max: " << d_config_doppler_max
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
DLOG(INFO) << "Channel: " << d_channel
|
||||
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " "<< d_gnss_synchro->PRN
|
||||
<< " ,sample stamp: " << d_sample_counter << ", threshold: "
|
||||
<< d_threshold << ", doppler_max: " << d_config_doppler_max
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
|
||||
|
||||
|
||||
// 2- Doppler frequency search loop
|
||||
float* p_tmp_vector;
|
||||
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
// 2- Doppler frequency search loop
|
||||
float* p_tmp_vector = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
for (int doppler_index=0;doppler_index<d_num_doppler_points;doppler_index++)
|
||||
{
|
||||
// doppler search steps
|
||||
// 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);
|
||||
// 3- Perform the FFT-based convolution (parallel time search)
|
||||
// Compute the FFT of the carrier wiped--off incoming signal
|
||||
d_fft_if->execute();
|
||||
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
|
||||
{
|
||||
// doppler search steps
|
||||
// Perform the carrier wipe-off
|
||||
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)
|
||||
// Compute the FFT of the carrier wiped--off incoming signal
|
||||
d_fft_if->execute();
|
||||
|
||||
// Multiply carrier wiped--off, Fourier transformed incoming signal
|
||||
// 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);
|
||||
// Multiply carrier wiped--off, Fourier transformed incoming signal
|
||||
// with the local FFT'd code reference using SIMD operations with VOLK library
|
||||
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
// compute the inverse FFT
|
||||
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);
|
||||
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_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
|
||||
const float* old_vector = d_grid_data[doppler_index];
|
||||
volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
free(p_tmp_vector);
|
||||
return d_fft_size;
|
||||
volk_free(p_tmp_vector);
|
||||
return d_fft_size;
|
||||
}
|
||||
|
||||
int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star &input_items, int available_samples)
|
||||
{
|
||||
|
||||
// Direct FFT
|
||||
int zero_padding_factor=16;
|
||||
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);
|
||||
//zero padding the entire vector
|
||||
memset(fft_operator->get_inbuf(),0,fft_size_extended*sizeof(gr_complex));
|
||||
// Direct FFT
|
||||
int zero_padding_factor = 16;
|
||||
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);
|
||||
|
||||
//1. generate local code aligned with the acquisition code phase estimation
|
||||
gr_complex *code_replica;
|
||||
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);
|
||||
//zero padding the entire vector
|
||||
memset(fft_operator->get_inbuf(), 0, fft_size_extended * sizeof(gr_complex));
|
||||
|
||||
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;
|
||||
// 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);
|
||||
}
|
||||
gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
|
||||
|
||||
//2. Perform code wipe-off
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
int shift_index = static_cast<int>(d_gnss_synchro->Acq_delay_samples);
|
||||
|
||||
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!)
|
||||
fft_operator->execute();
|
||||
//2. Perform code wipe-off
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
|
||||
// 4. Compute the magnitude and find the maximum
|
||||
float* p_tmp_vector;
|
||||
if (posix_memalign((void**)&p_tmp_vector, 16, fft_size_extended * sizeof(float)) == 0){};
|
||||
volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
|
||||
|
||||
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;
|
||||
volk_32f_index_max_16u_a(&tmp_index_freq,p_tmp_vector,fft_size_extended);
|
||||
// 4. Compute the magnitude and find the maximum
|
||||
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
|
||||
int counter=0;
|
||||
unsigned int tmp_index_freq = 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++)
|
||||
{
|
||||
fftFreqBins[counter]=(((float)d_fs_in/2.0)*(float)k)/((float)fft_size_extended/2.0);
|
||||
counter++;
|
||||
}
|
||||
float fftFreqBins[fft_size_extended];
|
||||
|
||||
for (int k=fft_size_extended/2;k>0;k--)
|
||||
{
|
||||
fftFreqBins[counter]=((-(float)d_fs_in/2)*(float)k)/((float)fft_size_extended/2.0);
|
||||
counter++;
|
||||
}
|
||||
for (int k=0; k < (fft_size_extended / 2); k++)
|
||||
{
|
||||
fftFreqBins[counter] = ((static_cast<float>(d_fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
|
||||
counter++;
|
||||
}
|
||||
|
||||
// 5. Update the Doppler estimation in Hz
|
||||
if (abs(fftFreqBins[tmp_index_freq]-d_gnss_synchro->Acq_doppler_hz)<1000)
|
||||
{
|
||||
d_gnss_synchro->Acq_doppler_hz=(double)fftFreqBins[tmp_index_freq];
|
||||
//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;
|
||||
DLOG(INFO)<<std::endl<<"Error estimating fine frequency Doppler"<<std::endl;
|
||||
//debug log
|
||||
//
|
||||
// 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);
|
||||
//
|
||||
// filename.str("");
|
||||
// filename << "../data/code_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*)code_replica, n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
// d_dump_file.close();
|
||||
//
|
||||
// filename.str("");
|
||||
// filename << "../data/signal_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*)in, n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
// 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();
|
||||
}
|
||||
for (int k = fft_size_extended / 2; k > 0; k--)
|
||||
{
|
||||
fftFreqBins[counter] = ((-static_cast<float>(d_fs_in) / 2) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
|
||||
counter++;
|
||||
}
|
||||
|
||||
// 5. Update the Doppler estimation in Hz
|
||||
if (abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000)
|
||||
{
|
||||
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;
|
||||
}
|
||||
else
|
||||
{
|
||||
DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz);
|
||||
DLOG(INFO) << "Error estimating fine frequency Doppler";
|
||||
//debug log
|
||||
//
|
||||
// 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);
|
||||
//
|
||||
// filename.str("");
|
||||
// filename << "../data/code_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*)code_replica, n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
// d_dump_file.close();
|
||||
//
|
||||
// filename.str("");
|
||||
// filename << "../data/signal_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*)in, n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
// 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!!
|
||||
delete fft_operator;
|
||||
free(code_replica);
|
||||
free(p_tmp_vector);
|
||||
return d_fft_size;
|
||||
// free memory!!
|
||||
delete fft_operator;
|
||||
volk_free(code_replica);
|
||||
volk_free(p_tmp_vector);
|
||||
return d_fft_size;
|
||||
}
|
||||
|
||||
|
||||
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_void_star &output_items)
|
||||
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
||||
gr_vector_void_star &output_items)
|
||||
{
|
||||
|
||||
/*!
|
||||
* TODO: High sensitivity acquisition algorithm:
|
||||
* State Mechine:
|
||||
* S0. StandBy. If d_active==1 -> S1
|
||||
* S1. ComputeGrid. Perform the FFT acqusition doppler and delay grid.
|
||||
* Accumulate the search grid matrix (#doppler_bins x #fft_size)
|
||||
* Compare maximum to threshold and decide positive or negative
|
||||
* If T>=gamma -> S4 else
|
||||
* If d_well_count<max_dwells -> S2
|
||||
* else -> S5.
|
||||
* S4. Positive_Acq: Send message and stop acq -> S0
|
||||
* S5. Negative_Acq: Send message and stop acq -> S0
|
||||
*/
|
||||
/*!
|
||||
* TODO: High sensitivity acquisition algorithm:
|
||||
* State Mechine:
|
||||
* S0. StandBy. If d_active==1 -> S1
|
||||
* S1. ComputeGrid. Perform the FFT acqusition doppler and delay grid.
|
||||
* Accumulate the search grid matrix (#doppler_bins x #fft_size)
|
||||
* Compare maximum to threshold and decide positive or negative
|
||||
* If T>=gamma -> S4 else
|
||||
* If d_well_count<max_dwells -> S2
|
||||
* else -> S5.
|
||||
* S4. Positive_Acq: Send message and stop acq -> S0
|
||||
* S5. Negative_Acq: Send message and stop acq -> S0
|
||||
*/
|
||||
|
||||
switch (d_state)
|
||||
{
|
||||
case 0: // S0. StandBy
|
||||
//DLOG(INFO) <<"S0"<<std::endl;
|
||||
if (d_active==true)
|
||||
{
|
||||
reset_grid();
|
||||
d_state=1;
|
||||
}
|
||||
break;
|
||||
case 1: // S1. ComputeGrid
|
||||
//DLOG(INFO) <<"S1"<<std::endl;
|
||||
compute_and_accumulate_grid(input_items);
|
||||
d_well_count++;
|
||||
if (d_well_count>=d_max_dwells)
|
||||
{
|
||||
d_state=2;
|
||||
}
|
||||
break;
|
||||
case 2: // Compute test statistics and decide
|
||||
//DLOG(INFO) <<"S2"<<std::endl;
|
||||
d_input_power=estimate_input_power(input_items);
|
||||
d_test_statistics=search_maximum();
|
||||
if (d_test_statistics > d_threshold)
|
||||
{
|
||||
d_state=3; //perform fine doppler estimation
|
||||
}else{
|
||||
d_state=5; //negative acquisition
|
||||
}
|
||||
break;
|
||||
switch (d_state)
|
||||
{
|
||||
case 0: // S0. StandBy
|
||||
//DLOG(INFO) <<"S0"<<std::endl;
|
||||
if (d_active == true)
|
||||
{
|
||||
reset_grid();
|
||||
d_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: // S1. ComputeGrid
|
||||
//DLOG(INFO) <<"S1"<<std::endl;
|
||||
compute_and_accumulate_grid(input_items);
|
||||
d_well_count++;
|
||||
if (d_well_count >= d_max_dwells)
|
||||
{
|
||||
d_state = 2;
|
||||
}
|
||||
break;
|
||||
case 2: // Compute test statistics and decide
|
||||
//DLOG(INFO) <<"S2"<<std::endl;
|
||||
d_input_power = estimate_input_power(input_items);
|
||||
d_test_statistics = search_maximum();
|
||||
if (d_test_statistics > d_threshold)
|
||||
{
|
||||
d_state = 3; //perform fine doppler estimation
|
||||
}
|
||||
else
|
||||
{
|
||||
d_state = 5; //negative acquisition
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
|
||||
case 3: // Fine doppler estimation
|
||||
//DLOG(INFO) <<"S3"<<std::endl;
|
||||
DLOG(INFO) << "Performing fine Doppler estimation";
|
||||
estimate_Doppler(input_items, ninput_items[0]); //disabled in repo
|
||||
d_state=4;
|
||||
break;
|
||||
case 4: // Positive_Acq
|
||||
//DLOG(INFO) <<"S4"<<std::endl;
|
||||
DLOG(INFO) << "positive acquisition";
|
||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||
DLOG(INFO) << "sample_stamp " << d_sample_counter;
|
||||
DLOG(INFO) << "test statistics value " << d_test_statistics;
|
||||
DLOG(INFO) << "test statistics threshold " << d_threshold;
|
||||
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
|
||||
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
DLOG(INFO) << "input signal power " << d_input_power;
|
||||
case 3: // Fine doppler estimation
|
||||
//DLOG(INFO) <<"S3"<<std::endl;
|
||||
DLOG(INFO) << "Performing fine Doppler estimation";
|
||||
estimate_Doppler(input_items, ninput_items[0]); //disabled in repo
|
||||
d_state = 4;
|
||||
break;
|
||||
case 4: // Positive_Acq
|
||||
//DLOG(INFO) <<"S4"<<std::endl;
|
||||
DLOG(INFO) << "positive acquisition";
|
||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||
DLOG(INFO) << "sample_stamp " << d_sample_counter;
|
||||
DLOG(INFO) << "test statistics value " << d_test_statistics;
|
||||
DLOG(INFO) << "test statistics threshold " << d_threshold;
|
||||
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
|
||||
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
DLOG(INFO) << "input signal power " << d_input_power;
|
||||
|
||||
d_active = false;
|
||||
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||
d_channel_internal_queue->push(1); // 1-> positive acquisition
|
||||
d_state=0;
|
||||
break;
|
||||
case 5: // Negative_Acq
|
||||
//DLOG(INFO) <<"S5"<<std::endl;
|
||||
DLOG(INFO) << "negative acquisition";
|
||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||
DLOG(INFO) << "sample_stamp " << d_sample_counter;
|
||||
DLOG(INFO) << "test statistics value " << d_test_statistics;
|
||||
DLOG(INFO) << "test statistics threshold " << d_threshold;
|
||||
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
|
||||
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
DLOG(INFO) << "input signal power " << d_input_power;
|
||||
d_active = false;
|
||||
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||
d_channel_internal_queue->push(1); // 1-> positive acquisition
|
||||
d_state = 0;
|
||||
break;
|
||||
case 5: // Negative_Acq
|
||||
//DLOG(INFO) <<"S5"<<std::endl;
|
||||
DLOG(INFO) << "negative acquisition";
|
||||
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
|
||||
DLOG(INFO) << "sample_stamp " << d_sample_counter;
|
||||
DLOG(INFO) << "test statistics value " << d_test_statistics;
|
||||
DLOG(INFO) << "test statistics threshold " << d_threshold;
|
||||
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
|
||||
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
DLOG(INFO) << "input signal power " << d_input_power;
|
||||
|
||||
d_active = false;
|
||||
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||
d_channel_internal_queue->push(2); // 2-> negative acquisition
|
||||
d_state=0;
|
||||
break;
|
||||
default:
|
||||
d_state=0;
|
||||
break;
|
||||
}
|
||||
d_active = false;
|
||||
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
|
||||
d_channel_internal_queue->push(2); // 2-> negative acquisition
|
||||
d_state = 0;
|
||||
break;
|
||||
default:
|
||||
d_state = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
//DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl;
|
||||
d_sample_counter += d_fft_size; // sample counter
|
||||
consume_each(d_fft_size);
|
||||
return 0;
|
||||
//DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl;
|
||||
d_sample_counter += d_fft_size; // sample counter
|
||||
consume_each(d_fft_size);
|
||||
return 0;
|
||||
}
|
||||
|
@ -82,9 +82,8 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
|
||||
d_input_power = 0.0;
|
||||
d_state = 0;
|
||||
d_disable_assist = false;
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_carrier, 16, 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()));
|
||||
d_carrier = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
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()
|
||||
{
|
||||
free(d_carrier);
|
||||
free(d_fft_codes);
|
||||
volk_free(d_carrier);
|
||||
volk_free(d_fft_codes);
|
||||
delete d_ifft;
|
||||
delete d_fft_if;
|
||||
if (d_dump)
|
||||
@ -150,7 +149,7 @@ void pcps_assisted_acquisition_cc::init()
|
||||
d_fft_if->execute(); // We need the FFT of 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;
|
||||
}
|
||||
// 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];
|
||||
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 search steps
|
||||
// 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];
|
||||
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
|
||||
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);
|
||||
|
||||
// 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
|
||||
d_gnss_synchro->Acq_delay_samples = (double)index_time;
|
||||
d_gnss_synchro->Acq_doppler_hz = (double)(index_doppler*d_doppler_step + d_doppler_min);
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
|
||||
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;
|
||||
|
||||
// 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
|
||||
// 1- Compute the input signal power estimation
|
||||
float* p_tmp_vector;
|
||||
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);
|
||||
float* p_tmp_vector = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
volk_32fc_magnitude_squared_32f(p_tmp_vector, in, d_fft_size);
|
||||
|
||||
const float* p_const_tmp_vector = p_tmp_vector;
|
||||
float power;
|
||||
volk_32f_accumulator_s32f_a(&power, p_const_tmp_vector, d_fft_size);
|
||||
free(p_tmp_vector);
|
||||
return ( power / (float)d_fft_size);
|
||||
volk_32f_accumulator_s32f(&power, p_const_tmp_vector, d_fft_size);
|
||||
volk_free(p_tmp_vector);
|
||||
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;
|
||||
|
||||
// 2- Doppler frequency search loop
|
||||
float* p_tmp_vector;
|
||||
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
float* p_tmp_vector = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
|
||||
{
|
||||
// doppler search steps
|
||||
// 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)
|
||||
// Compute the FFT of the carrier wiped--off incoming signal
|
||||
d_fft_if->execute();
|
||||
|
||||
// Multiply carrier wiped--off, Fourier transformed incoming signal
|
||||
// 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
|
||||
d_ifft->execute();
|
||||
|
||||
// save the grid matrix delay file
|
||||
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
|
||||
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_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
|
||||
const float* old_vector = d_grid_data[doppler_index];
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int pcps_assisted_acquisition_cc::general_work(int noutput_items,
|
||||
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
|
||||
gr_vector_void_star &output_items)
|
||||
|
@ -86,14 +86,13 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
|
||||
d_input_power = 0.0;
|
||||
d_num_doppler_bins = 0;
|
||||
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_fft_code_data, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_fft_code_pilot, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_data_correlation, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_pilot_correlation, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_correlation_plus, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_correlation_minus, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
d_fft_code_data = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_fft_code_pilot = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_data_correlation = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_pilot_correlation = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_correlation_plus = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_correlation_minus = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
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++)
|
||||
{
|
||||
free(d_grid_doppler_wipeoffs[i]);
|
||||
volk_free(d_grid_doppler_wipeoffs[i]);
|
||||
}
|
||||
delete[] d_grid_doppler_wipeoffs;
|
||||
}
|
||||
|
||||
free(d_fft_code_data);
|
||||
free(d_fft_code_pilot);
|
||||
free(d_data_correlation);
|
||||
free(d_pilot_correlation);
|
||||
free(d_correlation_plus);
|
||||
free(d_correlation_minus);
|
||||
free(d_magnitude);
|
||||
volk_free(d_fft_code_data);
|
||||
volk_free(d_fft_code_pilot);
|
||||
volk_free(d_data_correlation);
|
||||
volk_free(d_pilot_correlation);
|
||||
volk_free(d_correlation_plus);
|
||||
volk_free(d_correlation_minus);
|
||||
volk_free(d_magnitude);
|
||||
|
||||
delete d_ifft;
|
||||
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,
|
||||
std::complex<float> * code_pilot)
|
||||
void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float>* code_data,
|
||||
std::complex<float>* code_pilot)
|
||||
{
|
||||
// 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
|
||||
|
||||
//Conjugate the local code
|
||||
if (is_unaligned())
|
||||
{
|
||||
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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(d_fft_code_data,d_fft_if->get_outbuf(),d_fft_size);
|
||||
|
||||
// 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
|
||||
|
||||
//Conjugate the local code,
|
||||
if (is_unaligned())
|
||||
{
|
||||
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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(d_fft_code_pilot, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
|
||||
void pcps_cccwsr_acquisition_cc::init()
|
||||
@ -178,8 +163,8 @@ void pcps_cccwsr_acquisition_cc::init()
|
||||
|
||||
// Count the number of bins
|
||||
d_num_doppler_bins = 0;
|
||||
for (int doppler = (int)(-d_doppler_max);
|
||||
doppler <= (int)d_doppler_max;
|
||||
for (int doppler = static_cast<int>(-d_doppler_max);
|
||||
doppler <= static_cast<int>(d_doppler_max);
|
||||
doppler += d_doppler_step)
|
||||
{
|
||||
d_num_doppler_bins++;
|
||||
@ -187,12 +172,11 @@ void pcps_cccwsr_acquisition_cc::init()
|
||||
|
||||
// Create the carrier Doppler wipeoff signals
|
||||
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_fft_size * sizeof(gr_complex)) == 0){};
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
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],
|
||||
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_minus = 0.0;
|
||||
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
|
||||
|
||||
@ -252,18 +236,18 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= static_cast<float>(d_fft_size);
|
||||
|
||||
// 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=-(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);
|
||||
|
||||
// 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
|
||||
// with the local FFT'd data code reference (E1B) 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_data, d_fft_size);
|
||||
|
||||
// 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
|
||||
// with the local FFT'd pilot code reference (E1C) 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_pilot, d_fft_size);
|
||||
|
||||
// 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());
|
||||
}
|
||||
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_correlation_plus, d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_plus, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_plus, 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);
|
||||
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_correlation_minus, d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext_minus, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_correlation_minus, 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);
|
||||
|
||||
if (magt_plus >= magt_minus)
|
||||
@ -330,8 +314,8 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
|
||||
if (d_mag < magt)
|
||||
{
|
||||
d_mag = magt;
|
||||
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = (double)doppler;
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
|
||||
}
|
||||
|
||||
|
@ -92,12 +92,10 @@ pcps_multithread_acquisition_cc::pcps_multithread_acquisition_cc(
|
||||
//todo: do something if posix_memalign fails
|
||||
for (unsigned int i = 0; i < d_max_dwells; i++)
|
||||
{
|
||||
if (posix_memalign((void**)&d_in_buffer[i], 16,
|
||||
d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
|
||||
d_in_buffer[i] = 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){};
|
||||
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
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++)
|
||||
{
|
||||
free(d_grid_doppler_wipeoffs[i]);
|
||||
volk_free(d_grid_doppler_wipeoffs[i]);
|
||||
}
|
||||
delete[] d_grid_doppler_wipeoffs;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
free(d_fft_codes);
|
||||
free(d_magnitude);
|
||||
volk_free(d_fft_codes);
|
||||
volk_free(d_magnitude);
|
||||
|
||||
delete d_ifft;
|
||||
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];
|
||||
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_fft_size * sizeof(gr_complex)) == 0){};
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
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],
|
||||
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
|
||||
|
||||
//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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
|
||||
void pcps_multithread_acquisition_cc::acquisition_core()
|
||||
@ -208,18 +198,18 @@ void pcps_multithread_acquisition_cc::acquisition_core()
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
|
||||
// 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=-(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);
|
||||
|
||||
// 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
|
||||
// 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);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
|
||||
// Search maximum
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), 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
|
||||
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
|
||||
|
@ -105,7 +105,7 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
|
||||
d_well_count = 0;
|
||||
d_doppler_max = doppler_max;
|
||||
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_input_power = 0.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_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++)
|
||||
{
|
||||
if (posix_memalign((void**)&d_in_buffer[i], 16,
|
||||
d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
|
||||
d_in_buffer[i] = 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){};
|
||||
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size_pow2 * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_zero_vector, 16, (d_fft_size_pow2-d_fft_size) * sizeof(gr_complex)) == 0){};
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size_pow2 * sizeof(gr_complex), volk_get_alignment()));
|
||||
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++)
|
||||
{
|
||||
@ -156,20 +152,20 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
free(d_fft_codes);
|
||||
free(d_magnitude);
|
||||
free(d_zero_vector);
|
||||
volk_free(d_fft_codes);
|
||||
volk_free(d_magnitude);
|
||||
volk_free(d_zero_vector);
|
||||
|
||||
if (d_opencl == 0)
|
||||
{
|
||||
@ -302,8 +298,8 @@ void pcps_opencl_acquisition_cc::init()
|
||||
|
||||
// Count the number of bins
|
||||
d_num_doppler_bins = 0;
|
||||
for (int doppler = (int)(-d_doppler_max);
|
||||
doppler <= (int)d_doppler_max;
|
||||
for (int doppler = static_cast<int>(-d_doppler_max);
|
||||
doppler <= static_cast<int>(d_doppler_max);
|
||||
doppler += d_doppler_step)
|
||||
{
|
||||
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++)
|
||||
{
|
||||
if (posix_memalign((void**)&(d_grid_doppler_wipeoffs[doppler_index]), 16,
|
||||
d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
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],
|
||||
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)
|
||||
{
|
||||
if(d_opencl == 0)
|
||||
{
|
||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0,
|
||||
sizeof(gr_complex)*d_fft_size, code);
|
||||
{
|
||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, 0,
|
||||
sizeof(gr_complex)*d_fft_size, code);
|
||||
|
||||
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),
|
||||
d_zero_vector);
|
||||
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),
|
||||
d_zero_vector);
|
||||
|
||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)
|
||||
*(d_fft_size_pow2 - d_fft_size),
|
||||
sizeof(gr_complex)*d_fft_size, code);
|
||||
d_cl_queue->enqueueWriteBuffer(*d_cl_buffer_2, CL_TRUE, sizeof(gr_complex)
|
||||
*(d_fft_size_pow2 - d_fft_size),
|
||||
sizeof(gr_complex)*d_fft_size, code);
|
||||
|
||||
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
|
||||
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
|
||||
0, NULL, NULL);
|
||||
clFFT_ExecuteInterleaved((*d_cl_queue)(), d_cl_fft_plan, d_cl_fft_batch_size,
|
||||
clFFT_Forward, (*d_cl_buffer_2)(), (*d_cl_buffer_2)(),
|
||||
0, NULL, NULL);
|
||||
|
||||
//Conjucate the local code
|
||||
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
|
||||
kernel.setArg(0, *d_cl_buffer_2); //input
|
||||
kernel.setArg(1, *d_cl_buffer_fft_codes); //output
|
||||
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
|
||||
}
|
||||
//Conjucate the local code
|
||||
cl::Kernel kernel = cl::Kernel(d_cl_program, "conj_vector");
|
||||
kernel.setArg(0, *d_cl_buffer_2); //input
|
||||
kernel.setArg(1, *d_cl_buffer_fft_codes); //output
|
||||
d_cl_queue->enqueueNDRangeKernel(kernel, cl::NullRange, cl::NDRange(d_fft_size_pow2), cl::NullRange);
|
||||
}
|
||||
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
|
||||
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);
|
||||
}
|
||||
}
|
||||
//Conjugate the local code
|
||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
}
|
||||
|
||||
void pcps_opencl_acquisition_cc::acquisition_core_volk()
|
||||
@ -393,7 +381,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
|
||||
int doppler;
|
||||
unsigned int indext = 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];
|
||||
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;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= static_cast<float>(d_fft_size);
|
||||
|
||||
// 2- Doppler frequency search loop
|
||||
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||
{
|
||||
// doppler search steps
|
||||
|
||||
doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
|
||||
|
||||
|
||||
volk_32fc_x2_multiply_32fc_a(d_fft_if->get_inbuf(), in,
|
||||
doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
|
||||
|
||||
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)
|
||||
@ -430,15 +416,15 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
|
||||
|
||||
// Multiply carrier wiped--off, Fourier transformed incoming signal
|
||||
// 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);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
|
||||
// Search maximum
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, d_ifft->get_outbuf(), d_fft_size);
|
||||
volk_32f_index_max_16u_a(&indext, d_magnitude, d_fft_size);
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf(), 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
|
||||
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.
|
||||
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_doppler_hz = (double)doppler;
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
|
||||
|
||||
// 5- Compute the test statistics and compare to the threshold
|
||||
@ -517,7 +503,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
||||
int doppler;
|
||||
unsigned int indext = 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];
|
||||
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;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= static_cast<float>(d_fft_size);
|
||||
|
||||
cl::Kernel kernel;
|
||||
|
||||
@ -554,7 +540,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
||||
{
|
||||
// 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
|
||||
kernel = cl::Kernel(d_cl_program, "mult_vectors");
|
||||
@ -600,7 +586,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
|
||||
|
||||
// Search maximum
|
||||
// @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
|
||||
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.
|
||||
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_doppler_hz = (double)doppler;
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
|
||||
|
||||
// 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
|
||||
// consecutive signal blocks will be processed in multi-dwell operation. This is
|
||||
// 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++)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
@ -70,9 +70,9 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
|
||||
bool bit_transition_flag,
|
||||
gr::msg_queue::sptr queue, bool dump,
|
||||
std::string dump_filename):
|
||||
gr::block("pcps_quicksync_acquisition_cc",
|
||||
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::block("pcps_quicksync_acquisition_cc",
|
||||
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 )))
|
||||
{
|
||||
//DLOG(INFO) << "START CONSTRUCTOR";
|
||||
|
||||
@ -97,20 +97,17 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
|
||||
//fft size is reduced.
|
||||
d_fft_size = (d_samples_per_code) / d_folding_factor;
|
||||
|
||||
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
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_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_samples_per_code * d_folding_factor * sizeof(float), volk_get_alignment()));
|
||||
d_magnitude_folded = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
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
|
||||
original form to perform later correlation in time domain*/
|
||||
d_code = new gr_complex[d_samples_per_code]();
|
||||
|
||||
|
||||
// Direct FFT
|
||||
d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
|
||||
// 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++)
|
||||
{
|
||||
free(d_grid_doppler_wipeoffs[i]);
|
||||
volk_free(d_grid_doppler_wipeoffs[i]);
|
||||
}
|
||||
delete[] d_grid_doppler_wipeoffs;
|
||||
}
|
||||
|
||||
free(d_fft_codes);
|
||||
free(d_magnitude);
|
||||
free(d_magnitude_folded);
|
||||
volk_free(d_fft_codes);
|
||||
volk_free(d_magnitude);
|
||||
volk_free(d_magnitude_folded);
|
||||
|
||||
delete d_ifft;
|
||||
d_ifft = NULL;
|
||||
delete d_fft_if;
|
||||
d_fft_if = NULL;
|
||||
delete d_code;
|
||||
d_code = NULL;
|
||||
delete d_possible_delay;
|
||||
d_possible_delay = NULL;
|
||||
delete d_corr_output_f;
|
||||
d_corr_output_f = NULL;
|
||||
|
||||
if (d_dump)
|
||||
{
|
||||
d_dump_file.close();
|
||||
@ -156,44 +149,33 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
|
||||
// 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-
|
||||
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]();
|
||||
memcpy(d_fft_if->get_inbuf(), d_code_folded, sizeof(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));
|
||||
|
||||
/*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
|
||||
in the frequency domain after applying the fftw operation*/
|
||||
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(),
|
||||
std::plus<gr_complex>());
|
||||
}
|
||||
|
||||
|
||||
d_fft_if->execute(); // We need the FFT of local code
|
||||
|
||||
//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);
|
||||
}
|
||||
// DLOG(INFO) << "END LOCAL CODE";
|
||||
|
||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
|
||||
|
||||
void pcps_quicksync_acquisition_cc::init()
|
||||
{
|
||||
//DLOG(INFO) << "START init";
|
||||
@ -205,8 +187,8 @@ void pcps_quicksync_acquisition_cc::init()
|
||||
|
||||
// Count the number of bins
|
||||
d_num_doppler_bins = 0;
|
||||
for (int doppler = (int)(-d_doppler_max);
|
||||
doppler <= (int)(d_doppler_max);
|
||||
for (int doppler = static_cast<int>(-d_doppler_max);
|
||||
doppler <= static_cast<int>(d_doppler_max);
|
||||
doppler += d_doppler_step)
|
||||
{
|
||||
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];
|
||||
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_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){};
|
||||
|
||||
int doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
|
||||
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()));
|
||||
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_samples_per_code * d_folding_factor);
|
||||
@ -227,6 +207,7 @@ void pcps_quicksync_acquisition_cc::init()
|
||||
// DLOG(INFO) << "end init";
|
||||
}
|
||||
|
||||
|
||||
int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
||||
gr_vector_int &ninput_items, gr_vector_const_void_star &input_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;
|
||||
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
|
||||
|
||||
gr_complex *in_temp;
|
||||
if (posix_memalign((void**)&(in_temp), 16,d_samples_per_code * d_folding_factor * sizeof(gr_complex)) == 0){};
|
||||
|
||||
|
||||
gr_complex *in_temp_folded;
|
||||
if (posix_memalign((void**)&(in_temp_folded), 16,d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
gr_complex* in_temp = static_cast<gr_complex*>(volk_malloc(d_samples_per_code * d_folding_factor * sizeof(gr_complex), volk_get_alignment()));
|
||||
gr_complex* in_temp_folded = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
/*Create a signal to store a signal of size 1ms, to perform correlation
|
||||
in time. No folding on this data is required*/
|
||||
gr_complex *in_1code;
|
||||
if (posix_memalign((void**)&(in_1code), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
|
||||
gr_complex* in_1code = static_cast<gr_complex*>(volk_malloc(d_samples_per_code * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
/*Stores the values of the correlation output between the local code
|
||||
and the signal with doppler shift corrected */
|
||||
gr_complex *corr_output;
|
||||
if (posix_memalign((void**)&(corr_output), 16,d_samples_per_code * sizeof(gr_complex)) == 0){};
|
||||
gr_complex* corr_output = static_cast<gr_complex*>(volk_malloc(d_samples_per_code * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
/*Stores a copy of the folded version of the signal.This is used for
|
||||
the FFT operations in future steps of excecution*/
|
||||
// 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_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
|
||||
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_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_samples_per_code * d_folding_factor);
|
||||
d_input_power /= (float)(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(&d_input_power, d_magnitude, 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
|
||||
at zero. This is done to avoid over acumulation when performing
|
||||
the folding process to be stored in d_fft_if->get_inbuf()*/
|
||||
d_signal_folded = new gr_complex[d_fft_size]();
|
||||
memcpy( d_fft_if->get_inbuf(),d_signal_folded,
|
||||
sizeof(gr_complex)*(d_fft_size));
|
||||
memcpy( d_fft_if->get_inbuf(), d_signal_folded, sizeof(gr_complex) * (d_fft_size));
|
||||
|
||||
/*Doppler search steps and then multiplication of the incoming
|
||||
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
|
||||
complex exponential vector. This removes the frequency doppler
|
||||
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_samples_per_code * d_folding_factor);
|
||||
|
||||
/*Perform folding of the carrier wiped-off incoming signal. Since
|
||||
superlinear method is being used the folding factor in the
|
||||
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),
|
||||
(in_temp+((i+1)*d_fft_size)) ,
|
||||
std::transform ((in_temp + i * d_fft_size),
|
||||
(in_temp + ((i + 1) * d_fft_size)) ,
|
||||
d_fft_if->get_inbuf(),
|
||||
d_fft_if->get_inbuf(),
|
||||
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
|
||||
signal 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);
|
||||
|
||||
/* 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
|
||||
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);
|
||||
|
||||
/* Normalize the maximum value to correct the scale factor
|
||||
introduced by FFTW*/
|
||||
//volk_32f_s32f_multiply_32f_a(d_magnitude_folded,d_magnitude_folded,
|
||||
// (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;
|
||||
|
||||
@ -405,88 +377,62 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
|
||||
{
|
||||
unsigned int detected_delay_samples_folded = 0;
|
||||
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[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+
|
||||
(i)*d_fft_size;
|
||||
d_possible_delay[i] = detected_delay_samples_folded + (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.
|
||||
The copied signal must have doppler effect corrected*/
|
||||
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
|
||||
generated code with the incoming signal with doppler
|
||||
effect corrected and accumulates its value. This
|
||||
is indeed correlation in time for an specific value
|
||||
of a shift*/
|
||||
volk_32fc_x2_multiply_32fc_a(corr_output, in_1code,
|
||||
d_code, d_samples_per_code);
|
||||
volk_32fc_x2_multiply_32fc(corr_output, in_1code, 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]);
|
||||
}
|
||||
|
||||
}
|
||||
/*Obtain maximun value of correlation given the
|
||||
possible delay selected */
|
||||
volk_32fc_magnitude_squared_32f_a(d_corr_output_f,
|
||||
complex_acumulator, d_folding_factor);
|
||||
volk_32f_index_max_16u_a(&indext, d_corr_output_f,
|
||||
d_folding_factor);
|
||||
/*Obtain maximun value of correlation given the possible delay selected */
|
||||
volk_32fc_magnitude_squared_32f(d_corr_output_f, complex_acumulator, d_folding_factor);
|
||||
volk_32f_index_max_16u(&indext, d_corr_output_f, d_folding_factor);
|
||||
|
||||
/*Now save the real code phase in the gnss_syncro
|
||||
block for use in other stages*/
|
||||
d_gnss_synchro->Acq_delay_samples = (double)
|
||||
(d_possible_delay[indext]);
|
||||
d_gnss_synchro->Acq_doppler_hz = (double)doppler;
|
||||
/*Now save the real code phase in the gnss_syncro block for use in other stages*/
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
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;
|
||||
//delete complex_acumulator;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// Record results to file if required
|
||||
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
|
||||
of the FFT, it is esential to also keep the values obtained from the
|
||||
possible delay to show how it is maximize*/
|
||||
of the FFT, it is esential to also keep the values obtained from the
|
||||
possible delay to show how it is maximize*/
|
||||
std::stringstream filename;
|
||||
std::streamsize n = 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_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_magnitude_folded, n); //write directly |abs(x)|^2 in this Doppler bin?
|
||||
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);
|
||||
|
||||
delete d_code_folded;
|
||||
d_code_folded = NULL;
|
||||
|
||||
free(in_temp);
|
||||
free(in_1code);
|
||||
free(corr_output);
|
||||
|
||||
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 threshold " << d_threshold;
|
||||
DLOG(INFO) << "folding factor " << d_folding_factor;
|
||||
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];
|
||||
DLOG(INFO) << "possible delay correlation output";
|
||||
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) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
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) << "folding factor "<<d_folding_factor;
|
||||
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) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
|
||||
DLOG(INFO) << "magnitude folded " << d_mag;
|
||||
|
@ -112,12 +112,12 @@ private:
|
||||
|
||||
gr_complex* d_code;
|
||||
unsigned int d_folding_factor; // also referred in the paper as 'p'
|
||||
float * d_corr_acumulator;
|
||||
unsigned int *d_possible_delay;
|
||||
float *d_corr_output_f;
|
||||
float * d_magnitude_folded;
|
||||
gr_complex *d_signal_folded;
|
||||
gr_complex *d_code_folded;
|
||||
float* d_corr_acumulator;
|
||||
unsigned int* d_possible_delay;
|
||||
float* d_corr_output_f;
|
||||
float* d_magnitude_folded;
|
||||
gr_complex* d_signal_folded;
|
||||
gr_complex* d_code_folded;
|
||||
float d_noise_floor_power;
|
||||
|
||||
long d_fs_in;
|
||||
|
@ -99,9 +99,8 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
|
||||
d_input_power = 0.0;
|
||||
d_num_doppler_bins = 0;
|
||||
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&d_fft_codes, 16, d_fft_size * sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_magnitude, 16, d_fft_size * sizeof(float)) == 0){};
|
||||
d_fft_codes = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
d_magnitude = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
// Direct FFT
|
||||
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++)
|
||||
{
|
||||
free(d_grid_doppler_wipeoffs[i]);
|
||||
free(d_grid_data[i]);
|
||||
volk_free(d_grid_doppler_wipeoffs[i]);
|
||||
volk_free(d_grid_data[i]);
|
||||
}
|
||||
delete[] d_grid_doppler_wipeoffs;
|
||||
delete[] d_grid_data;
|
||||
}
|
||||
|
||||
free(d_fft_codes);
|
||||
free(d_magnitude);
|
||||
volk_free(d_fft_codes);
|
||||
volk_free(d_magnitude);
|
||||
|
||||
delete d_ifft;
|
||||
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
|
||||
|
||||
//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);
|
||||
}
|
||||
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
|
||||
}
|
||||
|
||||
void pcps_tong_acquisition_cc::init()
|
||||
@ -166,8 +158,8 @@ void pcps_tong_acquisition_cc::init()
|
||||
|
||||
// Count the number of bins
|
||||
d_num_doppler_bins = 0;
|
||||
for (int doppler = (int)(-d_doppler_max);
|
||||
doppler <= (int)d_doppler_max;
|
||||
for (int doppler = static_cast<int>(-d_doppler_max);
|
||||
doppler <= static_cast<int>(d_doppler_max);
|
||||
doppler += d_doppler_step)
|
||||
{
|
||||
d_num_doppler_bins++;
|
||||
@ -178,16 +170,14 @@ void pcps_tong_acquisition_cc::init()
|
||||
d_grid_data = new float*[d_num_doppler_bins];
|
||||
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_fft_size * sizeof(gr_complex)) == 0){};
|
||||
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
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],
|
||||
d_freq + doppler, d_fs_in, d_fft_size);
|
||||
|
||||
if (posix_memalign((void**)&(d_grid_data[doppler_index]), 16,
|
||||
d_fft_size * sizeof(float)) == 0){};
|
||||
d_grid_data[doppler_index] = static_cast<float*>(volk_malloc(d_fft_size * sizeof(float), volk_get_alignment()));
|
||||
|
||||
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;
|
||||
float magt = 0.0;
|
||||
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_mag = 0.0;
|
||||
|
||||
@ -257,18 +247,18 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
||||
<< ", doppler_step: " << d_doppler_step;
|
||||
|
||||
// 1- Compute the input signal power estimation
|
||||
volk_32fc_magnitude_squared_32f_a(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f_a(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= (float)d_fft_size;
|
||||
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
|
||||
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
d_input_power /= static_cast<float>(d_fft_size);
|
||||
|
||||
// 2- Doppler frequency search loop
|
||||
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
|
||||
{
|
||||
// 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);
|
||||
|
||||
// 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
|
||||
// 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);
|
||||
|
||||
// compute the inverse FFT
|
||||
d_ifft->execute();
|
||||
|
||||
// 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.
|
||||
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),
|
||||
d_fft_size);
|
||||
|
||||
// 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
|
||||
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];
|
||||
|
||||
@ -303,8 +293,8 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
|
||||
if (d_mag < magt)
|
||||
{
|
||||
d_mag = magt;
|
||||
d_gnss_synchro->Acq_delay_samples = (double)(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = (double)doppler;
|
||||
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
|
||||
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
|
||||
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
|
||||
}
|
||||
|
||||
|
@ -60,15 +60,14 @@ direct_resampler_conditioner_cc::direct_resampler_conditioner_cc(
|
||||
d_history(1)
|
||||
{
|
||||
// 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)
|
||||
{
|
||||
d_phase_step = (unsigned int)floor((double)4294967296.0
|
||||
* sample_freq_out / sample_freq_in);
|
||||
d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_out / sample_freq_in));
|
||||
}
|
||||
else
|
||||
{
|
||||
d_phase_step = (unsigned int)floor((double)4294967296.0
|
||||
* sample_freq_in / sample_freq_out);
|
||||
d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_in / sample_freq_out));
|
||||
}
|
||||
set_relative_rate(1.0 * sample_freq_out / sample_freq_in);
|
||||
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,
|
||||
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);
|
||||
unsigned ninputs = ninput_items_required.size();
|
||||
for (unsigned i = 0; i < ninputs; i++)
|
||||
|
@ -58,17 +58,15 @@ direct_resampler_conditioner_ss::direct_resampler_conditioner_ss(
|
||||
d_sample_freq_in(sample_freq_in), d_sample_freq_out(
|
||||
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
|
||||
if (d_sample_freq_in >= d_sample_freq_out)
|
||||
{
|
||||
d_phase_step = (unsigned int)floor((double)4294967296.0
|
||||
* sample_freq_out / sample_freq_in);
|
||||
d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_out / sample_freq_in));
|
||||
}
|
||||
else
|
||||
{
|
||||
d_phase_step = (unsigned int)floor((double)4294967296.0
|
||||
* sample_freq_in / sample_freq_out);
|
||||
d_phase_step = static_cast<unsigned int>(floor(two_32 * sample_freq_in / sample_freq_out));
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
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);
|
||||
unsigned ninputs = ninput_items_required.size();
|
||||
|
||||
|
@ -28,16 +28,18 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "signal_generator_c.h"
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <volk/volk.h>
|
||||
#include "signal_generator_c.h"
|
||||
#include "gps_sdr_signal_processing.h"
|
||||
#include "galileo_e1_signal_processing.h"
|
||||
#include "nco_lib.h"
|
||||
#include "galileo_e5_signal_processing.h"
|
||||
#include "Galileo_E5a.h"
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
|
||||
/*
|
||||
* Create a new instance of signal_generator_c and return
|
||||
* 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) :
|
||||
|
||||
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),
|
||||
system_(system),
|
||||
PRN_(PRN),
|
||||
@ -74,7 +76,7 @@ signal_generator_c::signal_generator_c (std::vector<std::string> signal1, std::v
|
||||
fs_in_(fs_in),
|
||||
num_sats_(PRN.size()),
|
||||
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();
|
||||
generate_codes();
|
||||
@ -84,10 +86,10 @@ void signal_generator_c::init()
|
||||
{
|
||||
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
|
||||
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++)
|
||||
{
|
||||
@ -95,24 +97,23 @@ void signal_generator_c::init()
|
||||
current_data_bit_int_.push_back(1);
|
||||
current_data_bits_.push_back(gr_complex(1, 0));
|
||||
ms_counter_.push_back(0);
|
||||
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));
|
||||
|
||||
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));
|
||||
|
||||
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)));
|
||||
|
||||
num_of_codes_per_vector_.push_back(gallileo_signal ? 4*(int)Galileo_E1_C_SECONDARY_CODE_LENGTH : 1);
|
||||
data_bit_duration_ms_.push_back(1e3/GPS_CA_TELEMETRY_RATE_BITS_SECOND);
|
||||
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);
|
||||
}
|
||||
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;
|
||||
samples_per_code_.push_back(round((float)fs_in_ / (Galileo_E5a_CODE_CHIP_RATE_HZ
|
||||
int codelen = static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS);
|
||||
samples_per_code_.push_back(round(static_cast<float>(fs_in_) / (Galileo_E5a_CODE_CHIP_RATE_HZ
|
||||
/ codelen)));
|
||||
num_of_codes_per_vector_.push_back(1);
|
||||
|
||||
@ -120,38 +121,18 @@ void signal_generator_c::init()
|
||||
}
|
||||
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)));
|
||||
|
||||
num_of_codes_per_vector_.push_back((int)Galileo_E1_C_SECONDARY_CODE_LENGTH);
|
||||
data_bit_duration_ms_.push_back(1e3/Galileo_E1_B_SYMBOL_RATE_BPS);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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()
|
||||
{
|
||||
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++)
|
||||
{
|
||||
if (posix_memalign((void**)&(sampled_code_data_[sat]), 16,
|
||||
vector_length_ * sizeof(gr_complex)) == 0){};
|
||||
sampled_code_data_[sat] = static_cast<gr_complex*>(std::malloc(vector_length_ * sizeof(gr_complex)));
|
||||
|
||||
gr_complex code[64000];//[samples_per_code_[sat]];
|
||||
gr_complex code[64000]; //[samples_per_code_[sat]];
|
||||
|
||||
if (system_[sat] == "G")
|
||||
{
|
||||
// Generate one code-period of 1C signal
|
||||
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.
|
||||
if (noise_flag_)
|
||||
@ -182,25 +162,19 @@ void signal_generator_c::generate_codes()
|
||||
// Concatenate "num_of_codes_per_vector_" codes
|
||||
for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++)
|
||||
{
|
||||
memcpy(&(sampled_code_data_[sat][i*samples_per_code_[sat]]),
|
||||
code, sizeof(gr_complex)*samples_per_code_[sat]);
|
||||
memcpy(&(sampled_code_data_[sat][i * samples_per_code_[sat]]),
|
||||
code, sizeof(gr_complex) * samples_per_code_[sat]);
|
||||
}
|
||||
}
|
||||
else if (system_[sat] == "E")
|
||||
{
|
||||
if(signal_[sat].at(0)=='5')
|
||||
if(signal_[sat].at(0) == '5')
|
||||
{
|
||||
char signal[3];
|
||||
strcpy(signal,"5X");
|
||||
|
||||
if (posix_memalign((void**)&(sampled_code_data_[sat]), 16,
|
||||
vector_length_ * sizeof(gr_complex)) == 0){};
|
||||
|
||||
strcpy(signal, "5X");
|
||||
|
||||
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
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -219,7 +192,7 @@ void signal_generator_c::generate_codes()
|
||||
strcpy(signal, "1B");
|
||||
|
||||
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.
|
||||
if (noise_flag_)
|
||||
@ -233,18 +206,17 @@ void signal_generator_c::generate_codes()
|
||||
// Concatenate "num_of_codes_per_vector_" codes
|
||||
for (unsigned int i = 0; i < num_of_codes_per_vector_[sat]; i++)
|
||||
{
|
||||
memcpy(&(sampled_code_data_[sat][i*samples_per_code_[sat]]),
|
||||
code, sizeof(gr_complex)*samples_per_code_[sat]);
|
||||
memcpy(&(sampled_code_data_[sat][i * samples_per_code_[sat]]),
|
||||
code, sizeof(gr_complex) * samples_per_code_[sat]);
|
||||
}
|
||||
|
||||
// Generate E1C signal (25 code-periods, with secondary code)
|
||||
if (posix_memalign((void**)&(sampled_code_pilot_[sat]), 16,
|
||||
vector_length_ * sizeof(gr_complex)) == 0){};
|
||||
sampled_code_pilot_[sat] = static_cast<gr_complex*>(std::malloc(vector_length_ * sizeof(gr_complex)));
|
||||
|
||||
strcpy(signal, "1C");
|
||||
|
||||
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.
|
||||
if (noise_flag_)
|
||||
@ -260,20 +232,18 @@ void signal_generator_c::generate_codes()
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Our virtual destructor.
|
||||
*/
|
||||
|
||||
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]);
|
||||
if (system_[sat] == "E" && signal_[sat].at(0)!='5')
|
||||
std::free(sampled_code_data_[sat]);
|
||||
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_;
|
||||
}
|
||||
|
||||
@ -298,7 +268,7 @@ gr_vector_void_star &output_items)
|
||||
|
||||
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);
|
||||
start_phase_rad_[sat] += vector_length_ * phase_step_rad;
|
||||
|
||||
@ -306,7 +276,7 @@ gr_vector_void_star &output_items)
|
||||
|
||||
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;
|
||||
|
||||
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_)
|
||||
{
|
||||
// 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++)
|
||||
@ -333,7 +303,7 @@ gr_vector_void_star &output_items)
|
||||
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];
|
||||
}
|
||||
}
|
||||
@ -343,12 +313,12 @@ gr_vector_void_star &output_items)
|
||||
if(signal_[sat].at(0)=='5')
|
||||
{
|
||||
// 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)
|
||||
* samples_per_code_[sat] / codelen;
|
||||
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]) )
|
||||
* complex_phase_[out_idx];
|
||||
out_idx++;
|
||||
@ -360,22 +330,21 @@ gr_vector_void_star &output_items)
|
||||
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);
|
||||
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++)
|
||||
{
|
||||
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]) )
|
||||
* complex_phase_[out_idx];
|
||||
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]) )
|
||||
* complex_phase_[out_idx];
|
||||
out_idx++;
|
||||
}
|
||||
|
||||
}
|
||||
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;
|
||||
|
||||
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++)
|
||||
{
|
||||
out[out_idx] += (sampled_code_data_[sat][out_idx] * current_data_bits_[sat]
|
||||
- sampled_code_pilot_[sat][out_idx])
|
||||
* complex_phase_[out_idx];
|
||||
- sampled_code_pilot_[sat][out_idx]) * complex_phase_[out_idx];
|
||||
out_idx++;
|
||||
}
|
||||
|
||||
if (ms_counter_[sat] == 0 && data_flag_)
|
||||
{
|
||||
// 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++)
|
||||
{
|
||||
out[out_idx] += (sampled_code_data_[sat][out_idx] * current_data_bits_[sat]
|
||||
- sampled_code_pilot_[sat][out_idx])
|
||||
* complex_phase_[out_idx];
|
||||
- sampled_code_pilot_[sat][out_idx])
|
||||
* complex_phase_[out_idx];
|
||||
out_idx++;
|
||||
}
|
||||
|
||||
|
||||
ms_counter_[sat] = (ms_counter_[sat] + (int)round(1e3*Galileo_E1_CODE_PERIOD))
|
||||
% data_bit_duration_ms_[sat];
|
||||
|
||||
ms_counter_[sat] = (ms_counter_[sat] + static_cast<int>(round(1e3 * Galileo_E1_CODE_PERIOD))) % data_bit_duration_ms_[sat];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -68,6 +68,8 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
dump_ = configuration->property(role + ".dump", false);
|
||||
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
|
||||
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)
|
||||
{
|
||||
@ -147,9 +149,11 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
|
||||
CHECK(samples_ > 0) << "File does not contain enough samples to process.";
|
||||
double signal_duration_s;
|
||||
signal_duration_s = (double)samples_ * ( 1 /(double)sampling_frequency_);
|
||||
#ifdef ARCH_64BITS
|
||||
signal_duration_s /= 2;
|
||||
#endif
|
||||
|
||||
if ((item_type_.compare("gr_complex") != 0) && (IF < 1e6) ) // if IF < BW/2, signal is complex (interleaved)
|
||||
{
|
||||
signal_duration_s /= 2;
|
||||
}
|
||||
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;
|
||||
|
||||
|
@ -456,7 +456,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items, gr_vector
|
||||
{
|
||||
d_CRC_error_counter = 0;
|
||||
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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
if (d_nav.flag_TOW_2 == true)
|
||||
{
|
||||
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;
|
||||
}
|
||||
if (d_nav.flag_TOW_3 == true)
|
||||
{
|
||||
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;
|
||||
}
|
||||
if (d_nav.flag_TOW_4 == true)
|
||||
{
|
||||
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;
|
||||
}
|
||||
else
|
||||
|
@ -83,7 +83,7 @@ galileo_e1_dll_pll_veml_make_tracking_cc(
|
||||
void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items,
|
||||
gr_vector_int &ninput_items_required)
|
||||
{
|
||||
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call
|
||||
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
|
||||
// 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,
|
||||
* 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
|
||||
*/
|
||||
|
||||
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());
|
||||
d_very_early_code = static_cast<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 = static_cast<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()));
|
||||
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()));
|
||||
|
||||
// correlator outputs (scalar)
|
||||
|
||||
d_Very_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment());
|
||||
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_Very_Late=(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 = static_cast<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 = static_cast<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()));
|
||||
|
||||
//--- Initializations ------------------------------
|
||||
// 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)
|
||||
d_rem_code_phase_samples = 0.0;
|
||||
// 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_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
|
||||
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;
|
||||
|
||||
systemName["E"] = std::string("Galileo");
|
||||
*d_Very_Early=gr_complex(0,0);
|
||||
*d_Early=gr_complex(0,0);
|
||||
*d_Prompt=gr_complex(0,0);
|
||||
*d_Late=gr_complex(0,0);
|
||||
*d_Very_Late=gr_complex(0,0);
|
||||
*d_Very_Early = gr_complex(0,0);
|
||||
*d_Early = gr_complex(0,0);
|
||||
*d_Prompt = gr_complex(0,0);
|
||||
*d_Late = gr_complex(0,0);
|
||||
*d_Very_Late = gr_complex(0,0);
|
||||
}
|
||||
|
||||
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,
|
||||
false,
|
||||
d_acquisition_gnss_synchro->PRN,
|
||||
2*Galileo_E1_CODE_CHIP_RATE_HZ,
|
||||
2 * Galileo_E1_CODE_CHIP_RATE_HZ,
|
||||
0);
|
||||
// Fill head and tail
|
||||
d_ca_code[0] = d_ca_code[(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[(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[0] = d_ca_code[static_cast<int>(2 * Galileo_E1_B_CODE_LENGTH_CHIPS)];
|
||||
d_ca_code[1] = d_ca_code[static_cast<int>(2 * Galileo_E1_B_CODE_LENGTH_CHIPS + 1)];
|
||||
d_ca_code[static_cast<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 + 3)] = d_ca_code[3];
|
||||
|
||||
d_carrier_lock_fail_counter = 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;
|
||||
float rem_code_phase_half_chips;
|
||||
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_half_chips;
|
||||
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;
|
||||
|
||||
// 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_half_chips = (2.0*(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 * 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);
|
||||
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);
|
||||
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++)
|
||||
{
|
||||
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];
|
||||
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_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_very_late_code, &d_very_early_code[2*very_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_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));
|
||||
}
|
||||
|
||||
void galileo_e1_dll_pll_veml_tracking_cc::update_local_carrier()
|
||||
{
|
||||
float phase_rad, phase_step_rad;
|
||||
// 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
|
||||
phase_rad = d_rem_carr_phase_rad;
|
||||
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_Late);
|
||||
volk_free(d_Very_Late);
|
||||
volk_free(d_ca_code);
|
||||
|
||||
delete[] d_ca_code;
|
||||
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;
|
||||
int acq_to_trk_delay_samples;
|
||||
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);
|
||||
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
|
||||
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_Prompt,
|
||||
d_Late,
|
||||
d_Very_Late,
|
||||
is_unaligned());
|
||||
d_Very_Late);
|
||||
|
||||
// ################## PLL ##########################################################
|
||||
// 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
|
||||
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
|
||||
// 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
|
||||
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=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;
|
||||
|
||||
// ################## 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 K_blk_samples;
|
||||
// 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_samples = T_prn_seconds * (double)d_fs_in;
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (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 * static_cast<double>(d_fs_in);
|
||||
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
|
||||
|
||||
@ -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 ##########
|
||||
|
||||
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
|
||||
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
|
||||
|
||||
// Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!)
|
||||
//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
|
||||
//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??)
|
||||
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
|
||||
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
|
||||
current_synchro_data.Code_phase_secs = 0;
|
||||
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
|
||||
current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
|
||||
current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
|
||||
current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
|
||||
*out[0] = current_synchro_data;
|
||||
|
||||
// ########## DEBUG OUTPUT
|
||||
@ -531,35 +522,35 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
|
||||
try
|
||||
{
|
||||
// Dump correlators output
|
||||
d_dump_file.write((char*)&tmp_VE, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_L, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_VL, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_VE), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_VL), sizeof(float));
|
||||
// PROMPT I and Q (to analyze navigation symbols)
|
||||
d_dump_file.write((char*)&prompt_I, sizeof(float));
|
||||
d_dump_file.write((char*)&prompt_Q, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
|
||||
// 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
|
||||
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
|
||||
d_dump_file.write((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_carrier_doppler_hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
|
||||
//PLL commands
|
||||
d_dump_file.write((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_hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
|
||||
//DLL commands
|
||||
d_dump_file.write((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_chips), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
|
||||
// CN0 and carrier lock test
|
||||
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
|
||||
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
|
||||
// AUX vars (for debug purposes)
|
||||
tmp_float = d_rem_code_phase_samples;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_double=(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_float), sizeof(float));
|
||||
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
}
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
|
@ -127,29 +127,22 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
|
||||
|
||||
// Initialization of local code replica
|
||||
// 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,
|
||||
* 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
|
||||
*/
|
||||
|
||||
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());
|
||||
d_very_early_code = static_cast<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 = static_cast<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()));
|
||||
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()));
|
||||
|
||||
// correlator outputs (scalar)
|
||||
|
||||
d_Very_Early=(gr_complex*)volk_malloc(sizeof(gr_complex),volk_get_alignment());
|
||||
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_Very_Late=(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 = static_cast<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 = static_cast<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 ------------------------------
|
||||
// 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_Late);
|
||||
volk_free(d_Very_Late);
|
||||
volk_free(d_ca_code);
|
||||
|
||||
delete[] d_ca_code;
|
||||
delete[] d_Prompt_buffer;
|
||||
|
||||
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_Prompt,
|
||||
d_Late,
|
||||
d_Very_Late,
|
||||
is_unaligned());
|
||||
d_Very_Late);
|
||||
|
||||
// ################## TCP CONNECTOR ##########################################################
|
||||
//! Variable used for control
|
||||
|
@ -77,7 +77,7 @@ galileo_e5a_dll_pll_make_tracking_cc(
|
||||
float early_late_space_chips)
|
||||
{
|
||||
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,
|
||||
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(
|
||||
@ -130,29 +130,20 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc(
|
||||
|
||||
// Initialization of local code replica
|
||||
// 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_codeI = 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[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) + 2];
|
||||
|
||||
/* 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
|
||||
*/
|
||||
d_early_code = static_cast<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()));
|
||||
d_prompt_code = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
|
||||
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());
|
||||
d_late_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_prompt_data_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)
|
||||
|
||||
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());
|
||||
// correlator outputs (complex number)
|
||||
d_Early = gr_complex(0, 0);
|
||||
d_Prompt = gr_complex(0, 0);
|
||||
d_Late = gr_complex(0, 0);
|
||||
d_Prompt_data = gr_complex(0, 0);
|
||||
|
||||
//--- Perform initializations ------------------------------
|
||||
// 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;
|
||||
// define residual carrier phase
|
||||
d_rem_carr_phase_rad = 0.0;
|
||||
|
||||
//Filter error vars
|
||||
d_code_error_filt_secs = 0.0;
|
||||
|
||||
|
||||
// sample synchronization
|
||||
d_sample_counter = 0;
|
||||
d_acq_sample_stamp = 0;
|
||||
|
||||
d_last_seg = 0;
|
||||
d_first_transition = false;
|
||||
|
||||
d_secondary_lock=false;
|
||||
d_secondary_delay=0;
|
||||
d_secondary_lock = false;
|
||||
d_secondary_delay = 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
|
||||
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_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["C"] = std::string("Compass");
|
||||
|
||||
}
|
||||
|
||||
|
||||
Galileo_E5a_Dll_Pll_Tracking_cc::~Galileo_E5a_Dll_Pll_Tracking_cc ()
|
||||
{
|
||||
d_dump_file.close();
|
||||
|
||||
|
||||
volk_free(d_prompt_code);
|
||||
volk_free(d_late_code);
|
||||
volk_free(d_early_code);
|
||||
volk_free(d_carr_sign);
|
||||
|
||||
volk_free(d_prompt_data_code);
|
||||
volk_free(d_Prompt_data);
|
||||
|
||||
delete[] d_codeQ;
|
||||
delete[] d_codeI;
|
||||
delete[] d_codeQ;
|
||||
delete[] d_Prompt_buffer;
|
||||
}
|
||||
|
||||
@ -224,9 +203,9 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::start_tracking()
|
||||
|
||||
long int acq_trk_diff_samples;
|
||||
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;
|
||||
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
|
||||
// Fd=(C/(C+Vr))*F
|
||||
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;
|
||||
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_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);
|
||||
|
||||
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;
|
||||
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
|
||||
float N_prn_diff;
|
||||
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
|
||||
float corrected_acq_phase_samples, delay_correction_samples;
|
||||
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * (float)d_fs_in), T_prn_true_samples);
|
||||
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)
|
||||
{
|
||||
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];
|
||||
strcpy(sig,"5Q");
|
||||
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[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 1] = d_codeQ[1];
|
||||
d_codeQ[0] = d_codeQ[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS)];
|
||||
d_codeQ[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) + 1] = d_codeQ[1];
|
||||
|
||||
strcpy(sig,"5I");
|
||||
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[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 1] = d_codeI[1];
|
||||
d_codeI[0] = d_codeI[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS)];
|
||||
d_codeI[static_cast<int>(Galileo_E5a_CODE_LENGTH_CHIPS) + 1] = d_codeI[1];
|
||||
|
||||
d_carrier_lock_fail_counter = 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
|
||||
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')
|
||||
{
|
||||
sec_code_signed[i]=1;
|
||||
sec_code_signed[i] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sec_code_signed[i]=-1;
|
||||
sec_code_signed[i] = -1;
|
||||
}
|
||||
}
|
||||
// 2. Transform buffer to 1 and -1
|
||||
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)
|
||||
{
|
||||
in_corr[i]=1;
|
||||
in_corr[i] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
in_corr[i]=-1;
|
||||
in_corr[i] = -1;
|
||||
}
|
||||
}
|
||||
// 3. Serial search
|
||||
int out_corr;
|
||||
int current_best_=0;
|
||||
for (unsigned int i=0; i<Galileo_E5a_Q_SECONDARY_CODE_LENGTH; i++)
|
||||
int current_best_ = 0;
|
||||
for (unsigned int i = 0; i < Galileo_E5a_Q_SECONDARY_CODE_LENGTH; i++)
|
||||
{
|
||||
out_corr=0;
|
||||
for (unsigned int j=0; j<CN0_ESTIMATION_SAMPLES; j++)
|
||||
out_corr = 0;
|
||||
for (unsigned int j = 0; j < CN0_ESTIMATION_SAMPLES; j++)
|
||||
{
|
||||
//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_)
|
||||
{
|
||||
current_best_ = abs(out_corr);
|
||||
d_secondary_delay=i;
|
||||
d_secondary_delay = i;
|
||||
}
|
||||
}
|
||||
if (current_best_ == CN0_ESTIMATION_SAMPLES) // all bits correlate
|
||||
{
|
||||
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;
|
||||
int associated_chip_index;
|
||||
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;
|
||||
int early_late_spc_samples;
|
||||
int epl_loop_length_samples;
|
||||
|
||||
// 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);
|
||||
tcode_chips = -rem_code_phase_chips;
|
||||
|
||||
// Alternative EPL code generation (40% of speed improvement!)
|
||||
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++)
|
||||
{
|
||||
@ -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];
|
||||
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_late_code,&d_early_code[early_late_spc_samples*2],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));
|
||||
|
||||
}
|
||||
|
||||
@ -389,7 +368,7 @@ void Galileo_E5a_Dll_Pll_Tracking_cc::update_local_carrier()
|
||||
{
|
||||
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;
|
||||
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;
|
||||
}
|
||||
}
|
||||
*d_Early = gr_complex(0,0);
|
||||
*d_Prompt = gr_complex(0,0);
|
||||
*d_Late = gr_complex(0,0);
|
||||
*d_Prompt_data = gr_complex(0,0);
|
||||
d_Early = gr_complex(0,0);
|
||||
d_Prompt = gr_complex(0,0);
|
||||
d_Late = gr_complex(0,0);
|
||||
d_Prompt_data = gr_complex(0,0);
|
||||
|
||||
*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;
|
||||
int acq_to_trk_delay_samples;
|
||||
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);
|
||||
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.
|
||||
|
||||
// make an output to not stop the rest of the processing blocks
|
||||
current_synchro_data.Prompt_I = 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.Code_phase_secs = 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_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_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_I = gr_complex((Galileo_E5a_I_SECONDARY_CODE.at(d_secondary_delay % Galileo_E5a_I_SECONDARY_CODE_LENGTH) == '0' ? -1 : 1), 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
sec_sign_Q = gr_complex(1.0,0.0);
|
||||
sec_sign_I = gr_complex(1.0,0.0);
|
||||
sec_sign_Q = gr_complex(1.0, 0.0);
|
||||
sec_sign_I = gr_complex(1.0, 0.0);
|
||||
}
|
||||
// Reset integration counter
|
||||
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_carrier();
|
||||
// Reset accumulated values
|
||||
*d_Early = gr_complex(0,0);
|
||||
*d_Prompt = gr_complex(0,0);
|
||||
*d_Late = gr_complex(0,0);
|
||||
d_Early = gr_complex(0,0);
|
||||
d_Prompt = gr_complex(0,0);
|
||||
d_Late = gr_complex(0,0);
|
||||
}
|
||||
gr_complex single_early;
|
||||
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_prompt,
|
||||
&single_late,
|
||||
d_Prompt_data,
|
||||
is_unaligned());
|
||||
&d_Prompt_data);
|
||||
|
||||
// Accumulate results (coherent integration since there are no bit transitions in pilot signal)
|
||||
*d_Early += single_early * sec_sign_Q;
|
||||
*d_Prompt += single_prompt * sec_sign_Q;
|
||||
*d_Late += single_late * sec_sign_Q;
|
||||
*d_Prompt_data *= sec_sign_I;
|
||||
d_Early += single_early * sec_sign_Q;
|
||||
d_Prompt += single_prompt * sec_sign_Q;
|
||||
d_Late += single_late * sec_sign_Q;
|
||||
d_Prompt_data *= sec_sign_I;
|
||||
d_integration_counter++;
|
||||
|
||||
// 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];
|
||||
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
|
||||
current_synchro_data.Prompt_I = 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.Code_phase_secs = 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)
|
||||
{
|
||||
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
|
||||
{
|
||||
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
|
||||
@ -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);
|
||||
}
|
||||
//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
|
||||
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);
|
||||
|
||||
// ################## DLL ##########################################################
|
||||
if (d_integration_counter == d_current_ti_ms)
|
||||
{
|
||||
// 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_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
|
||||
//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;
|
||||
|
||||
@ -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
|
||||
double T_chip_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 K_blk_samples;
|
||||
// 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_samples = T_prn_seconds * (double)d_fs_in;
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + d_code_error_filt_secs*(float)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 * static_cast<double>(d_fs_in);
|
||||
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
|
||||
|
||||
@ -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)
|
||||
{
|
||||
// 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++;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
|
||||
d_Prompt_buffer[d_cn0_estimation_counter] = d_Prompt;
|
||||
// ATTEMPT SECONDARY CODE ACQUISITION
|
||||
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 ==========================================
|
||||
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_code_loop_filter.initialize();
|
||||
// d_carrier_loop_filter.initialize();
|
||||
d_code_loop_filter.set_DLL_BW(d_dll_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;
|
||||
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())
|
||||
{
|
||||
d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
|
||||
}
|
||||
delete cmf;
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
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;
|
||||
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())
|
||||
{
|
||||
d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
|
||||
}
|
||||
delete cmf;
|
||||
d_carrier_lock_fail_counter = 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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
@ -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.
|
||||
if (d_secondary_lock && d_first_transition)
|
||||
{
|
||||
current_synchro_data.Prompt_I = (double)((*d_Prompt_data).real());
|
||||
current_synchro_data.Prompt_Q = (double)((*d_Prompt_data).imag());
|
||||
current_synchro_data.Prompt_I = static_cast<double>((d_Prompt_data).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((d_Prompt_data).imag());
|
||||
// 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
|
||||
current_synchro_data.Code_phase_secs = 0;
|
||||
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
|
||||
current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
|
||||
current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
|
||||
current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
// make an output to not stop the rest of the processing blocks
|
||||
current_synchro_data.Prompt_I = 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.Code_phase_secs = 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 tmp_float;
|
||||
double tmp_double;
|
||||
prompt_I = (*d_Prompt_data).real();
|
||||
prompt_Q = (*d_Prompt_data).imag();
|
||||
prompt_I = (d_Prompt_data).real();
|
||||
prompt_Q = (d_Prompt_data).imag();
|
||||
if (d_integration_counter == d_current_ti_ms)
|
||||
{
|
||||
tmp_E = std::abs<float>(*d_Early);
|
||||
tmp_P = std::abs<float>(*d_Prompt);
|
||||
tmp_L = std::abs<float>(*d_Late);
|
||||
tmp_E = std::abs<float>(d_Early);
|
||||
tmp_P = std::abs<float>(d_Prompt);
|
||||
tmp_L = std::abs<float>(d_Late);
|
||||
}
|
||||
try
|
||||
{
|
||||
// EPR
|
||||
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_L, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
|
||||
// PROMPT I and Q (to analyze navigation symbols)
|
||||
d_dump_file.write((char*)&prompt_I, sizeof(float));
|
||||
d_dump_file.write((char*)&prompt_Q, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
|
||||
// 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
|
||||
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
|
||||
d_dump_file.write((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_carrier_doppler_hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
|
||||
|
||||
//PLL commands
|
||||
d_dump_file.write((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_hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
|
||||
|
||||
//DLL commands
|
||||
d_dump_file.write((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_chips), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
|
||||
|
||||
// CN0 and carrier lock test
|
||||
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
|
||||
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
|
||||
|
||||
// AUX vars (for debug purposes)
|
||||
tmp_float = d_rem_code_phase_samples;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_double=(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_float), sizeof(float));
|
||||
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
}
|
||||
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
|
||||
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
|
||||
|
@ -151,10 +151,10 @@ private:
|
||||
gr_complex* d_prompt_data_code;
|
||||
gr_complex* d_carr_sign;
|
||||
|
||||
gr_complex *d_Early;
|
||||
gr_complex *d_Prompt;
|
||||
gr_complex *d_Late;
|
||||
gr_complex *d_Prompt_data;
|
||||
gr_complex d_Early;
|
||||
gr_complex d_Prompt;
|
||||
gr_complex d_Late;
|
||||
gr_complex d_Prompt_data;
|
||||
|
||||
float tmp_E;
|
||||
float tmp_P;
|
||||
|
@ -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)
|
||||
{
|
||||
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_if_freq = (double)if_freq;
|
||||
d_fs_in = (double)fs_in;
|
||||
d_if_freq = static_cast<double>(if_freq);
|
||||
d_fs_in = static_cast<double>(fs_in);
|
||||
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;
|
||||
|
||||
// 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);
|
||||
|
||||
// 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
|
||||
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_early_code = static_cast<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 = static_cast<gr_complex*>(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
// 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)
|
||||
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_Early = static_cast<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 = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
// sample synchronization
|
||||
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_code_phase_samples = 0;
|
||||
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
|
||||
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;
|
||||
float acq_trk_diff_seconds;
|
||||
acq_trk_diff_samples = (long int)d_sample_counter - (long int)d_acq_sample_stamp;
|
||||
acq_trk_diff_seconds = (double)acq_trk_diff_samples / d_fs_in;
|
||||
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp);
|
||||
acq_trk_diff_seconds = static_cast<double>(acq_trk_diff_samples) / d_fs_in;
|
||||
//doppler effect
|
||||
// Fd=(C/(C+Vr))*F
|
||||
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)
|
||||
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[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
|
||||
d_ca_code[0] = d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS)];
|
||||
d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1];
|
||||
|
||||
d_carrier_lock_fail_counter = 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 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;
|
||||
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
|
||||
// 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()
|
||||
{
|
||||
d_dump_file.close();
|
||||
delete[] d_ca_code;
|
||||
|
||||
volk_free(d_ca_code);
|
||||
volk_free(d_prompt_code);
|
||||
volk_free(d_late_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_Prompt);
|
||||
volk_free(d_Late);
|
||||
|
||||
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;
|
||||
double acq_trk_shif_correction_samples;
|
||||
int acq_to_trk_delay_samples;
|
||||
acq_to_trk_delay_samples = d_sample_counter-d_acq_sample_stamp;
|
||||
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((double)acq_to_trk_delay_samples, (double)d_current_prn_length_samples);
|
||||
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
|
||||
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);
|
||||
// /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
|
||||
@ -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
|
||||
current_synchro_data.Prompt_I = 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.Code_phase_secs = 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_Early,
|
||||
d_Prompt,
|
||||
d_Late,
|
||||
is_unaligned());
|
||||
d_Late);
|
||||
// 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)
|
||||
{
|
||||
@ -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
|
||||
current_synchro_data.Prompt_I = 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.Code_phase_secs = 0.0;
|
||||
current_synchro_data.CN0_dB_hz = 0.0;
|
||||
current_synchro_data.Flag_valid_tracking = false;
|
||||
|
||||
*out[0] =current_synchro_data;
|
||||
*out[0] = current_synchro_data;
|
||||
|
||||
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);
|
||||
|
||||
//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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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;
|
||||
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_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_samples = T_prn_seconds * d_fs_in;
|
||||
|
||||
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;
|
||||
|
||||
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
|
||||
|
||||
// ########### Output the tracking data to navigation and PVT ##########
|
||||
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
|
||||
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
|
||||
// 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_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
|
||||
// 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;
|
||||
|
@ -132,7 +132,7 @@ private:
|
||||
void CN0_estimation_and_lock_detectors();
|
||||
|
||||
// class private vars
|
||||
Gnss_Synchro *d_acquisition_gnss_synchro;
|
||||
Gnss_Synchro* d_acquisition_gnss_synchro;
|
||||
boost::shared_ptr<gr::msg_queue> d_queue;
|
||||
concurrent_queue<int> *d_channel_internal_queue;
|
||||
unsigned int d_vector_length;
|
||||
|
@ -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_fs_in = fs_in;
|
||||
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;
|
||||
|
||||
// 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
|
||||
// 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
|
||||
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_early_code = static_cast<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 = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
// 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)
|
||||
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_Early = static_cast<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 = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
//--- Perform initializations ------------------------------
|
||||
// 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_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
|
||||
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;
|
||||
|
||||
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;
|
||||
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;
|
||||
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
|
||||
// Fd=(C/(C+Vr))*F
|
||||
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
|
||||
float T_chip_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;
|
||||
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_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);
|
||||
|
||||
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
|
||||
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
|
||||
float T_prn_diff_seconds;
|
||||
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
|
||||
float N_prn_diff;
|
||||
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
|
||||
float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
|
||||
float 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 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)
|
||||
{
|
||||
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)
|
||||
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[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
|
||||
d_ca_code[0] = d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS)];
|
||||
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.
|
||||
// No code doppler correction
|
||||
double tcode_chips;
|
||||
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;
|
||||
int early_late_spc_samples;
|
||||
int epl_loop_length_samples;
|
||||
|
||||
// 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;
|
||||
|
||||
// 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 rem_code_phase_chips;
|
||||
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;
|
||||
int early_late_spc_samples;
|
||||
int epl_loop_length_samples;
|
||||
|
||||
// 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);
|
||||
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()
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
int acq_to_trk_delay_samples;
|
||||
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);
|
||||
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
|
||||
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_Early,
|
||||
d_Prompt,
|
||||
d_Late,
|
||||
is_unaligned());
|
||||
d_Late);
|
||||
#else
|
||||
d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples,
|
||||
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_Early,
|
||||
d_Prompt,
|
||||
d_Late,
|
||||
is_unaligned());
|
||||
d_Late);
|
||||
#endif
|
||||
// ################## PLL ##########################################################
|
||||
// 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
|
||||
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
|
||||
// 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 K_blk_samples;
|
||||
// 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_samples = T_prn_seconds * (double)d_fs_in;
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (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 * static_cast<double>(d_fs_in);
|
||||
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
|
||||
|
||||
@ -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 ##########
|
||||
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
|
||||
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
|
||||
// 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_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 = (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
|
||||
// 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.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
|
||||
current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
|
||||
current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
|
||||
current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
|
||||
*out[0] = current_synchro_data;
|
||||
|
||||
// ########## DEBUG OUTPUT
|
||||
|
@ -82,7 +82,7 @@ gps_l1_ca_dll_pll_make_tracking_cc(
|
||||
void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items,
|
||||
gr_vector_int &ninput_items_required)
|
||||
{
|
||||
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call
|
||||
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
|
||||
// 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
|
||||
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_early_code = static_cast<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 = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
// 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)
|
||||
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_Early = static_cast<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 = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
|
||||
//--- 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_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
|
||||
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;
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
|
||||
@ -184,13 +176,12 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
|
||||
|
||||
long int acq_trk_diff_samples;
|
||||
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;
|
||||
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
|
||||
// Fd=(C/(C+Vr))*F
|
||||
float radial_velocity;
|
||||
radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
|
||||
float radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
|
||||
// new chip and prn sequence periods based on acq Doppler
|
||||
float T_chip_mod_seconds;
|
||||
float T_prn_mod_seconds;
|
||||
@ -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;
|
||||
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_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);
|
||||
|
||||
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
|
||||
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
|
||||
float T_prn_diff_seconds;
|
||||
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
|
||||
float N_prn_diff;
|
||||
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
|
||||
float T_prn_true_samples = T_prn_true_seconds * static_cast<float>(d_fs_in);
|
||||
float 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 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)
|
||||
{
|
||||
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)
|
||||
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[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
|
||||
d_ca_code[0] = d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS)];
|
||||
d_ca_code[static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) + 1] = d_ca_code[1];
|
||||
|
||||
d_carrier_lock_fail_counter = 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 rem_code_phase_chips;
|
||||
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;
|
||||
int early_late_spc_samples;
|
||||
int epl_loop_length_samples;
|
||||
|
||||
// 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);
|
||||
tcode_chips = -rem_code_phase_chips;
|
||||
|
||||
// Alternative EPL code generation (40% of speed improvement!)
|
||||
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++)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
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_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));
|
||||
}
|
||||
|
||||
|
||||
@ -294,7 +283,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_carrier()
|
||||
{
|
||||
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;
|
||||
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_Prompt);
|
||||
volk_free(d_Late);
|
||||
volk_free(d_ca_code);
|
||||
|
||||
delete[] d_ca_code;
|
||||
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;
|
||||
int acq_to_trk_delay_samples;
|
||||
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);
|
||||
// /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_pull_in = false;
|
||||
//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_Early,
|
||||
d_Prompt,
|
||||
d_Late,
|
||||
is_unaligned());
|
||||
d_Late);
|
||||
|
||||
// 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)
|
||||
@ -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
|
||||
current_synchro_data.Prompt_I = 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.Code_phase_secs = 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 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
|
||||
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
|
||||
// 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 K_blk_samples;
|
||||
// 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_samples = T_prn_seconds * (double)d_fs_in;
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (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 * static_cast<double>(d_fs_in);
|
||||
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
|
||||
|
||||
@ -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 ##########
|
||||
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
|
||||
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
|
||||
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
|
||||
|
||||
// Tracking_timestamp_secs is aligned with the NEXT PRN start sample (Hybridization problem!)
|
||||
//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
|
||||
//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??)
|
||||
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
|
||||
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
|
||||
current_synchro_data.Code_phase_secs = 0;
|
||||
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
|
||||
current_synchro_data.Carrier_phase_rads = static_cast<double>(d_acc_carrier_phase_rad);
|
||||
current_synchro_data.Carrier_Doppler_hz = static_cast<double>(d_carrier_doppler_hz);
|
||||
current_synchro_data.CN0_dB_hz = static_cast<double>(d_CN0_SNV_dB_Hz);
|
||||
*out[0] = current_synchro_data;
|
||||
|
||||
// ########## DEBUG OUTPUT
|
||||
@ -569,39 +557,39 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
|
||||
try
|
||||
{
|
||||
// EPR
|
||||
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_L, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
|
||||
// PROMPT I and Q (to analyze navigation symbols)
|
||||
d_dump_file.write((char*)&prompt_I, sizeof(float));
|
||||
d_dump_file.write((char*)&prompt_Q, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
|
||||
// PRN start sample stamp
|
||||
//tmp_float=(float)d_sample_counter;
|
||||
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
|
||||
// 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
|
||||
d_dump_file.write((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_carrier_doppler_hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(float));
|
||||
|
||||
//PLL commands
|
||||
d_dump_file.write((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_hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&carr_error_filt_hz), sizeof(float));
|
||||
|
||||
//DLL commands
|
||||
d_dump_file.write((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_chips), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(float));
|
||||
|
||||
// CN0 and carrier lock test
|
||||
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
|
||||
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(float));
|
||||
|
||||
// AUX vars (for debug purposes)
|
||||
tmp_float = d_rem_code_phase_samples;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_double=(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_float), sizeof(float));
|
||||
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
}
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
|
@ -126,26 +126,21 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
|
||||
|
||||
// Initialization of local code replica
|
||||
// Get space for a vector with the C/A code replica sampled 1x/chip
|
||||
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2];
|
||||
d_carr_sign = new gr_complex[d_vector_length*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 = 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
|
||||
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_early_code = static_cast<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 = static_cast<gr_complex*>(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
// 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)
|
||||
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_Early = static_cast<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 = static_cast<gr_complex*>(volk_malloc(sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
//--- Perform initializations ------------------------------
|
||||
// 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_Prompt);
|
||||
volk_free(d_Late);
|
||||
volk_free(d_ca_code);
|
||||
|
||||
delete[] d_ca_code;
|
||||
delete[] d_Prompt_buffer;
|
||||
|
||||
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_Early,
|
||||
d_Prompt,
|
||||
d_Late,
|
||||
is_unaligned());
|
||||
d_Late);
|
||||
|
||||
// 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)
|
||||
|
@ -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* input_aligned;
|
||||
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
//todo: do something if posix_memalign fails
|
||||
if (posix_memalign((void**)&bb_signal, 16, signal_length_samples * sizeof(gr_complex)) == 0) {};
|
||||
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);
|
||||
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)
|
||||
{
|
||||
//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);
|
||||
//}
|
||||
volk_free(bb_signal);
|
||||
}
|
||||
|
||||
//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* input_aligned;
|
||||
//
|
||||
// //todo: do something if posix_memalign fails
|
||||
// if (posix_memalign((void**)&bb_signal, 16, integration_time * prn_length_samples * sizeof(gr_complex)) == 0) {};
|
||||
//
|
||||
// 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);
|
||||
// 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, integration_time * prn_length_samples);
|
||||
// volk_32fc_x2_dot_prod_32fc(E_out, bb_signal, E_code, integration_time * prn_length_samples);
|
||||
// volk_32fc_x2_dot_prod_32fc(P_out, bb_signal, P_code, integration_time * prn_length_samples);
|
||||
// volk_32fc_x2_dot_prod_32fc(L_out, bb_signal, L_code, integration_time * prn_length_samples);
|
||||
// // Vector of Prompts of I code
|
||||
// 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* input_aligned;
|
||||
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
|
||||
|
||||
bb_signal=(gr_complex*)volk_malloc(signal_length_samples * sizeof(gr_complex),volk_get_alignment());
|
||||
|
||||
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, 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_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);
|
||||
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(P_data_out, bb_signal, P_data_code, signal_length_samples);
|
||||
|
||||
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
|
||||
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);
|
||||
}
|
||||
#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 ()
|
||||
{}
|
||||
|
@ -57,10 +57,10 @@ class Correlator
|
||||
{
|
||||
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_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_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_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 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(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);
|
||||
// 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);
|
||||
Correlator();
|
||||
~Correlator();
|
||||
#ifndef USING_VOLK_CW_EPL_CORR
|
||||
@ -68,11 +68,7 @@ public:
|
||||
#endif
|
||||
|
||||
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);
|
||||
void cpu_arch_test_volk_32fc_x2_dot_prod_32fc_a();
|
||||
void cpu_arch_test_volk_32fc_x2_multiply_32fc_a();
|
||||
};
|
||||
#endif
|
||||
|
||||
|
@ -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
|
||||
* Satellite Systems receiver
|
||||
@ -16,7 +16,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -40,7 +40,7 @@ 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)
|
||||
{
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
@ -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);
|
||||
|
@ -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
|
||||
* Satellite Systems receiver
|
||||
@ -16,7 +16,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -45,12 +45,12 @@ public:
|
||||
StringConverter();
|
||||
virtual ~StringConverter();
|
||||
|
||||
bool convert(std::string value, bool default_value);
|
||||
long convert(std::string value, long default_value);
|
||||
int convert(std::string value, int default_value);
|
||||
unsigned int convert(std::string value, unsigned int default_value);
|
||||
float convert(std::string value, float default_value);
|
||||
double convert(std::string value, double default_value);
|
||||
bool convert(const std::string& value, bool default_value);
|
||||
long convert(const std::string& value, long default_value);
|
||||
int convert(const std::string& value, int default_value);
|
||||
unsigned int convert(const std::string& value, unsigned int default_value);
|
||||
float convert(const std::string& value, float default_value);
|
||||
double convert(const std::string& value, double default_value);
|
||||
};
|
||||
|
||||
#endif /*GNSS_SDR_STRING_CONVERTER_H_*/
|
||||
|
@ -184,8 +184,8 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetObservables(std::shared
|
||||
std::string default_implementation = "GPS_L1_CA_Observables";
|
||||
std::string implementation = configuration->property("Observables.implementation", default_implementation);
|
||||
LOG(INFO) << "Getting Observables with implementation " << implementation;
|
||||
unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 12);
|
||||
unsigned int GPS_channels = configuration->property("Channels_GPS.count", 12);
|
||||
unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 0);
|
||||
unsigned int GPS_channels = configuration->property("Channels_GPS.count", 0);
|
||||
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 implementation = configuration->property("PVT.implementation", default_implementation);
|
||||
LOG(INFO) << "Getting PVT with implementation " << implementation;
|
||||
unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 12);
|
||||
unsigned int GPS_channels = configuration->property("Channels_GPS.count", 12);
|
||||
unsigned int Galileo_channels = configuration->property("Channels_Galileo.count", 0);
|
||||
unsigned int GPS_channels = configuration->property("Channels_GPS.count", 0);
|
||||
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;
|
||||
|
||||
//**************** 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";
|
||||
|
||||
@ -304,7 +304,7 @@ std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFacto
|
||||
}
|
||||
|
||||
//**************** 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";
|
||||
|
||||
@ -325,9 +325,6 @@ std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFacto
|
||||
acquisition_implementation, tracking, telemetry_decoder, channel_absolute_id, queue)));
|
||||
channel_absolute_id++;
|
||||
}
|
||||
|
||||
|
||||
|
||||
return channels;
|
||||
}
|
||||
|
||||
|
@ -194,8 +194,8 @@ Galileo_Fnav_Message::Galileo_Fnav_Message()
|
||||
//}
|
||||
void Galileo_Fnav_Message::split_page(std::string page_string)
|
||||
{
|
||||
std::string message_word = page_string.substr(0,214);
|
||||
std::string CRC_data = page_string.substr(214,24);
|
||||
std::string message_word = page_string.substr(0, 214);
|
||||
std::string CRC_data = page_string.substr(214, 24);
|
||||
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> Word_for_CRC_bits(message_word);
|
||||
std::bitset<24> checksum(CRC_data);
|
||||
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;
|
||||
|
||||
@ -225,7 +225,7 @@ bool Galileo_Fnav_Message::_CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> b
|
||||
|
||||
std::vector<unsigned char> 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 );
|
||||
|
||||
@ -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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
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_IODnav_1=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_1_bit);
|
||||
FNAV_t0c_1=(double)read_navigation_unsigned(data_bits,FNAV_t0c_1_bit);
|
||||
FNAV_SV_ID_PRN_1 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SV_ID_PRN_1_bit));
|
||||
FNAV_IODnav_1 =static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_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_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_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_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_SISA_1=(double)read_navigation_unsigned(data_bits,FNAV_SISA_1_bit);
|
||||
FNAV_ai0_1=(double)read_navigation_unsigned(data_bits,FNAV_ai0_1_bit);
|
||||
FNAV_SISA_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_SISA_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_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_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_region1_1=(bool)read_navigation_unsigned(data_bits,FNAV_region1_1_bit);
|
||||
FNAV_region2_1=(bool)read_navigation_unsigned(data_bits,FNAV_region2_1_bit);
|
||||
FNAV_region3_1=(bool)read_navigation_unsigned(data_bits,FNAV_region3_1_bit);
|
||||
FNAV_region4_1=(bool)read_navigation_unsigned(data_bits,FNAV_region4_1_bit);
|
||||
FNAV_region5_1=(bool)read_navigation_unsigned(data_bits,FNAV_region5_1_bit);
|
||||
FNAV_BGD_1=(double)read_navigation_signed(data_bits,FNAV_BGD_1_bit);
|
||||
FNAV_region1_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region1_1_bit));
|
||||
FNAV_region2_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region2_1_bit));
|
||||
FNAV_region3_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region3_1_bit));
|
||||
FNAV_region4_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region4_1_bit));
|
||||
FNAV_region5_1 = static_cast<bool>(read_navigation_unsigned(data_bits, FNAV_region5_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_E5ahs_1=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_1_bit);
|
||||
FNAV_WN_1=(double)read_navigation_unsigned(data_bits,FNAV_WN_1_bit);
|
||||
FNAV_TOW_1=(double)read_navigation_unsigned(data_bits,FNAV_TOW_1_bit);
|
||||
FNAV_E5advs_1=(double)read_navigation_unsigned(data_bits,FNAV_E5advs_1_bit);
|
||||
FNAV_E5ahs_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5ahs_1_bit));
|
||||
FNAV_WN_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN_1_bit));
|
||||
FNAV_TOW_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_1_bit));
|
||||
FNAV_E5advs_1 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5advs_1_bit));
|
||||
|
||||
flag_TOW_1=true;
|
||||
flag_TOW_set=true;
|
||||
flag_iono_and_GST = true; //set to false externally
|
||||
flag_TOW_1 = true;
|
||||
flag_TOW_set = true;
|
||||
flag_iono_and_GST = true; //set to false externally
|
||||
break;
|
||||
case 2: // Ephemeris (1/3) and GST
|
||||
FNAV_IODnav_2=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_2_bit);
|
||||
FNAV_M0_2=(double)read_navigation_unsigned(data_bits,FNAV_M0_2_bit);
|
||||
FNAV_IODnav_2 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_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_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_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_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_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_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_WN_2=(double)read_navigation_unsigned(data_bits,FNAV_WN_2_bit);
|
||||
FNAV_TOW_2=(double)read_navigation_unsigned(data_bits,FNAV_TOW_2_bit);
|
||||
FNAV_WN_2 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN_2_bit));
|
||||
FNAV_TOW_2 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_2_bit));
|
||||
|
||||
flag_TOW_2=true;
|
||||
flag_TOW_set=true;
|
||||
flag_ephemeris_1=true;
|
||||
flag_TOW_2 = true;
|
||||
flag_TOW_set = true;
|
||||
flag_ephemeris_1 = true;
|
||||
break;
|
||||
case 3: // Ephemeris (2/3) and GST
|
||||
FNAV_IODnav_3=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_3_bit);
|
||||
FNAV_i0_3=(double)read_navigation_signed(data_bits,FNAV_i0_3_bit);
|
||||
FNAV_IODnav_3 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_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_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_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_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_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_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_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_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_WN_3=(double)read_navigation_unsigned(data_bits,FNAV_WN_3_bit);
|
||||
FNAV_TOW_3=(double)read_navigation_unsigned(data_bits,FNAV_TOW_3_bit);
|
||||
FNAV_WN_3 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN_3_bit));
|
||||
FNAV_TOW_3 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_3_bit));
|
||||
|
||||
flag_TOW_3=true;
|
||||
flag_TOW_set=true;
|
||||
flag_ephemeris_2=true;
|
||||
flag_TOW_3 = true;
|
||||
flag_TOW_set = true;
|
||||
flag_ephemeris_2 = true;
|
||||
break;
|
||||
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_Cic_4=(double)read_navigation_unsigned(data_bits,FNAV_Cic_4_bit);
|
||||
case 4: // Ephemeris (3/3), GST-UTC conversion, GST-GPS conversion and TOW
|
||||
FNAV_IODnav_4 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODnav_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_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_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_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_deltatls_4=(double)read_navigation_signed(data_bits,FNAV_deltatls_4_bit);
|
||||
FNAV_t0t_4=(double)read_navigation_unsigned(data_bits,FNAV_t0t_4_bit);
|
||||
FNAV_deltatls_4 = static_cast<double>(read_navigation_signed(data_bits, FNAV_deltatls_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_WNot_4=(double)read_navigation_unsigned(data_bits,FNAV_WNot_4_bit);
|
||||
FNAV_WNlsf_4=(double)read_navigation_unsigned(data_bits,FNAV_WNlsf_4_bit);
|
||||
FNAV_DN_4=(double)read_navigation_unsigned(data_bits,FNAV_DN_4_bit);
|
||||
FNAV_deltatlsf_4=(double)read_navigation_signed(data_bits,FNAV_deltatlsf_4_bit);
|
||||
FNAV_t0g_4=(double)read_navigation_unsigned(data_bits,FNAV_t0g_4_bit);
|
||||
FNAV_WNot_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WNot_4_bit));
|
||||
FNAV_WNlsf_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WNlsf_4_bit));
|
||||
FNAV_DN_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_DN_4_bit));
|
||||
FNAV_deltatlsf_4 = static_cast<double>(read_navigation_signed(data_bits, FNAV_deltatlsf_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_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_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_WN0g_4=(double)read_navigation_unsigned(data_bits,FNAV_WN0g_4_bit);
|
||||
FNAV_TOW_4=(double)read_navigation_unsigned(data_bits,FNAV_TOW_4_bit);
|
||||
FNAV_WN0g_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WN0g_4_bit));
|
||||
FNAV_TOW_4 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_TOW_4_bit));
|
||||
|
||||
flag_TOW_4=true;
|
||||
flag_TOW_set=true;
|
||||
flag_ephemeris_3=true;
|
||||
flag_TOW_4 = true;
|
||||
flag_TOW_set = true;
|
||||
flag_ephemeris_3 = true;
|
||||
flag_utc_model = true; //set to false externally
|
||||
break;
|
||||
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_WNa_5=(double)read_navigation_unsigned(data_bits,FNAV_WNa_5_bit);
|
||||
FNAV_t0a_5=(double)read_navigation_unsigned(data_bits,FNAV_t0a_5_bit);
|
||||
FNAV_IODa_5 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_IODa_5_bit));
|
||||
FNAV_WNa_5 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_WNa_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_SVID1_5=(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_SVID1_5 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SVID1_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_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_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_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_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_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_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_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_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_E5ahs_1_5=(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_Deltaa12_2_5=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_2_5_bit);
|
||||
FNAV_E5ahs_1_5 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5ahs_1_5_bit));
|
||||
FNAV_SVID2_5 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SVID2_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_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_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_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;
|
||||
//TODO check this
|
||||
// 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;
|
||||
//
|
||||
//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;
|
||||
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
|
||||
* 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::bitset<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0);
|
||||
const std::vector<std::pair<int,int>> om_bit({{0,12}});
|
||||
FNAV_Omega0_2_6=(double)read_navigation_signed(omega_bits,om_bit);
|
||||
const std::vector<std::pair<int, int>> om_bit({{0, 12}});
|
||||
FNAV_Omega0_2_6 = static_cast<double>(read_navigation_signed(omega_bits, om_bit));
|
||||
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_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_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_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_E5ahs_2_6=(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_Deltaa12_3_6=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_3_6_bit);
|
||||
FNAV_E5ahs_2_6 = static_cast<double>(read_navigation_unsigned(data_bits, FNAV_E5ahs_2_6_bit));
|
||||
FNAV_SVID3_6 = static_cast<int>(read_navigation_unsigned(data_bits, FNAV_SVID3_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_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_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_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_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_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_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_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_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_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;
|
||||
}
|
||||
|
||||
@ -528,6 +528,7 @@ signed long int Galileo_Fnav_Message::read_navigation_signed(std::bitset<GALILEO
|
||||
return value;
|
||||
}
|
||||
|
||||
|
||||
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))
|
||||
@ -552,6 +553,8 @@ bool Galileo_Fnav_Message::have_new_ephemeris() //Check if we have a new ephemer
|
||||
else
|
||||
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
|
||||
{
|
||||
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
|
||||
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
|
||||
{
|
||||
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
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
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))
|
||||
@ -586,6 +593,8 @@ bool Galileo_Fnav_Message::have_new_almanac() //Check if we have a new almanac d
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris()
|
||||
{
|
||||
Galileo_Ephemeris ephemeris;
|
||||
@ -622,6 +631,8 @@ Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris()
|
||||
return ephemeris;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Galileo_Iono Galileo_Fnav_Message::get_iono()
|
||||
{
|
||||
Galileo_Iono iono;
|
||||
@ -644,6 +655,8 @@ Galileo_Iono Galileo_Fnav_Message::get_iono()
|
||||
return iono;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model()
|
||||
{
|
||||
Galileo_Utc_Model utc_model;
|
||||
@ -664,6 +677,8 @@ Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model()
|
||||
return utc_model;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Galileo_Almanac Galileo_Fnav_Message::get_almanac()
|
||||
{
|
||||
Galileo_Almanac almanac;
|
||||
|
@ -66,11 +66,11 @@ void Galileo_Navigation_Message::reset()
|
||||
flag_TOW_5 = 0;
|
||||
flag_TOW_set = false;
|
||||
|
||||
flag_GGTO= false;
|
||||
flag_GGTO_1= false;
|
||||
flag_GGTO_2= false;
|
||||
flag_GGTO_3= false;
|
||||
flag_GGTO_4= false;
|
||||
flag_GGTO = false;
|
||||
flag_GGTO_1 = false;
|
||||
flag_GGTO_2 = false;
|
||||
flag_GGTO_3 = false;
|
||||
flag_GGTO_4 = false;
|
||||
|
||||
IOD_ephemeris = 0;
|
||||
/*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
|
||||
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
|
||||
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;
|
||||
std::string Data_jk_ephemeris = Data_k + Data_j;
|
||||
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);
|
||||
//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;
|
||||
|
||||
switch (page_number)
|
||||
{
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
DLOG(INFO) << "A_1= " << A_1;
|
||||
flag_ephemeris_1 = true;
|
||||
@ -702,18 +702,18 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
|
||||
break;
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
DLOG(INFO) << "iDot_2= " << iDot_2;
|
||||
flag_ephemeris_2 = true;
|
||||
@ -721,57 +721,57 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
|
||||
break;
|
||||
|
||||
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 ;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
flag_ephemeris_3 = true;
|
||||
DLOG(INFO) << "flag_tow_set" << flag_TOW_set;
|
||||
break;
|
||||
|
||||
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 ;
|
||||
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 ;
|
||||
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;
|
||||
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;
|
||||
DLOG(INFO) << "C_is_4= " << C_is_4;
|
||||
/*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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
flag_ephemeris_4 = true;
|
||||
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*/
|
||||
/*Ionospheric correction*/
|
||||
/*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;
|
||||
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;
|
||||
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;
|
||||
DLOG(INFO) << "ai2_5= " << ai2_5;
|
||||
/*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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
/*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;
|
||||
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;
|
||||
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;
|
||||
flag_iono_and_GST = 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;
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
flag_TOW_6 = 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;
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
DLOG(INFO) << "M0_7= " << M0_7;
|
||||
flag_almanac_1 = true;
|
||||
@ -891,36 +891,36 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
|
||||
break;
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
DLOG(INFO) << "Omega_dot_8= " << Omega_dot_8;
|
||||
flag_almanac_2 = true;
|
||||
@ -928,38 +928,38 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
|
||||
break;
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
DLOG(INFO) << "delta_i_9= " << delta_i_9;
|
||||
flag_almanac_3 = true;
|
||||
@ -967,40 +967,40 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
|
||||
break;
|
||||
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
flag_GGTO_1=true;
|
||||
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;
|
||||
flag_GGTO_2=true;
|
||||
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;
|
||||
flag_GGTO_3=true;
|
||||
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;
|
||||
DLOG(INFO) << "WN_0G_10= " << WN_0G_10;
|
||||
flag_almanac_4 = true;
|
||||
@ -1008,11 +1008,11 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
|
||||
break;
|
||||
|
||||
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;
|
||||
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;
|
||||
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) << "flag_tow_set" << flag_TOW_set;
|
||||
break;
|
||||
|
@ -67,7 +67,7 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int WN)
|
||||
* the GST/Utc relationship is given by
|
||||
*/
|
||||
//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);
|
||||
}
|
||||
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:
|
||||
*/
|
||||
//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;
|
||||
t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
|
||||
//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:
|
||||
*/
|
||||
//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);
|
||||
}
|
||||
|
||||
|
@ -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
|
||||
* Satellite Systems receiver
|
||||
@ -15,7 +15,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* 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::set_system(system_);
|
||||
@ -61,12 +61,12 @@ Gnss_Satellite::~Gnss_Satellite()
|
||||
|
||||
void Gnss_Satellite::reset()
|
||||
{
|
||||
system_set = {"GPS", "GLONASS", "SBAS", "Galileo", "Compass"};
|
||||
system_set = {"GPS", "GLONASS", "SBAS", "Galileo", "Beidou"};
|
||||
satelliteSystem["GPS"] = "G";
|
||||
satelliteSystem["GLONASS"] = "R";
|
||||
satelliteSystem["SBAS"] = "S";
|
||||
satelliteSystem["Galileo"] = "E";
|
||||
satelliteSystem["Compass"] = "C";
|
||||
satelliteSystem["Beidou"] = "C";
|
||||
PRN = 0;
|
||||
system = 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"}
|
||||
std::set<std::string>::iterator it = system_set.find(system_);
|
||||
@ -126,7 +126,7 @@ void Gnss_Satellite::set_system(std::string system_)
|
||||
}
|
||||
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("");
|
||||
}
|
||||
}
|
||||
@ -217,7 +217,7 @@ unsigned int Gnss_Satellite::get_PRN() 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_;
|
||||
system_ = 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)
|
||||
{
|
||||
|
@ -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
|
||||
* Satellite Systems receiver
|
||||
@ -15,7 +15,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -48,10 +48,10 @@ class Gnss_Satellite
|
||||
{
|
||||
public:
|
||||
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.
|
||||
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_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
|
||||
@ -63,9 +63,9 @@ private:
|
||||
std::map<std::string,std::string> satelliteSystem;
|
||||
std::string block;
|
||||
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_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"};
|
||||
void reset();
|
||||
|
||||
|
@ -6,7 +6,7 @@
|
||||
* 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
|
||||
* Satellite Systems receiver
|
||||
@ -16,7 +16,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -28,6 +28,7 @@
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "gnss_signal.h"
|
||||
|
||||
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->signal = signal_;
|
||||
|
@ -6,7 +6,7 @@
|
||||
* 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
|
||||
* Satellite Systems receiver
|
||||
@ -16,7 +16,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -47,7 +47,7 @@ private:
|
||||
std::string signal;
|
||||
public:
|
||||
Gnss_Signal();
|
||||
Gnss_Signal(Gnss_Satellite satellite_, std::string signal_);
|
||||
Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_);
|
||||
~Gnss_Signal();
|
||||
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
|
||||
|
@ -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
|
||||
* Satellite Systems receiver
|
||||
@ -17,7 +17,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -303,11 +303,11 @@ double Gps_Navigation_Message::check_t(double time)
|
||||
corrTime = time;
|
||||
if (time > half_week)
|
||||
{
|
||||
corrTime = time - 2*half_week;
|
||||
corrTime = time - 2 * half_week;
|
||||
}
|
||||
else if (time < -half_week)
|
||||
{
|
||||
corrTime = time + 2*half_week;
|
||||
corrTime = time + 2 * half_week;
|
||||
}
|
||||
return corrTime;
|
||||
}
|
||||
@ -349,13 +349,13 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
|
||||
// Find satellite's position ----------------------------------------------
|
||||
|
||||
// Restore semi-major axis
|
||||
a = d_sqrt_A*d_sqrt_A;
|
||||
a = d_sqrt_A * d_sqrt_A;
|
||||
|
||||
// Time from ephemeris reference epoch
|
||||
tk = check_t(transmitTime - d_Toe);
|
||||
|
||||
// Computed mean motion
|
||||
n0 = sqrt(GM / (a*a*a));
|
||||
n0 = sqrt(GM / (a * a * a));
|
||||
|
||||
// Corrected mean motion
|
||||
n = n0 + d_Delta_n;
|
||||
@ -364,7 +364,7 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
|
||||
M = d_M_0 + n * tk;
|
||||
|
||||
// Reduce mean anomaly to between 0 and 2pi
|
||||
M = fmod((M + 2*GPS_PI), (2*GPS_PI));
|
||||
M = fmod((M + 2 * GPS_PI), (2 * GPS_PI));
|
||||
|
||||
// Initial guess of eccentric anomaly
|
||||
E = M;
|
||||
@ -374,7 +374,7 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
|
||||
{
|
||||
E_old = 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)
|
||||
{
|
||||
//Necessary precision is reached, exit from the loop
|
||||
@ -394,22 +394,22 @@ void Gps_Navigation_Message::satellitePosition(double transmitTime)
|
||||
phi = nu + d_OMEGA;
|
||||
|
||||
// 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
|
||||
u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi);
|
||||
u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi);
|
||||
|
||||
// Correct radius
|
||||
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi);
|
||||
r = a * (1 - d_e_eccentricity * cos(E)) + d_Crc * cos(2 * phi) + d_Crs * sin(2 * phi);
|
||||
|
||||
// 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
|
||||
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
|
||||
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
|
||||
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
|
||||
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++)
|
||||
{
|
||||
memcpy(&gps_word, &subframe[i*4], sizeof(char)*4);
|
||||
word_bits = std::bitset<(GPS_WORD_BITS+2)>(gps_word);
|
||||
for (int j=0; j<GPS_WORD_BITS; j++)
|
||||
memcpy(&gps_word, &subframe[i * 4], sizeof(char) * 4);
|
||||
word_bits = std::bitset<(GPS_WORD_BITS + 2) > (gps_word);
|
||||
for (int j = 0; j < GPS_WORD_BITS; j++)
|
||||
{
|
||||
subframe_bits[GPS_WORD_BITS*(9-i) + j] = word_bits[j];
|
||||
subframe_bits[GPS_WORD_BITS * (9 - i) + j] = word_bits[j];
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
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
|
||||
// (the variable subframe at this point contains bits of the last subframe).
|
||||
//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) !
|
||||
d_TOW_SF1 = d_TOW_SF1*6;
|
||||
d_TOW_SF1 = d_TOW_SF1 * 6;
|
||||
d_TOW = d_TOW_SF1 - 6; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
|
||||
i_GPS_week = (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_health = (int)read_navigation_unsigned(subframe_bits, SV_HEALTH);
|
||||
i_GPS_week = static_cast<int>(read_navigation_unsigned(subframe_bits, GPS_WEEK));
|
||||
i_SV_accuracy = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_ACCURACY)); // (20.3.3.3.1.3)
|
||||
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); //
|
||||
i_code_on_L2 = (int)read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2);
|
||||
d_TGD = (double)read_navigation_signed(subframe_bits, T_GD);
|
||||
d_TGD = d_TGD*T_GD_LSB;
|
||||
d_IODC = (double)read_navigation_unsigned(subframe_bits, IODC);
|
||||
d_Toc = (double)read_navigation_unsigned(subframe_bits, T_OC);
|
||||
d_Toc = d_Toc*T_OC_LSB;
|
||||
d_A_f0 = (double)read_navigation_signed(subframe_bits, A_F0);
|
||||
d_A_f0 = d_A_f0*A_F0_LSB;
|
||||
d_A_f1 = (double)read_navigation_signed(subframe_bits, A_F1);
|
||||
d_A_f1 = d_A_f1*A_F1_LSB;
|
||||
d_A_f2 = (double)read_navigation_signed(subframe_bits, A_F2);
|
||||
d_A_f2 = d_A_f2*A_F2_LSB;
|
||||
i_code_on_L2 = static_cast<int>(read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2));
|
||||
d_TGD = static_cast<double>(read_navigation_signed(subframe_bits, T_GD));
|
||||
d_TGD = d_TGD * T_GD_LSB;
|
||||
d_IODC = static_cast<double>(read_navigation_unsigned(subframe_bits, IODC));
|
||||
d_Toc = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OC));
|
||||
d_Toc = d_Toc * T_OC_LSB;
|
||||
d_A_f0 = static_cast<double>(read_navigation_signed(subframe_bits, A_F0));
|
||||
d_A_f0 = d_A_f0 * A_F0_LSB;
|
||||
d_A_f1 = static_cast<double>(read_navigation_signed(subframe_bits, A_F1));
|
||||
d_A_f1 = d_A_f1 * A_F1_LSB;
|
||||
d_A_f2 = static_cast<double>(read_navigation_signed(subframe_bits, A_F2));
|
||||
d_A_f2 = d_A_f2 * A_F2_LSB;
|
||||
|
||||
break;
|
||||
|
||||
case 2: //--- It is subframe 2 -------------------
|
||||
d_TOW_SF2 = (double)read_navigation_unsigned(subframe_bits, TOW);
|
||||
d_TOW_SF2 = d_TOW_SF2*6;
|
||||
d_TOW_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF2 = d_TOW_SF2 * 6;
|
||||
d_TOW = d_TOW_SF2 - 6; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
|
||||
d_IODE_SF2 = (double)read_navigation_unsigned(subframe_bits, IODE_SF2);
|
||||
d_Crs = (double)read_navigation_signed(subframe_bits, C_RS);
|
||||
d_IODE_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, IODE_SF2));
|
||||
d_Crs = static_cast<double>(read_navigation_signed(subframe_bits, C_RS));
|
||||
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_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_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_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_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_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_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;
|
||||
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;
|
||||
|
||||
break;
|
||||
|
||||
case 3: // --- It is subframe 3 -------------------------------------
|
||||
d_TOW_SF3 = (double)read_navigation_unsigned(subframe_bits, TOW);
|
||||
d_TOW_SF3 = d_TOW_SF3*6;
|
||||
d_TOW_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF3 = d_TOW_SF3 * 6;
|
||||
d_TOW = d_TOW_SF3 - 6; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
|
||||
d_Cic = (double)read_navigation_signed(subframe_bits, C_IC);
|
||||
d_Cic = static_cast<double>(read_navigation_signed(subframe_bits, C_IC));
|
||||
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_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_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_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_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_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_IODE_SF3 = (double)read_navigation_unsigned(subframe_bits, IODE_SF3);
|
||||
d_IDOT = (double)read_navigation_signed(subframe_bits, I_DOT);
|
||||
d_IDOT = d_IDOT*I_DOT_LSB;
|
||||
d_IODE_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, IODE_SF3));
|
||||
d_IDOT = static_cast<double>(read_navigation_signed(subframe_bits, I_DOT));
|
||||
d_IDOT = d_IDOT * I_DOT_LSB;
|
||||
|
||||
break;
|
||||
|
||||
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 = d_TOW_SF4*6;
|
||||
d_TOW_SF4 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF4 = d_TOW_SF4 * 6;
|
||||
d_TOW = d_TOW_SF4 - 6; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
|
||||
SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID);
|
||||
SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE);
|
||||
SV_data_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_DATA_ID));
|
||||
SV_page = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_PAGE));
|
||||
|
||||
if (SV_page == 13)
|
||||
{
|
||||
@ -564,33 +564,33 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
|
||||
if (SV_page == 18)
|
||||
{
|
||||
// Page 18 - Ionospheric and UTC data
|
||||
d_alpha0 = (double)read_navigation_signed(subframe_bits, ALPHA_0);
|
||||
d_alpha0 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_0));
|
||||
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_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_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_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_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_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_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_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_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_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;
|
||||
i_WN_T = (int)read_navigation_unsigned(subframe_bits, WN_T);
|
||||
d_DeltaT_LS = (double)read_navigation_signed(subframe_bits, DELTAT_LS);
|
||||
i_WN_LSF = (int)read_navigation_unsigned(subframe_bits, WN_LSF);
|
||||
i_DN = (int)read_navigation_unsigned(subframe_bits, DN); // Right-justified ?
|
||||
d_DeltaT_LSF = (double)read_navigation_signed(subframe_bits, DELTAT_LSF);
|
||||
i_WN_T = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_T));
|
||||
d_DeltaT_LS = static_cast<double>(read_navigation_signed(subframe_bits, DELTAT_LS));
|
||||
i_WN_LSF = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_LSF));
|
||||
i_DN = static_cast<int>(read_navigation_unsigned(subframe_bits, DN)); // Right-justified ?
|
||||
d_DeltaT_LSF = static_cast<double>(read_navigation_signed(subframe_bits, DELTAT_LSF));
|
||||
flag_iono_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)
|
||||
//! \TODO Read Anti-Spoofing, SV config
|
||||
almanacHealth[25] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV25);
|
||||
almanacHealth[26] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV26);
|
||||
almanacHealth[27] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV27);
|
||||
almanacHealth[28] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV28);
|
||||
almanacHealth[29] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV29);
|
||||
almanacHealth[30] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV30);
|
||||
almanacHealth[31] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV31);
|
||||
almanacHealth[32] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV32);
|
||||
almanacHealth[25] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV25));
|
||||
almanacHealth[26] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV26));
|
||||
almanacHealth[27] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV27));
|
||||
almanacHealth[28] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV28));
|
||||
almanacHealth[29] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV29));
|
||||
almanacHealth[30] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV30));
|
||||
almanacHealth[31] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV31));
|
||||
almanacHealth[32] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV32));
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
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 = d_TOW_SF5*6;
|
||||
d_TOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF5 = d_TOW_SF5 * 6;
|
||||
d_TOW = d_TOW_SF5 - 6; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG);
|
||||
SV_data_ID = (int)read_navigation_unsigned(subframe_bits, SV_DATA_ID);
|
||||
SV_page = (int)read_navigation_unsigned(subframe_bits, SV_PAGE);
|
||||
SV_data_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_DATA_ID));
|
||||
SV_page = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_PAGE));
|
||||
if (SV_page < 25)
|
||||
{
|
||||
//! \TODO read almanac
|
||||
}
|
||||
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;
|
||||
i_WN_A = (int)read_navigation_unsigned(subframe_bits, WN_A);
|
||||
almanacHealth[1] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV1);
|
||||
almanacHealth[2] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV2);
|
||||
almanacHealth[3] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV3);
|
||||
almanacHealth[4] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV4);
|
||||
almanacHealth[5] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV5);
|
||||
almanacHealth[6] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV6);
|
||||
almanacHealth[7] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV7);
|
||||
almanacHealth[8] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV8);
|
||||
almanacHealth[9] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV9);
|
||||
almanacHealth[10] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV10);
|
||||
almanacHealth[11] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV11);
|
||||
almanacHealth[12] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV12);
|
||||
almanacHealth[13] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV13);
|
||||
almanacHealth[14] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV14);
|
||||
almanacHealth[15] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV15);
|
||||
almanacHealth[16] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV16);
|
||||
almanacHealth[17] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV17);
|
||||
almanacHealth[18] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV18);
|
||||
almanacHealth[19] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV19);
|
||||
almanacHealth[20] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV20);
|
||||
almanacHealth[21] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV21);
|
||||
almanacHealth[22] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV22);
|
||||
almanacHealth[23] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV23);
|
||||
almanacHealth[24] = (int)read_navigation_unsigned(subframe_bits, HEALTH_SV24);
|
||||
i_WN_A = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_A));
|
||||
almanacHealth[1] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV1));
|
||||
almanacHealth[2] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV2));
|
||||
almanacHealth[3] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV3));
|
||||
almanacHealth[4] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV4));
|
||||
almanacHealth[5] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV5));
|
||||
almanacHealth[6] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV6));
|
||||
almanacHealth[7] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV7));
|
||||
almanacHealth[8] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV8));
|
||||
almanacHealth[9] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV9));
|
||||
almanacHealth[10] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV10));
|
||||
almanacHealth[11] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV11));
|
||||
almanacHealth[12] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV12));
|
||||
almanacHealth[13] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV13));
|
||||
almanacHealth[14] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV14));
|
||||
almanacHealth[15] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV15));
|
||||
almanacHealth[16] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV16));
|
||||
almanacHealth[17] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV17));
|
||||
almanacHealth[18] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV18));
|
||||
almanacHealth[19] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV19));
|
||||
almanacHealth[20] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV20));
|
||||
almanacHealth[21] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV21));
|
||||
almanacHealth[22] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV22));
|
||||
almanacHealth[23] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV23));
|
||||
almanacHealth[24] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV24));
|
||||
}
|
||||
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_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
|
||||
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)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
@ -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),
|
||||
* and the user<EFBFBD>s current time does not fall in the time span as given above
|
||||
* in 20.3.3.5.2.4b,*/
|
||||
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T));
|
||||
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);
|
||||
}
|
||||
|
||||
|
@ -15,7 +15,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -231,7 +231,7 @@ public:
|
||||
* \brief Computes the Coordinated Universal Time (UTC) and
|
||||
* 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();
|
||||
|
||||
|
@ -47,7 +47,7 @@ double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
|
||||
{
|
||||
double t_utc;
|
||||
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
|
||||
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)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
@ -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),
|
||||
* and the user<EFBFBD>s current time does not fall in the time span as given above
|
||||
* in 20.3.3.5.2.4b,*/
|
||||
Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * (double)(i_GPS_week - i_WN_T));
|
||||
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);
|
||||
}
|
||||
|
||||
|
@ -15,7 +15,7 @@
|
||||
* 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
|
||||
* 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,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
@ -64,14 +64,14 @@ public:
|
||||
{
|
||||
rx_time.relate(time_relation);
|
||||
}
|
||||
Sbas_Time get_rx_time_obj(){ return rx_time; }
|
||||
int get_prn() { return i_prn; }
|
||||
Sbas_Time get_rx_time_obj() const { return rx_time; }
|
||||
int get_prn() const { return i_prn; }
|
||||
std::vector<unsigned char> get_msg() const { return d_msg; }
|
||||
int get_preamble()
|
||||
{
|
||||
return d_msg[0];
|
||||
}
|
||||
int get_msg_type()
|
||||
int get_msg_type() const
|
||||
{
|
||||
return d_msg[1] >> 2;
|
||||
}
|
||||
|
@ -178,7 +178,7 @@ int main(int argc, char** argv)
|
||||
gettimeofday(&tv, NULL);
|
||||
long long int end = tv.tv_sec * 1000000 + tv.tv_usec;
|
||||
std::cout << "Total GNSS-SDR run time "
|
||||
<< ((double)(end - begin))/1000000.0
|
||||
<< (static_cast<double>(end - begin)) / 1000000.0
|
||||
<< " [seconds]" << std::endl;
|
||||
|
||||
google::ShutDownCommandLineFlags();
|
||||
|
@ -116,8 +116,8 @@ TEST(Conjugate_Test, ArmadilloComplexImplementation)
|
||||
|
||||
TEST(Conjugate_Test, VolkComplexImplementation)
|
||||
{
|
||||
std::complex<float>* input = new std::complex<float>[FLAGS_size_conjugate_test];
|
||||
std::complex<float>* output = 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 = 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);
|
||||
|
||||
struct timeval tv;
|
||||
@ -132,6 +132,6 @@ TEST(Conjugate_Test, VolkComplexImplementation)
|
||||
<< "-length complex float vector using VOLK finished in " << (end - begin)
|
||||
<< " microseconds" << std::endl;
|
||||
ASSERT_LE(0, end - begin);
|
||||
delete [] input;
|
||||
delete [] output;
|
||||
volk_free(input);
|
||||
volk_free(output);
|
||||
}
|
||||
|
@ -116,14 +116,14 @@ TEST(MagnitudeSquared_Test, ArmadilloComplexImplementation)
|
||||
|
||||
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);
|
||||
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;
|
||||
gettimeofday(&tv, NULL);
|
||||
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);
|
||||
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)
|
||||
<< " microseconds" << std::endl;
|
||||
ASSERT_LE(0, end - begin);
|
||||
delete [] input;
|
||||
delete [] output;
|
||||
volk_free(input);
|
||||
volk_free(output);
|
||||
}
|
||||
|
||||
// volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
|
||||
|
@ -181,8 +181,8 @@ TEST(Multiply_Test, ArmadilloComplexImplementation)
|
||||
|
||||
TEST(Multiply_Test, VolkComplexImplementation)
|
||||
{
|
||||
std::complex<float>* input = new std::complex<float>[FLAGS_size_multiply_test];
|
||||
std::complex<float>* output = 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 = 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);
|
||||
|
||||
struct timeval tv;
|
||||
@ -198,8 +198,9 @@ TEST(Multiply_Test, VolkComplexImplementation)
|
||||
<< " microseconds" << std::endl;
|
||||
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);
|
||||
|
||||
float* result = new float(0);
|
||||
volk_32f_accumulator_s32f(result, mag, FLAGS_size_multiply_test);
|
||||
// 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
|
||||
float expected = 0;
|
||||
ASSERT_FLOAT_EQ(expected, result[0]);
|
||||
delete [] input;
|
||||
delete [] output;
|
||||
delete [] mag;
|
||||
volk_free(input);
|
||||
volk_free(output);
|
||||
volk_free(mag);
|
||||
}
|
||||
|
||||
|
@ -183,19 +183,11 @@ void GalileoE5aPcpsAcquisitionGSoC2014GensourceTest::config_1()
|
||||
|
||||
config = std::make_shared<InMemoryConfiguration>();
|
||||
|
||||
|
||||
config->set_property("Channel.signal",signal);
|
||||
|
||||
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.item_type", "gr_complex");
|
||||
|
||||
config->set_property("SignalSource.num_satellites", "1");
|
||||
|
||||
config->set_property("SignalSource.system_0", "E");
|
||||
config->set_property("SignalSource.signal_0", "5X");
|
||||
config->set_property("SignalSource.PRN_0", "11");
|
||||
@ -325,12 +317,8 @@ void GalileoE5aPcpsAcquisitionGSoC2014GensourceTest::config_3()
|
||||
expected_delay_sec3 = 77;
|
||||
expected_doppler_hz3 = 5000;
|
||||
|
||||
|
||||
|
||||
max_doppler_error_hz = 2/(3*integration_time_ms*1e-3);
|
||||
max_delay_error_chips = 0.50;
|
||||
|
||||
|
||||
//max_doppler_error_hz = 1000;
|
||||
//max_delay_error_chips = 1;
|
||||
|
||||
@ -497,9 +485,9 @@ void GalileoE5aPcpsAcquisitionGSoC2014GensourceTest::process_message()
|
||||
|
||||
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 << message << "message" <<std::endl;
|
||||
//std::cout << message << "message" <<std::endl;
|
||||
if (realization_counter == num_of_realizations)
|
||||
{
|
||||
mse_delay /= num_of_realizations;
|
||||
@ -611,7 +599,7 @@ TEST_F(GalileoE5aPcpsAcquisitionGSoC2014GensourceTest, ValidationOfSIM)
|
||||
config_1();
|
||||
|
||||
//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 = 0;
|
||||
@ -722,9 +710,9 @@ TEST_F(GalileoE5aPcpsAcquisitionGSoC2014GensourceTest, ValidationOfSIM)
|
||||
top_block->run(); // Start threads and wait
|
||||
}) << "Failure running the top_block."<< 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_samplestamp_samples << "acq samples" <<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_samplestamp_samples << "acq samples" <<std::endl;
|
||||
// if (i == 0)
|
||||
// {
|
||||
// EXPECT_EQ(1, message) << "Acquisition failure. Expected message: 1=ACQ SUCCESS.";
|
||||
|
@ -85,6 +85,7 @@ void GalileoE5aTrackingTest::init()
|
||||
gnss_synchro.System = 'E';
|
||||
std::string signal = "5Q";
|
||||
signal.copy(gnss_synchro.Signal, 2, 0);
|
||||
gnss_synchro.PRN = 11;
|
||||
|
||||
config->set_property("GNSS-SDR.internal_fs_hz", "32000000");
|
||||
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.implementation", "Galileo_E5a_DLL_PLL_Tracking");
|
||||
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.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.dll_bw_hz_init","2.0");
|
||||
config->set_property("Tracking_Galileo.dll_bw_hz", "2");
|
||||
config->set_property("Tracking_Galileo.ti_ms", "1");
|
||||
}
|
||||
|
||||
TEST_F(GalileoE5aTrackingTest, ValidationOfResults)
|
||||
|
Loading…
x
Reference in New Issue
Block a user