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

Merge branch 'next' of https://github.com/carlesfernandez/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-11-07 21:15:57 +01:00
commit 9e43e57d84
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
9 changed files with 38 additions and 266 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/algorithms/PVT/adapters/rtklib_pvt.h
View File

@ -57,18 +57,17 @@ public:
return role_;
}
//! Returns "RTKLIB_Pvt"
//! Returns "RTKLIB_PVT"
inline std::string implementation() override
{
return "RTKLIB_PVT";
}
void clear_ephemeris();
std::map<int, Gps_Ephemeris> get_gps_ephemeris();
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris();
std::map<int, Gps_Almanac> get_gps_almanac();
std::map<int, Galileo_Almanac> get_galileo_almanac();
void clear_ephemeris() override;
std::map<int, Gps_Ephemeris> get_gps_ephemeris() override;
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() override;
std::map<int, Gps_Almanac> get_gps_almanac() override;
std::map<int, Galileo_Almanac> get_galileo_almanac() override;
void connect(gr::top_block_sptr top_block) override;
void disconnect(gr::top_block_sptr top_block) override;
@ -91,7 +90,7 @@ public:
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time);
time_t* UTC_time) override;
private:
rtklib_pvt_cc_sptr pvt_;

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

@ -36,7 +36,6 @@
#include "gnss_sdr_create_directory.h"
#include <boost/archive/xml_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/exception/all.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/serialization/map.hpp>
@ -47,7 +46,6 @@
#include <iostream>
#include <map>
#include <exception>
#include <boost/date_time/posix_time/conversion.hpp>
#if OLD_BOOST
#include <boost/math/common_factor_rt.hpp>
namespace bc = boost::math;
@ -909,7 +907,7 @@ bool rtklib_pvt_cc::get_latest_PVT(double* longitude_deg,
*height_m = d_ls_pvt->get_height();
*ground_speed_kmh = d_ls_pvt->get_speed_over_ground() * 3600.0 / 1000.0;
*course_over_ground_deg = d_ls_pvt->get_course_over_ground();
*UTC_time = boost::posix_time::to_time_t(d_ls_pvt->get_position_UTC_time());
*UTC_time = to_time_t(d_ls_pvt->get_position_UTC_time());
return true;
}
@ -919,6 +917,7 @@ bool rtklib_pvt_cc::get_latest_PVT(double* longitude_deg,
}
}
int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star& output_items __attribute__((unused)))
{

7
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@ -40,6 +40,8 @@
#include "rtcm_printer.h"
#include "pvt_conf.h"
#include "rtklib_solver.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include <gnuradio/sync_block.h>
#include <sys/types.h>
#include <sys/ipc.h>
@ -135,6 +137,11 @@ private:
bool d_xml_storage;
std::string xml_base_path;
inline std::time_t to_time_t(boost::posix_time::ptime pt)
{
return (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds();
}
public:
rtklib_pvt_cc(uint32_t nchannels,

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

@ -150,8 +150,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// 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 - 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);
@ -213,8 +211,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
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 - 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
@ -265,8 +261,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// 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;
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)

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

@ -236,9 +236,6 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
double* elev = 0;
double* dist = 0;
Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2));
this->set_visible_satellites_Az(i, *azim);
this->set_visible_satellites_El(i, *elev);
this->set_visible_satellites_Distance(i, *dist);
if (traveltime < 0.1 && nmbOfSatellites > 3)
{
@ -253,7 +250,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(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);
Ls_Pvt::tropo(&trop, sin(*elev * 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
}
}
@ -280,9 +277,6 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
}
}
//-- compute the Dilution Of Precision values
//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)
{

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

@ -562,6 +562,7 @@ bool Pvt_Solution::is_averaging() const
return d_flag_averaging;
}
bool Pvt_Solution::is_valid_position() const
{
return b_valid_position;
@ -611,172 +612,3 @@ void Pvt_Solution::set_num_valid_observations(int num)
{
d_valid_observations = num;
}
bool Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat ID to channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
if (prn >= PVT_MAX_PRN)
{
LOG(WARNING) << "Setting to channel " << index << " a PRN of " << prn << " (the maximum is " << PVT_MAX_PRN << ")";
return false;
}
else
{
d_visible_satellites_IDs[index] = prn;
return true;
}
}
}
unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat ID for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0;
}
else
{
return d_visible_satellites_IDs[index];
}
}
bool Pvt_Solution::set_visible_satellites_El(size_t index, double el)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
if (el > 90.0)
{
LOG(WARNING) << "Setting a sat elevation > 90 [degrees]. Saturating to 90";
d_visible_satellites_El[index] = 90.0;
}
else
{
if (el < -90.0)
{
LOG(WARNING) << "Setting a sat elevation < -90 [degrees]. Saturating to -90";
d_visible_satellites_El[index] = -90.0;
}
else
{
d_visible_satellites_El[index] = el;
}
}
return true;
}
}
double Pvt_Solution::get_visible_satellites_El(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_El[index];
}
}
bool Pvt_Solution::set_visible_satellites_Az(size_t index, double az)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
d_visible_satellites_Az[index] = az;
return true;
}
}
double Pvt_Solution::get_visible_satellites_Az(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_Az[index];
}
}
bool Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
d_visible_satellites_Distance[index] = dist;
return true;
}
}
double Pvt_Solution::get_visible_satellites_Distance(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_Distance[index];
}
}
bool Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0)
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Setting sat Cn0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return false;
}
else
{
d_visible_satellites_CN0_dB[index] = cn0;
return true;
}
}
double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const
{
if (index >= PVT_MAX_CHANNELS)
{
LOG(WARNING) << "Getting received CN0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")";
return 0.0;
}
else
{
return d_visible_satellites_CN0_dB[index];
}
}

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

@ -72,12 +72,6 @@ private:
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
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();
@ -110,21 +104,6 @@ public:
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)
bool 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
bool set_visible_satellites_El(size_t index, double el); //!< Set the LOS Elevation, in degrees, of the visible satellite index channel
double get_visible_satellites_El(size_t index) const; //!< Get the LOS Elevation, in degrees, of the visible satellite index channel
bool set_visible_satellites_Az(size_t index, double az); //!< Set the LOS Azimuth, in degrees, of the visible satellite index channel
double get_visible_satellites_Az(size_t index) const; //!< Get the LOS Azimuth, in degrees, of the visible satellite index channel
bool 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
bool 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

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

@ -777,9 +777,9 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
for (int i = 0; i < MAXSAT; i++)
{
nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */
nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */
nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; // L1/E1
nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; // L2
nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; // L5/E5
}
result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data);
@ -788,20 +788,20 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
LOG(INFO) << "RTKLIB rtkpos error";
DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
this->set_time_offset_s(0.0); //reset rx time estimation
this->set_time_offset_s(0.0); // reset rx time estimation
this->set_num_valid_observations(0);
}
else
{
this->set_num_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;
// DOP computation
unsigned int used_sats = 0;
for (unsigned int i = 0; i < MAXSAT; i++)
{
pvt_ssat[i] = rtk_.ssat[i];
if (rtk_.ssat[i].vs == 1)
{
pvt_ssat[i] = rtk_.ssat[i];
used_sats++;
}
}

60
src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc
View File

@ -160,13 +160,20 @@ TEST_F(NmeaPrinterTest, PrintLine)
std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, false, rtk);
boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc
pvt_solution->set_position_UTC_time(pt);
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46));
std::time_t tim = (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds();
gtime_t gtime;
gtime.time = tim;
gtime.sec = 0.0;
arma::vec pos = {49.27416667, -123.18533333, 0};
pvt_solution->set_rx_pos(pos);
pvt_solution->set_valid_position(true);
pvt_solution->pvt_sol.rr[0] = -2282104.0; //49.27416667;
pvt_solution->pvt_sol.rr[1] = -3489369.0; //-123.18533333;
pvt_solution->pvt_sol.rr[2] = 4810507.0; // 0
pvt_solution->pvt_sol.rr[3] = 0.0;
pvt_solution->pvt_sol.rr[4] = 0.0;
pvt_solution->pvt_sol.rr[5] = 0.0;
pvt_solution->pvt_sol.stat = 1; // SOLQ_FIX
pvt_solution->pvt_sol.time = gtime;
bool flag_nmea_output_file = true;
ASSERT_NO_THROW({
@ -184,46 +191,7 @@ TEST_F(NmeaPrinterTest, PrintLine)
std::size_t found = line.find(GPRMC);
if (found != std::string::npos)
{
EXPECT_EQ(line, "$GPRMC,225446.000,A,4916.4500,N,12311.1199,W,0.00,0.00,191194,,*1c\r");
}
}
test_file.close();
}
EXPECT_EQ(0, remove(filename.c_str())) << "Failure deleting a temporary file.";
}
TEST_F(NmeaPrinterTest, PrintLineLessthan10min)
{
std::string filename("nmea_test.nmea");
std::shared_ptr<rtklib_solver> pvt_solution = std::make_shared<rtklib_solver>(12, "filename", false, false, rtk);
boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19),
boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc
pvt_solution->set_position_UTC_time(pt);
arma::vec pos = {49.07416667, -123.02527778, 0};
pvt_solution->set_rx_pos(pos);
pvt_solution->set_valid_position(true);
bool flag_nmea_output_file = true;
ASSERT_NO_THROW({
std::shared_ptr<Nmea_Printer> nmea_printer = std::make_shared<Nmea_Printer>(filename, flag_nmea_output_file, false, "");
nmea_printer->Print_Nmea_Line(pvt_solution, false);
}) << "Failure printing NMEA messages.";
std::ifstream test_file(filename);
std::string line;
std::string GPRMC("$GPRMC");
if (test_file.is_open())
{
while (getline(test_file, line))
{
std::size_t found = line.find(GPRMC);
if (found != std::string::npos)
{
EXPECT_EQ(line, "$GPRMC,225446.000,A,4904.4500,N,12301.5166,W,0.00,0.00,191194,,*1a\r");
EXPECT_EQ(line, "$GPRMC,225436.00,A,4916.4497617,N,12311.1202744,W,0.00,0.00,191194,0.0,E,D*21\r");
}
}
test_file.close();