mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 12:10:34 +00:00
Code Issues Releases Wiki Activity

Declare all Pvt_Solution data members private

Browse Source
This commit is contained in:
Carles Fernandez 2017-08-16 12:45:00 +02:00
parent 6c19437520
commit c1bbdd74d4
9 changed files with 393 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

@ -569,13 +569,13 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite
for (std::map<int,Gnss_Synchro>::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it)
{
it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s;
it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->get_time_offset_s() * GPS_C_m_s;
}
if(first_fix == true)
{
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << std::endl;
ttff_msgbuf ttff;
ttff.mtype = 1;
end = std::chrono::system_clock::now();
@ -1136,20 +1136,20 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite
}
// DEBUG MESSAGE: Display position in console output
if( (d_ls_pvt->b_valid_position == true) && (flag_display_pvt == true) )
if( (d_ls_pvt->is_valid_position() == true) && (flag_display_pvt == true) )
{
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC using " << d_ls_pvt->d_valid_observations << " observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << std::endl;
LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC using "<< d_ls_pvt->d_valid_observations << " observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]";
LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using "<< d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< " [deg], Height= " << d_ls_pvt->get_height() << " [m]";
/* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = "
<< d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP
<< " GDOP = " << d_ls_pvt->d_GDOP << std::endl; */
/* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using "<< d_ls_pvt->get_num_valid_observations()<<" observations is HDOP = " << d_ls_pvt->get_HDOP() << " VDOP = "
<< d_ls_pvt->get_VDOP() <<" TDOP = " << d_ls_pvt->get_TDOP()
<< " GDOP = " << d_ls_pvt->get_GDOP() << std::endl; */
}
// MULTIPLEXED FILE RECORDING - Record results to file

12
src/algorithms/PVT/libs/geojson_printer.cc
View File

@ -138,15 +138,15 @@ bool GeoJSON_Printer::print_position(const std::shared_ptr<Pvt_Solution>& positi
if (print_average_values == false)
{
latitude = position_->d_latitude_d;
longitude = position_->d_longitude_d;
height = position_->d_height_m;
latitude = position_->get_latitude();
longitude = position_->get_longitude();
height = position_->get_height();
}
else
{
latitude = position_->d_avg_latitude_d;
longitude = position_->d_avg_longitude_d;
height = position_->d_avg_height_m;
latitude = position_->get_avg_latitude();
longitude = position_->get_avg_longitude();
height = position_->get_avg_height();
}
if (geojson_file.is_open())

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

@ -46,7 +46,7 @@ hybrid_ls_pvt::hybrid_ls_pvt(int nchannels, std::string dump_filename, bool flag
d_flag_dump_enabled = flag_dump_to_file;
d_galileo_current_time = 0;
count_valid_position = 0;
d_flag_averaging = false;
this->set_averaging_flag(false);
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
@ -104,7 +104,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
double TX_time_corrected_s = 0.0;
double SV_clock_bias_s = 0.0;
d_flag_averaging = flag_averaging;
this->set_averaging_flag(flag_averaging);
// ********************************************************************************
// ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
@ -148,9 +148,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - this->get_time_offset_s() * GALILEO_C_m_s;
this->set_visible_satellites_ID(valid_obs, galileo_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
@ -211,9 +211,9 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
double P1_P2=(1.0-Gamma)*(gps_ephemeris_iter->second.d_TGD* GPS_C_m_s);
double Code_bias_m= P1_P2/(1.0-Gamma);
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s-Code_bias_m-d_rx_dt_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s-Code_bias_m-this->get_time_offset_s() * GPS_C_m_s;
this->set_visible_satellites_ID(valid_obs, gps_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
// SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
@ -264,8 +264,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr*GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
this->set_visible_satellites_ID(valid_obs, gps_cnav_ephemeris_iter->second.i_satellite_PRN);
this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz);
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
GPS_week=GPS_week%1024; //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV)
@ -296,7 +296,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// ********************************************************************************
// ****** SOLVE LEAST SQUARES******************************************************
// ********************************************************************************
d_valid_observations = valid_obs;
this->set_num_valid_observations(valid_obs);
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
@ -309,23 +309,23 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
try
{
// check if this is the initial position computation
if (d_rx_dt_s == 0)
if (this->get_time_offset_s() == 0)
{
// execute Bancroft's algorithm to estimate initial receiver position and time
DLOG(INFO) << " Executing Bancroft algorithm...";
rx_position_and_time = bancroftPos(satpos.t(), obs);
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds]
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
this->set_time_offset_s(rx_position_and_time(3) / GPS_C_m_s); // save time for the next iteration [meters]->[seconds]
}
// Execute WLS using previous position as the initialization point
rx_position_and_time = leastSquarePos(satpos, obs, W);
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
this->set_time_offset_s(this->get_time_offset_s() + rx_position_and_time(3) / GPS_C_m_s); // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
DLOG(INFO) << "Accumulated rx clock error=" << this->get_time_offset_s() << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
// Compute GST and Gregorian time
if( GST != 0.0)
@ -341,13 +341,13 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
this->set_position_UTC_time(p_time);
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()
<< " [deg], Height= " << this->get_height() << " [m]" << " RX time offset= " << this->get_time_offset_s() << " [s]";
// ###### Compute DOPs ########
hybrid_ls_pvt::compute_DOP();
@ -375,13 +375,13 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
tmp_double = rx_position_and_time(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
tmp_double = this->get_latitude();
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
tmp_double = this->get_longitude();
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
tmp_double = this->get_height();
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (const std::ifstream::failure& e)
@ -391,18 +391,18 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
}
// MOVING AVERAGE PVT
pos_averaging(flag_averaging);
this->perform_pos_averaging();
}
catch(const std::exception & e)
{
d_rx_dt_s = 0; //reset rx time estimation
this->set_time_offset_s(0.0); //reset rx time estimation
LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
b_valid_position = false;
this->set_valid_position(false);
}
}
else
{
b_valid_position = false;
this->set_valid_position(false);
}
return b_valid_position;
return this->is_valid_position();
}

12
src/algorithms/PVT/libs/kml_printer.cc
View File

@ -137,15 +137,15 @@ bool Kml_Printer::print_position(const std::shared_ptr<Pvt_Solution>& position,
if (print_average_values == false)
{
latitude = position_->d_latitude_d;
longitude = position_->d_longitude_d;
height = position_->d_height_m;
latitude = position_->get_latitude();
longitude = position_->get_longitude();
height = position_->get_height();
}
else
{
latitude = position_->d_avg_latitude_d;
longitude = position_->d_avg_longitude_d;
height = position_->d_avg_height_m;
latitude = position_->get_avg_latitude();
longitude = position_->get_avg_longitude();
height = position_->get_avg_height();
}
if (kml_file.is_open())

18
src/algorithms/PVT/libs/ls_pvt.cc
View File

@ -192,7 +192,8 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
arma::mat w = arma::zeros(nmbOfSatellites, nmbOfSatellites);
w.diag() = w_vec; //diagonal weight matrix
arma::vec pos = {d_rx_pos(0), d_rx_pos(0), d_rx_pos(0), 0}; // time error in METERS (time x speed)
arma::vec rx_pos = this->get_rx_pos();
arma::vec pos = {rx_pos(0), rx_pos(1), rx_pos(2), 0}; // time error in METERS (time x speed)
arma::mat A;
arma::mat omc;
arma::mat az;
@ -236,11 +237,12 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites
Ls_Pvt::topocent(&d_visible_satellites_Az[i],
&d_visible_satellites_El[i],
&d_visible_satellites_Distance[i],
pos.subvec(0,2),
Rot_X - pos.subvec(0, 2));
double *az, *el, *dist;
Ls_Pvt::topocent(az, el, dist, pos.subvec(0,2), Rot_X - pos.subvec(0, 2));
this->set_visible_satellites_Az(i, *az);
this->set_visible_satellites_El(i, *el);
this->set_visible_satellites_Distance(i, *dist);
if(traveltime < 0.1 && nmbOfSatellites > 3)
{
//--- Find receiver's height
@ -254,7 +256,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
else
{
//--- Find delay due to troposphere (in meters)
Ls_Pvt::tropo(&trop, sin(d_visible_satellites_El[i] * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
Ls_Pvt::tropo(&trop, sin(this->get_visible_satellites_El(i) * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if(trop > 5.0 ) trop = 0.0; //check for erratic values
}
}
@ -282,7 +284,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
}
//-- compute the Dilution Of Precision values
d_Q = arma::inv(arma::htrans(A) * A);
this->set_Q(arma::inv(arma::htrans(A) * A));
// check the consistency of the PVT solution
if (((fabs(pos(3)) * 1000.0) / GPS_C_m_s) > GPS_STARTOFFSET_ms * 2)

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

@ -338,7 +338,7 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
std::string Nmea_Printer::get_GPRMC()
{
// Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10
bool valid_fix = d_PVT_data->b_valid_position;
bool valid_fix = d_PVT_data->is_valid_position();
// ToDo: Compute speed and course over ground
double speed_over_ground_knots = 0;
@ -354,7 +354,7 @@ std::string Nmea_Printer::get_GPRMC()
sentence_str << sentence_header;
//UTC Time: hhmmss.sss
sentence_str << get_UTC_NMEA_time(d_PVT_data->d_position_UTC_time);
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
//Status: A: data valid, V: data NOT valid
@ -370,16 +370,16 @@ std::string Nmea_Printer::get_GPRMC()
if (print_avg_pos == true)
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->d_avg_latitude_d);
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->d_avg_longitude_d);
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
}
else
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->d_latitude_d);
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->d_longitude_d);
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
}
//Speed over ground (knots)
@ -395,7 +395,7 @@ std::string Nmea_Printer::get_GPRMC()
sentence_str << course_over_ground_deg;
// Date ddmmyy
boost::gregorian::date sentence_date = d_PVT_data->d_position_UTC_time.date();
boost::gregorian::date sentence_date = d_PVT_data->get_position_UTC_time().date();
unsigned int year = sentence_date.year();
unsigned int day = sentence_date.day();
unsigned int month = sentence_date.month();
@ -441,11 +441,11 @@ std::string Nmea_Printer::get_GPGSA()
{
//$GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
// GSA-GNSS DOP and Active Satellites
bool valid_fix = d_PVT_data->b_valid_position;
int n_sats_used = d_PVT_data->d_valid_observations;
double pdop = d_PVT_data->d_PDOP;
double hdop = d_PVT_data->d_HDOP;
double vdop = d_PVT_data->d_VDOP;
bool valid_fix = d_PVT_data->is_valid_position();
int n_sats_used = d_PVT_data->get_num_valid_observations();
double pdop = d_PVT_data->get_PDOP();
double hdop = d_PVT_data->get_HDOP();
double vdop = d_PVT_data->get_VDOP();
std::stringstream sentence_str;
std::string sentence_header;
@ -479,7 +479,7 @@ std::string Nmea_Printer::get_GPGSA()
{
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << d_PVT_data->d_visible_satellites_IDs[i];
sentence_str << d_PVT_data->get_visible_satellites_ID(i);
}
}
@ -527,7 +527,7 @@ std::string Nmea_Printer::get_GPGSV()
{
// GSV-GNSS Satellites in View
// Notice that NMEA 2.1 only supports 12 channels
int n_sats_used = d_PVT_data->d_valid_observations;
int n_sats_used = d_PVT_data->get_num_valid_observations();
std::stringstream sentence_str;
std::stringstream frame_str;
std::string sentence_header;
@ -567,22 +567,22 @@ std::string Nmea_Printer::get_GPGSV()
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << d_PVT_data->d_visible_satellites_IDs[current_satellite];
frame_str << std::dec << d_PVT_data->get_visible_satellites_ID(current_satellite);
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->d_visible_satellites_El[current_satellite]);
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_El(current_satellite));
frame_str << ",";
frame_str.width(3);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->d_visible_satellites_Az[current_satellite]);
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_Az(current_satellite));
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->d_visible_satellites_CN0_dB[current_satellite]);
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_CN0_dB(current_satellite));
current_satellite++;
@ -617,18 +617,18 @@ std::string Nmea_Printer::get_GPGSV()
std::string Nmea_Printer::get_GPGGA()
{
//boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
bool valid_fix = d_PVT_data->b_valid_position;
int n_channels = d_PVT_data->d_valid_observations;//d_nchannels
double hdop = d_PVT_data->d_HDOP;
bool valid_fix = d_PVT_data->is_valid_position();
int n_channels = d_PVT_data->get_num_valid_observations();//d_nchannels
double hdop = d_PVT_data->get_HDOP();
double MSL_altitude;
if (d_PVT_data->d_flag_averaging == true)
if (d_PVT_data->is_averaging() == true)
{
MSL_altitude = d_PVT_data->d_avg_height_m;
MSL_altitude = d_PVT_data->get_avg_height();
}
else
{
MSL_altitude = d_PVT_data->d_height_m;
MSL_altitude = d_PVT_data->get_height();
}
std::stringstream sentence_str;
@ -639,21 +639,21 @@ std::string Nmea_Printer::get_GPGGA()
sentence_str << sentence_header;
//UTC Time: hhmmss.sss
sentence_str << get_UTC_NMEA_time(d_PVT_data->d_position_UTC_time);
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
if (d_PVT_data->d_flag_averaging == true)
if (d_PVT_data->is_averaging() == true)
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->d_avg_latitude_d);
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->d_avg_longitude_d);
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
}
else
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->d_latitude_d);
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->d_longitude_d);
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
}
// Position fix indicator

210
src/algorithms/PVT/libs/pvt_solution.cc
View File

@ -487,17 +487,22 @@ int Pvt_Solution::compute_DOP()
}
int Pvt_Solution::set_averaging_depth(int depth)
void Pvt_Solution::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
return 0;
}
int Pvt_Solution::pos_averaging(bool flag_averaring)
void Pvt_Solution::set_averaging_flag(bool flag)
{
d_flag_averaging = flag;
}
void Pvt_Solution::perform_pos_averaging()
{
// MOVING AVERAGE PVT
bool avg = flag_averaring;
bool avg = d_flag_averaging;
if (avg == true)
{
if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth)
@ -543,6 +548,201 @@ int Pvt_Solution::pos_averaging(bool flag_averaring)
{
b_valid_position = true;
}
return 0;
}
double Pvt_Solution::get_time_offset_s() const
{
return d_rx_dt_s;
}
void Pvt_Solution::set_time_offset_s(double offset)
{
d_rx_dt_s = offset;
}
double Pvt_Solution::get_latitude() const
{
return d_latitude_d;
}
double Pvt_Solution::get_longitude() const
{
return d_longitude_d;
}
double Pvt_Solution::get_height() const
{
return d_height_m;
}
double Pvt_Solution::get_avg_latitude() const
{
return d_avg_latitude_d;
}
double Pvt_Solution::get_avg_longitude() const
{
return d_avg_longitude_d;
}
double Pvt_Solution::get_avg_height() const
{
return d_avg_height_m;
}
bool Pvt_Solution::is_averaging() const
{
return d_flag_averaging;
}
bool Pvt_Solution::is_valid_position() const
{
return b_valid_position;
}
void Pvt_Solution::set_valid_position(bool is_valid)
{
b_valid_position = is_valid;
}
void Pvt_Solution::set_rx_pos(arma::vec pos)
{
d_rx_pos = pos;
}
arma::vec Pvt_Solution::get_rx_pos() const
{
return d_rx_pos;
}
boost::posix_time::ptime Pvt_Solution::get_position_UTC_time() const
{
return d_position_UTC_time;
}
void Pvt_Solution::set_position_UTC_time(const boost::posix_time::ptime pt)
{
d_position_UTC_time = pt;
}
int Pvt_Solution::get_num_valid_observations() const
{
return d_valid_observations;
}
void Pvt_Solution::set_num_valid_observations(int num)
{
d_valid_observations = num;
}
void Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn)
{
d_visible_satellites_IDs[index] = prn;
}
unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const
{
return d_visible_satellites_IDs[index];
}
void Pvt_Solution::set_visible_satellites_El(size_t index, double el)
{
d_visible_satellites_El[index] = el;
}
double Pvt_Solution::get_visible_satellites_El(size_t index) const
{
return d_visible_satellites_El[index];
}
void Pvt_Solution::set_visible_satellites_Az(size_t index, double az)
{
d_visible_satellites_Az[index] = az;
}
double Pvt_Solution::get_visible_satellites_Az(size_t index) const
{
return d_visible_satellites_Az[index];
}
void Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist)
{
d_visible_satellites_Distance[index] = dist;
}
double Pvt_Solution::get_visible_satellites_Distance(size_t index) const
{
return d_visible_satellites_Distance[index];
}
void Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0)
{
d_visible_satellites_CN0_dB[index] = cn0;
}
double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const
{
return d_visible_satellites_CN0_dB[index];
}
void Pvt_Solution::set_Q(arma::mat Q)
{
d_Q = Q;
}
double Pvt_Solution::get_GDOP() const
{
return d_GDOP;
}
double Pvt_Solution::get_PDOP() const
{
return d_PDOP;
}
double Pvt_Solution::get_HDOP() const
{
return d_HDOP;
}
double Pvt_Solution::get_VDOP() const
{
return d_VDOP;
}
double Pvt_Solution::get_TDOP() const
{
return d_TDOP;
}

102
src/algorithms/PVT/libs/pvt_solution.h
View File

@ -45,50 +45,100 @@
*/
class Pvt_Solution
{
public:
Pvt_Solution();
private:
double d_rx_dt_s; // RX time offset [s]
double d_latitude_d; //!< RX position Latitude WGS84 [deg]
double d_longitude_d; //!< RX position Longitude WGS84 [deg]
double d_height_m; //!< RX position height WGS84 [m]
double d_latitude_d; // RX position Latitude WGS84 [deg]
double d_longitude_d; // RX position Longitude WGS84 [deg]
double d_height_m; // RX position height WGS84 [m]
arma::vec d_rx_pos;
double d_rx_dt_s; //!< RX time offset [s]
boost::posix_time::ptime d_position_UTC_time;
double d_avg_latitude_d; // Averaged latitude in degrees
double d_avg_longitude_d; // Averaged longitude in degrees
double d_avg_height_m; // Averaged height [m]
bool b_valid_position;
int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {}; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS] = {}; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {}; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {}; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS] = {}; //!< Array with the IDs of the valid satellites
//averaging
int d_averaging_depth; //!< Length of averaging window
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
int pos_averaging(bool flag_averaging);
bool d_flag_averaging;
int d_averaging_depth; // Length of averaging window
arma::vec d_rx_pos;
boost::posix_time::ptime d_position_UTC_time;
int d_valid_observations;
int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
double d_visible_satellites_El[PVT_MAX_CHANNELS] = {}; // Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {}; // Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {}; // Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites
public:
Pvt_Solution();
double get_time_offset_s() const; //!< Get RX time offset [s]
void set_time_offset_s(double offset); //!< Set RX time offset [s]
double get_latitude() const; //!< Get RX position Latitude WGS84 [deg]
double get_longitude() const; //!< Get RX position Longitude WGS84 [deg]
double get_height() const; //!< Get RX position height WGS84 [m]
double get_avg_latitude() const; //!< Get RX position averaged Latitude WGS84 [deg]
double get_avg_longitude() const; //!< Get RX position averaged Longitude WGS84 [deg]
double get_avg_height() const; //!< Get RX position averaged height WGS84 [m]
void set_rx_pos(arma::vec pos);
arma::vec get_rx_pos() const;
bool is_valid_position() const;
void set_valid_position(bool is_valid);
boost::posix_time::ptime get_position_UTC_time() const;
void set_position_UTC_time(const boost::posix_time::ptime pt);
int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites)
void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites)
void set_visible_satellites_ID(size_t index, unsigned int prn); //!< Set the ID of the visible satellite index channel
unsigned int get_visible_satellites_ID(size_t index) const; //!< Get the ID of the visible satellite index channel
void set_visible_satellites_El(size_t index, double el); //!< Set the LOS Elevation of the visible satellite index channel
double get_visible_satellites_El(size_t index) const; //!< Get the LOS Elevation of the visible satellite index channel
void set_visible_satellites_Az(size_t index, double az); //!< Set the LOS Azimuth of the visible satellite index channel
double get_visible_satellites_Az(size_t index) const; //!< Get the LOS Azimuth of the visible satellite index channel
void set_visible_satellites_Distance(size_t index, double dist); //!< Set the LOS Distance of the visible satellite index channel
double get_visible_satellites_Distance(size_t index) const; //!< Get the LOS Distance of the visible satellite index channel
void set_visible_satellites_CN0_dB(size_t index, double cn0); //!< Set the CN0 in dB of the visible satellite index channel
double get_visible_satellites_CN0_dB(size_t index) const; //!< Get the CN0 in dB of the visible satellite index channel
//averaging
void perform_pos_averaging();
void set_averaging_depth(int depth); //!< Set length of averaging window
bool is_averaging() const;
void set_averaging_flag(bool flag);
// DOP estimations
void set_Q(arma::mat Q);
int compute_DOP(); //!< Compute Dilution Of Precision parameters
bool d_flag_averaging;
int set_averaging_depth(int depth);
double get_GDOP() const;
double get_PDOP() const;
double get_HDOP() const;
double get_VDOP() const;
double get_TDOP() const;
arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat);

34
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -67,7 +67,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
count_valid_position = 0;
d_flag_averaging = false;
this->set_averaging_flag(false);
rtk_ = rtk;
pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, '0', '0', '0', 0, 0, 0 };
@ -115,7 +115,7 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
std::map<int,Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
d_flag_averaging = flag_averaging;
this->set_averaging_flag(flag_averaging);
// ********************************************************************************
// ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************
@ -280,7 +280,7 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
// ****** SOLVE PVT******************************************************
// **********************************************************************
b_valid_position = false;
this->set_valid_position(false);
if (valid_obs > 0)
{
int result = 0;
@ -298,33 +298,33 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
if(result == 0)
{
LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
d_rx_dt_s = 0; //reset rx time estimation
d_valid_observations = 0;
this->set_time_offset_s(0.0); //reset rx time estimation
this->set_num_valid_observations(0);
}
else
{
d_valid_observations = rtk_.sol.ns; //record the number of valid satellites used by the PVT solver
this->set_num_valid_observations(rtk_.sol.ns); //record the number of valid satellites used by the PVT solver
pvt_sol = rtk_.sol;
b_valid_position = true;
this->set_valid_position(true);
arma::vec rx_position_and_time(4);
rx_position_and_time(0) = pvt_sol.rr[0];
rx_position_and_time(1) = pvt_sol.rr[1];
rx_position_and_time(2) = pvt_sol.rr[2];
rx_position_and_time(3) = pvt_sol.dtr[0];
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
this->set_time_offset_s(this->get_time_offset_s() + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
boost::posix_time::ptime p_time;
gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time);
p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
d_position_UTC_time = p_time;
p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
this->set_position_UTC_time(p_time);
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()
<< " [deg], Height= " << this->get_height() << " [m]" << " RX time offset= " << this->get_time_offset_s() << " [s]";
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
@ -349,13 +349,13 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
tmp_double = rx_position_and_time(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double = d_latitude_d;
tmp_double = this->get_latitude();
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double = d_longitude_d;
tmp_double = this->get_longitude();
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double = d_height_m;
tmp_double = this->get_height();
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (const std::ifstream::failure& e)
@ -365,5 +365,5 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
}
}
}
return b_valid_position;
return this->is_valid_position();
}