mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Merge with next

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

26
CMakeLists.txt
View File

@ -44,13 +44,32 @@ option(ENABLE_GPERFTOOLS "Enable linking to Gperftools libraries (tcmalloc and p
option(ENABLE_GENERIC_ARCH "Builds a portable binary" OFF)
# 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

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

@ -347,7 +347,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
d_position_UTC_time = p_time;
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;

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

@ -332,7 +332,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
}
// Compute UTC time and print PVT solution
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
}

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

@ -416,7 +416,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, do
d_position_UTC_time = p_time;
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
}

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

@ -222,8 +222,8 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
north = true;
}
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";

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

@ -143,7 +143,7 @@ Rinex_Printer::Rinex_Printer()
if ( FLAGS_RINEX_version.compare("3.01") == 0 )
{
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;
}

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

@ -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.

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

@ -277,7 +277,7 @@ std::string Rtcm_Printer::print_M1005_test ()
{
std::bitset<152> m1005 = get_M1005_test();
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() +

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

@ -268,7 +268,7 @@ float GalileoE1Pcps8msAmbiguousAcquisition::calculate_threshold(float pfa)
DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa;
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);

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

@ -277,9 +277,9 @@ float GalileoE1PcpsAmbiguousAcquisition::calculate_threshold(float pfa)
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
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);

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

@ -64,7 +64,7 @@ GalileoE1PcpsCccwsrAmbiguousAcquisition::GalileoE1PcpsCccwsrAmbiguousAcquisition
if (sampled_ms_ % 4 != 0)
{
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;

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

@ -305,7 +305,7 @@ float GalileoE1PcpsQuickSyncAmbiguousAcquisition::calculate_threshold(float pfa)
DLOG(INFO) <<"Channel "<<channel_<<" Pfa = "<< pfa;
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);

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

@ -268,10 +268,10 @@ float GalileoE1PcpsTongAmbiguousAcquisition::calculate_threshold(float pfa)
frequency_bins++;
}
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);

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

@ -293,9 +293,9 @@ float GalileoE5aNoncoherentIQAcquisitionCaf::calculate_threshold(float pfa)
{
frequency_bins++;
}
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);

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

@ -254,8 +254,8 @@ float GpsL1CaPcpsAcquisition::calculate_threshold(float pfa)
frequency_bins++;
}
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);

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

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

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

@ -250,11 +250,11 @@ float GpsL1CaPcpsMultithreadAcquisition::calculate_threshold(float pfa)
frequency_bins++;
}
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);

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

@ -249,11 +249,11 @@ float GpsL1CaPcpsOpenClAcquisition::calculate_threshold(float pfa)
frequency_bins++;
}
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);

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

@ -286,10 +286,10 @@ float GpsL1CaPcpsQuickSyncAcquisition::calculate_threshold(float pfa)
frequency_bins++;
}
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);

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

@ -239,14 +239,14 @@ float GpsL1CaPcpsTongAcquisition::calculate_threshold(float pfa)
frequency_bins++;
}
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;
}

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

@ -107,35 +107,27 @@ galileo_e5a_noncoherentIQ_acquisition_caf_cc::galileo_e5a_noncoherentIQ_acquisit
d_both_signal_components = both_signal_components_;
d_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

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

@ -79,10 +79,9 @@ galileo_pcps_8ms_acquisition_cc::galileo_pcps_8ms_acquisition_cc(
d_input_power = 0.0;
d_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;
}

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

@ -86,9 +86,8 @@ pcps_acquisition_cc::pcps_acquisition_cc(
d_num_doppler_bins = 0;
d_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

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

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

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

@ -82,9 +82,8 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
d_input_power = 0.0;
d_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)

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

@ -86,14 +86,13 @@ pcps_cccwsr_acquisition_cc::pcps_cccwsr_acquisition_cc(
d_input_power = 0.0;
d_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;
}

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

@ -92,12 +92,10 @@ pcps_multithread_acquisition_cc::pcps_multithread_acquisition_cc(
//todo: do something if posix_memalign fails
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);

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

@ -105,7 +105,7 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
d_well_count = 0;
d_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);
}

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

@ -70,9 +70,9 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
bool bit_transition_flag,
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;

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

@ -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;

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

@ -99,9 +99,8 @@ pcps_tong_acquisition_cc::pcps_tong_acquisition_cc(
d_input_power = 0.0;
d_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;
}

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

@ -60,15 +60,14 @@ direct_resampler_conditioner_cc::direct_resampler_conditioner_cc(
d_history(1)
{
// 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++)

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

@ -58,17 +58,15 @@ direct_resampler_conditioner_ss::direct_resampler_conditioner_ss(
d_sample_freq_in(sample_freq_in), d_sample_freq_out(
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();

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

@ -28,16 +28,18 @@
* -------------------------------------------------------------------------
*/
#include "signal_generator_c.h"
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <gnuradio/io_signature.h>
#include <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];
}
}
}

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

@ -68,6 +68,8 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
dump_ = configuration->property(role + ".dump", false);
dump_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;

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

@ -456,7 +456,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items, gr_vector
{
d_CRC_error_counter = 0;
d_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

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

@ -83,7 +83,7 @@ galileo_e1_dll_pll_veml_make_tracking_cc(
void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items,
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)
{

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

@ -127,29 +127,22 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
// Initialization of local code replica
// 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

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

@ -77,7 +77,7 @@ galileo_e5a_dll_pll_make_tracking_cc(
float early_late_space_chips)
{
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

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

@ -151,10 +151,10 @@ private:
gr_complex* d_prompt_data_code;
gr_complex* d_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;

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

@ -84,7 +84,7 @@ gps_l1_ca_dll_fll_pll_tracking_cc_sptr gps_l1_ca_dll_fll_pll_make_tracking_cc(
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
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;

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

@ -132,7 +132,7 @@ private:
void CN0_estimation_and_lock_detectors();
// 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;

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

@ -107,7 +107,7 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
d_if_freq = if_freq;
d_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

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

@ -82,7 +82,7 @@ gps_l1_ca_dll_pll_make_tracking_cc(
void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items,
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)
{

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

@ -126,26 +126,21 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
// Initialization of local code replica
// 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)

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

@ -79,234 +79,75 @@ void Correlator::Carrier_wipeoff_and_EPL_generic(int signal_length_samples, cons
void Correlator::Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, bool input_vector_unaligned)
void Correlator::Carrier_wipeoff_and_EPL_volk(int signal_length_samples, const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out)
{
gr_complex* bb_signal;
//gr_complex* 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 ()
{}

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

@ -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

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

@ -6,7 +6,7 @@
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* 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);

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

@ -6,7 +6,7 @@
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* 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_*/

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

@ -184,8 +184,8 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetObservables(std::shared
std::string default_implementation = "GPS_L1_CA_Observables";
std::string 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;
}

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

@ -194,8 +194,8 @@ Galileo_Fnav_Message::Galileo_Fnav_Message()
//}
void Galileo_Fnav_Message::split_page(std::string page_string)
{
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;

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

@ -66,11 +66,11 @@ void Galileo_Navigation_Message::reset()
flag_TOW_5 = 0;
flag_TOW_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;

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

@ -67,7 +67,7 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int WN)
* the GST/Utc relationship is given by
*/
//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);
}

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

@ -5,7 +5,7 @@
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* 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)
{

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

@ -5,7 +5,7 @@
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* 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();

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

@ -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
src/core/system_parameters/gnss_signal.h
View File

@ -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

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

@ -7,7 +7,7 @@
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* 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);
}

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

@ -15,7 +15,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify
* 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();

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

@ -47,7 +47,7 @@ double Gps_Utc_Model::utc_time(double gpstime_corrected, int i_GPS_week)
{
double t_utc;
double t_utc_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);
}

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

@ -15,7 +15,7 @@
* GNSS-SDR is free software: you can redistribute it and/or modify
* 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;
}

2
src/main/main.cc
View File

@ -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();

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

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

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

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

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

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

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

@ -183,19 +183,11 @@ void GalileoE5aPcpsAcquisitionGSoC2014GensourceTest::config_1()
config = std::make_shared<InMemoryConfiguration>();
config->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.";

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

@ -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)