diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 618ae7385..0f830414d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -569,13 +569,13 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite for (std::map::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 diff --git a/src/algorithms/PVT/libs/geojson_printer.cc b/src/algorithms/PVT/libs/geojson_printer.cc index cf4599b85..b1062f65f 100644 --- a/src/algorithms/PVT/libs/geojson_printer.cc +++ b/src/algorithms/PVT/libs/geojson_printer.cc @@ -138,15 +138,15 @@ bool GeoJSON_Printer::print_position(const std::shared_ptr& 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()) diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 545854d6b..f673f76f3 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -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 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 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 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 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 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 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 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(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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 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 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(); } diff --git a/src/algorithms/PVT/libs/kml_printer.cc b/src/algorithms/PVT/libs/kml_printer.cc index 8e008d2c0..1db4f6f61 100644 --- a/src/algorithms/PVT/libs/kml_printer.cc +++ b/src/algorithms/PVT/libs/kml_printer.cc @@ -137,15 +137,15 @@ bool Kml_Printer::print_position(const std::shared_ptr& 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()) diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index f8055241d..0b91d1cd0 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -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) diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index f0b4daec8..bb49a1c3a 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -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(d_PVT_data->d_visible_satellites_El[current_satellite]); + frame_str << std::dec << static_cast(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(d_PVT_data->d_visible_satellites_Az[current_satellite]); + frame_str << std::dec << static_cast(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(d_PVT_data->d_visible_satellites_CN0_dB[current_satellite]); + frame_str << std::dec << static_cast(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 diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index d3cbbe14f..b606d1b10 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -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; +} diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 3b0681391..947934026 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -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 d_hist_latitude_d; std::deque d_hist_longitude_d; std::deque 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); diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index f64be535c..4232768ff 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -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 & gnss_observables_ std::map::iterator gps_ephemeris_iter; std::map::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 & 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 & 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(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(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 & 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 & gnss_observables_ } } } - return b_valid_position; + return this->is_valid_position(); }