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

Merge with next

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

24
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_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

8
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)
{
@@ -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
}

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
}

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

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

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

@@ -278,7 +278,7 @@ 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 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/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);

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

@@ -271,7 +271,7 @@ float GalileoE1PcpsTongAmbiguousAcquisition::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);

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

@@ -295,7 +295,7 @@ float GalileoE5aNoncoherentIQAcquisitionCaf::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);

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

@@ -255,7 +255,7 @@ float GpsL1CaPcpsAcquisition::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);

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()
{

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

@@ -253,7 +253,7 @@ float GpsL1CaPcpsMultithreadAcquisition::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);

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

@@ -252,7 +252,7 @@ float GpsL1CaPcpsOpenClAcquisition::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);

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

@@ -287,7 +287,7 @@ float GpsL1CaPcpsQuickSyncAcquisition::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);

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

@@ -242,7 +242,7 @@ float GpsL1CaPcpsTongAcquisition::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);

182
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,14 +198,8 @@ 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)
{
@@ -214,51 +209,32 @@ 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_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);
}
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],
volk_32fc_s32fc_multiply_32fc(&(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
//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);
}
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_a(&(d_fft_if->get_inbuf())[0],
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
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);
}
volk_32fc_conjugate_32fc(d_fft_code_Q_B,d_fft_if->get_outbuf(),d_fft_size);
}
}
}
@@ -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()));
}
}
}
@@ -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++)
{
// 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
@@ -610,12 +584,10 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
if (d_CAF_window_hz > 0)
{
int CAF_bins_half;
float* accum;
// double* accum;
if (posix_memalign((void**)&accum, 16, sizeof(float)) == 0){};
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/(float)CAF_bins_half;
weighting_factor = 0.5 / static_cast<float>(CAF_bins_half);
// weighting_factor = 0;
// std::cout << "weighting_factor " << weighting_factor << std::endl;
// Initialize first iterations
@@ -625,7 +597,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// 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] += 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]
@@ -635,7 +607,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// 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] += 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]
@@ -649,7 +621,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// 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] += 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;
@@ -659,7 +631,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
// 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] += 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;
@@ -692,9 +664,9 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
}
// 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;
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)
{
@@ -707,6 +679,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_dump_file.write((char*)d_CAF_vector, n);
d_dump_file.close();
}
volk_free(accum);
}
if (d_well_count == d_max_dwells)
@@ -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

76
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;
@@ -127,31 +126,17 @@ void galileo_pcps_8ms_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_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],
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++;
@@ -175,12 +160,9 @@ void galileo_pcps_8ms_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){};
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++)
{
// 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;
}

59
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;
@@ -127,18 +126,8 @@ 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);
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++)
{
// 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

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

@@ -81,10 +81,9 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
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_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);
@@ -107,28 +106,27 @@ void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_ste
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){};
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];
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);
volk_free(d_carrier);
volk_free(d_fft_codes);
volk_free(d_magnitude);
delete d_ifft;
delete d_fft_if;
if (d_dump)
@@ -146,8 +144,7 @@ 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);
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
void pcps_acquisition_fine_doppler_cc::init()
@@ -190,8 +187,7 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
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;
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);
}
@@ -207,7 +203,7 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
for (int i=0;i<d_num_doppler_points;i++)
{
volk_32f_index_max_16u_a(&tmp_intex_t,d_grid_data[i],d_fft_size);
volk_32f_index_max_16u(&tmp_intex_t, d_grid_data[i], d_fft_size);
if (d_grid_data[i][tmp_intex_t] > magt)
{
magt = d_grid_data[i][tmp_intex_t];
@@ -218,15 +214,15 @@ double pcps_acquisition_fine_doppler_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 = 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_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
@@ -250,22 +246,12 @@ double pcps_acquisition_fine_doppler_cc::search_maximum()
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;
// 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)
@@ -282,34 +268,33 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons
// 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);
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_u(d_grid_data[doppler_index],old_vector,p_tmp_vector,d_fft_size);
volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
}
free(p_tmp_vector);
volk_free(p_tmp_vector);
return d_fft_size;
}
@@ -320,17 +305,17 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
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));
//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){};
gr_complex *code_replica = static_cast<gr_complex*>(volk_malloc(d_fft_size * sizeof(gr_complex), volk_get_alignment()));
gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
int shift_index=(int)d_gnss_synchro->Acq_delay_samples;
int shift_index = static_cast<int>(d_gnss_synchro->Acq_delay_samples);
//std::cout<<"shift_index="<<shift_index<<std::endl;
// Rotate to align the local code replica using acquisition time delay estimation
if (shift_index != 0)
{
@@ -340,21 +325,18 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
//2. Perform code wipe-off
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
volk_32fc_x2_multiply_32fc_u(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
// 3. Perform the FFT (zero padded!)
fft_operator->execute();
// 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){};
float* p_tmp_vector = static_cast<float*>(volk_malloc(fft_size_extended * sizeof(float), volk_get_alignment()));
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended);
volk_32fc_magnitude_squared_32f(p_tmp_vector, fft_operator->get_outbuf(), fft_size_extended);
unsigned int tmp_index_freq = 0;
volk_32f_index_max_16u_a(&tmp_index_freq,p_tmp_vector,fft_size_extended);
//std::cout<<"FFT maximum index present at "<<tmp_index_freq<<std::endl;
volk_32f_index_max_16u(&tmp_index_freq, p_tmp_vector, fft_size_extended);
//case even
int counter = 0;
@@ -363,24 +345,26 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
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);
fftFreqBins[counter] = ((static_cast<float>(d_fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
counter++;
}
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);
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=(double)fftFreqBins[tmp_index_freq];
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)<<std::endl;
DLOG(INFO)<<std::endl<<"Error estimating fine frequency Doppler"<<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;
@@ -414,10 +398,12 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
// free memory!!
delete fft_operator;
free(code_replica);
free(p_tmp_vector);
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)
@@ -463,7 +449,9 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
if (d_test_statistics > d_threshold)
{
d_state = 3; //perform fine doppler estimation
}else{
}
else
{
d_state = 5; //negative acquisition
}
break;

47
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);
}
@@ -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);
// 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);
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_a(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
}
free(p_tmp_vector);
volk_free(p_tmp_vector);
return d_fft_size;
}
int pcps_assisted_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)

86
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;
@@ -143,14 +142,7 @@ void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> * code_data,
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);
@@ -158,14 +150,7 @@ void pcps_cccwsr_acquisition_cc::set_local_code(std::complex<float> * code_data,
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++;
@@ -189,10 +174,9 @@ void pcps_cccwsr_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 = -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++)
{
// 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;
}

40
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,8 +158,7 @@ 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;
complex_exp_gen_conj(d_grid_doppler_wipeoffs[doppler_index],
@@ -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,8 +198,8 @@ 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
@@ -219,7 +209,7 @@ void pcps_multithread_acquisition_cc::acquisition_core()
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);

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

@@ -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);
@@ -376,14 +371,7 @@ void pcps_opencl_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);
}
}
@@ -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 = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
doppler = -(int)d_doppler_max + d_doppler_step*doppler_index;
volk_32fc_x2_multiply_32fc_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)
@@ -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);
}

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

@@ -97,11 +97,9 @@ 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];
@@ -110,7 +108,6 @@ pcps_quicksync_acquisition_cc::pcps_quicksync_acquisition_cc(
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,16 +149,14 @@ pcps_quicksync_acquisition_cc::~pcps_quicksync_acquisition_cc()
// DLOG(INFO) << "END DESTROYER";
}
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);
d_code_folded = new gr_complex[d_fft_size]();
gr_complex* d_code_folded = new gr_complex[d_fft_size]();
memcpy(d_fft_if->get_inbuf(), d_code_folded, sizeof(gr_complex) * (d_fft_size));
/*perform folding of the code by the factorial factor parameter. Notice that
@@ -178,21 +169,12 @@ void pcps_quicksync_acquisition_cc::set_local_code(std::complex<float> * code)
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);
volk_32fc_conjugate_32fc(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
}
else
{
volk_32fc_conjugate_32fc_a(d_fft_codes,d_fft_if->get_outbuf(), d_fft_size);
}
// DLOG(INFO) << "END LOCAL CODE";
}
void pcps_quicksync_acquisition_cc::init()
{
@@ -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,11 +297,9 @@ 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++)
{
@@ -334,25 +307,24 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
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)) ,
@@ -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,14 +348,14 @@ 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);
@@ -405,21 +377,15 @@ 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.
@@ -432,52 +398,32 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
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*/
@@ -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;
}
@@ -544,7 +487,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 correlation output";
for (int i = 0; i < (int)d_folding_factor; i++) DLOG(INFO) << d_possible_delay[i] <<"\t\t\t"<<d_corr_output_f[i];
for (int i = 0; i < static_cast<int>(d_folding_factor); i++) DLOG(INFO) << d_possible_delay[i] << "\t\t\t" << d_corr_output_f[i];
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "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;

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

105
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.
@@ -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++)
{
@@ -98,21 +100,20 @@ void signal_generator_c::init()
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);
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')
{
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);
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,8 +140,7 @@ 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]];
@@ -168,7 +148,7 @@ void signal_generator_c::generate_codes()
{
// 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_)
@@ -193,14 +173,8 @@ void signal_generator_c::generate_codes()
char signal[3];
strcpy(signal, "5X");
if (posix_memalign((void**)&(sampled_code_data_[sat]), 16,
vector_length_ * sizeof(gr_complex)) == 0){};
galileo_e5_a_code_gen_complex_sampled(sampled_code_data_[sat] , signal, PRN_[sat], fs_in_,
(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_)
@@ -238,13 +211,12 @@ void signal_generator_c::generate_codes()
}
// 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]);
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++)
@@ -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,7 +313,7 @@ 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++)
@@ -362,7 +332,7 @@ gr_vector_void_star &output_items)
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);
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++)
{
@@ -371,11 +341,10 @@ gr_vector_void_star &output_items)
* 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,8 +352,7 @@ 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++;
}
@@ -402,10 +370,7 @@ gr_vector_void_star &output_items)
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];
}
}
}

8
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
if ((item_type_.compare("gr_complex") != 0) && (IF < 1e6) ) // if IF < BW/2, signal is complex (interleaved)
{
signal_duration_s /= 2;
#endif
}
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

127
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;
@@ -201,10 +193,10 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
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,11 +236,11 @@ 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);
@@ -271,7 +263,7 @@ 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

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

@@ -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,15 +152,11 @@ 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;
@@ -177,7 +164,7 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc(
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;
@@ -358,13 +337,13 @@ 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;
@@ -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;
@@ -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
@@ -585,7 +563,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_
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
@@ -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;
}
@@ -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)
{

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;

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

@@ -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;
}
@@ -356,7 +352,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
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_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,7 +397,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;
@@ -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;
@@ -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;

89
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,9 +174,9 @@ 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;
@@ -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

128
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,13 +252,13 @@ 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;
@@ -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)

249
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_free(bb_signal);
}
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)
//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)
//{
// free(input_aligned);
//}
}
//void Correlator::Carrier_wipeoff_and_EPL_volk_IQ(int prn_length_samples,int integration_time ,const gr_complex* input, gr_complex* carrier, gr_complex* E_code, gr_complex* P_code, gr_complex* L_code, gr_complex* P_data_code, gr_complex* E_out, gr_complex* P_out, gr_complex* L_out, gr_complex* P_data_out, bool input_vector_unaligned)
//{
// 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)
{
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 == true)
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)
{
//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));
gr_complex* bb_signal = static_cast<gr_complex*>(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment()));
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_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);
}
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);
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()));
free(bb_signal);
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;
}

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

@@ -246,110 +246,110 @@ void Galileo_Fnav_Message::decode_page(std::string data)
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
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;
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;
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);
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;
@@ -357,50 +357,50 @@ void Galileo_Fnav_Message::decode_page(std::string data)
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);
//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;
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.*/
@@ -408,38 +408,38 @@ void Galileo_Fnav_Message::decode_page(std::string data)
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);
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;
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;

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

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

190
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
@@ -449,7 +449,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
}
}
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 = 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);
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 = (double)read_navigation_unsigned(subframe_bits, IODC);
d_Toc = (double)read_navigation_unsigned(subframe_bits, T_OC);
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 = (double)read_navigation_signed(subframe_bits, A_F0);
d_A_f0 = static_cast<double>(read_navigation_signed(subframe_bits, A_F0));
d_A_f0 = d_A_f0 * A_F0_LSB;
d_A_f1 = (double)read_navigation_signed(subframe_bits, A_F1);
d_A_f1 = static_cast<double>(read_navigation_signed(subframe_bits, A_F1));
d_A_f1 = d_A_f1 * A_F1_LSB;
d_A_f2 = (double)read_navigation_signed(subframe_bits, A_F2);
d_A_f2 = static_cast<double>(read_navigation_signed(subframe_bits, A_F2));
d_A_f2 = d_A_f2 * A_F2_LSB;
break;
case 2: //--- It is subframe 2 -------------------
d_TOW_SF2 = (double)read_navigation_unsigned(subframe_bits, TOW);
d_TOW_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
d_TOW_SF2 = d_TOW_SF2 * 6;
d_TOW = 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 = 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_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 = 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 = 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<65>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<65>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)