mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-30 17:03:15 +00:00
Code Issues Releases Wiki Activity

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

Browse Source
This commit is contained in:
Carles Fernandez 2018-11-08 15:39:39 +01:00
commit dfdac8fb57
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
12 changed files with 535 additions and 543 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -536,23 +536,31 @@ RtklibPvt::~RtklibPvt()
rtkfree(&rtk);
}
std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris()
std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris() const
{
return pvt_->get_gps_ephemeris_map();
}
std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris()
std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris() const
{
return pvt_->get_galileo_ephemeris_map();
}
std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac()
std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac() const
{
return pvt_->get_gps_almanac_map();
}
std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac()
std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac() const
{
return pvt_->get_galileo_almanac_map();
}
void RtklibPvt::connect(gr::top_block_sptr top_block)
{
if (top_block)

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

@ -64,10 +64,10 @@ public:
}
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;
std::map<int, Gps_Ephemeris> get_gps_ephemeris() const override;
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() const override;
std::map<int, Gps_Almanac> get_gps_almanac() const override;
std::map<int, Galileo_Almanac> get_galileo_almanac() const override;
void connect(gr::top_block_sptr top_block) override;
void disconnect(gr::top_block_sptr top_block) override;

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

File diff suppressed because it is too large Load Diff

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

@ -111,8 +111,9 @@ private:
bool d_geojson_output_enabled;
bool d_gpx_output_enabled;
bool d_kml_output_enabled;
bool d_nmea_output_file_enabled;
std::shared_ptr<rtklib_solver> d_ls_pvt;
std::shared_ptr<rtklib_solver> d_pvt_solver;
std::map<int, Gnss_Synchro> gnss_observables_map;
bool observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b);
@ -152,13 +153,13 @@ public:
* \brief Get latest set of ephemeris from PVT block
*
*/
std::map<int, Gps_Ephemeris> get_gps_ephemeris_map();
std::map<int, Gps_Ephemeris> get_gps_ephemeris_map() const;
std::map<int, Gps_Almanac> get_gps_almanac_map();
std::map<int, Gps_Almanac> get_gps_almanac_map() const;
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map();
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map() const;
std::map<int, Galileo_Almanac> get_galileo_almanac_map();
std::map<int, Galileo_Almanac> get_galileo_almanac_map() const;
/*!
* \brief Clear all ephemeris information and the almanacs for GPS and Galileo
@ -166,7 +167,6 @@ public:
*/
void clear_ephemeris();
/*!
* \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available
*/

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

@ -338,7 +338,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
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);
cart_to_geo(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 = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()

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

@ -87,49 +87,13 @@ arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec
}
int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
int Pvt_Solution::cart_to_geo(double X, double Y, double Z, int elipsoid_selection)
{
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
Choices of Reference Ellipsoid for Geographical Coordinates
0. International Ellipsoid 1924
1. International Ellipsoid 1967
2. World Geodetic System 1972
3. Geodetic Reference System 1980
4. World Geodetic System 1984
*/
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
double lambda = atan2(Y, X);
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1;
double oldh = 0.0;
double N;
int iterations = 0;
do
{
oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
h = sqrt(X * X + Y * Y) / cos(phi) - N;
iterations = iterations + 1;
if (iterations > 100)
{
DLOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break;
}
}
while (std::abs(h - oldh) > 1.0e-12);
d_latitude_d = phi * 180.0 / GPS_PI;
d_longitude_d = lambda * 180.0 / GPS_PI;
d_height_m = h;
//todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
arma::vec XYZ = {X, Y, Z};
arma::vec LLH = cart2geo(XYZ, elipsoid_selection);
d_latitude_d = LLH(0) * 180.0 / GPS_PI;
d_longitude_d = LLH(1) * 180.0 / GPS_PI;
d_height_m = LLH(2);
return 0;
}

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

@ -127,7 +127,7 @@ public:
* 4 - World Geodetic System 1984.
*
*/
int cart2geo(double X, double Y, double Z, int elipsoid_selection);
int cart_to_geo(double X, double Y, double Z, int elipsoid_selection);
/*!
* \brief Tropospheric correction

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

@ -99,6 +99,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
}
}
bool rtklib_solver::save_matfile()
{
// READ DUMP FILE
@ -395,6 +396,7 @@ bool rtklib_solver::save_matfile()
return true;
}
rtklib_solver::~rtklib_solver()
{
if (d_dump_file.is_open() == true)
@ -868,7 +870,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
p_time += boost::posix_time::microseconds(static_cast<long>(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);
cart_to_geo(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 = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()

2
src/algorithms/libs/geofunctions.cc
View File

@ -301,7 +301,7 @@ double mstokph(double MetersPerSeconds)
arma::vec CTM_to_Euler(const arma::mat &C)
{
// Calculate Euler angles using (2.23)
arma::mat CTM = C;
arma::mat CTM(C);
arma::vec eul = arma::zeros(3, 1);
eul(0) = atan2(CTM(1, 2), CTM(2, 2)); // roll
if (CTM(0, 2) < -1.0) CTM(0, 2) = -1.0;

8
src/core/interfaces/pvt_interface.h
View File

@ -57,10 +57,10 @@ class PvtInterface : public GNSSBlockInterface
public:
virtual void reset() = 0;
virtual void clear_ephemeris() = 0;
virtual std::map<int, Gps_Ephemeris> get_gps_ephemeris() = 0;
virtual std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() = 0;
virtual std::map<int, Gps_Almanac> get_gps_almanac() = 0;
virtual std::map<int, Galileo_Almanac> get_galileo_almanac() = 0;
virtual std::map<int, Gps_Ephemeris> get_gps_ephemeris() const = 0;
virtual std::map<int, Galileo_Ephemeris> get_galileo_ephemeris() const = 0;
virtual std::map<int, Gps_Almanac> get_gps_almanac() const = 0;
virtual std::map<int, Galileo_Almanac> get_galileo_almanac() const = 0;
virtual bool get_latest_PVT(double* longitude_deg,
double* latitude_deg,

28
src/core/receiver/control_thread.cc
View File

@ -94,6 +94,7 @@ ControlThread::ControlThread(std::shared_ptr<ConfigurationInterface> configurati
{
configuration_ = configuration;
delete_configuration_ = false;
restart_ = false;
init();
}
@ -112,6 +113,7 @@ void ControlThread::telecommand_listener()
cmd_interface_.run_cmd_server(tcp_cmd_port);
}
/*
* Runs the control thread that manages the receiver control plane
*
@ -154,17 +156,16 @@ int ControlThread::run()
return 0;
}
//launch GNSS assistance process AFTER the flowgraph is running because the GNURadio asynchronous queues must be already running to transport msgs
// launch GNSS assistance process AFTER the flowgraph is running because the GNU Radio asynchronous queues must be already running to transport msgs
assist_GNSS();
// start the keyboard_listener thread
keyboard_thread_ = boost::thread(&ControlThread::keyboard_listener, this);
sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
//start the telecommand listener thread
// start the telecommand listener thread
cmd_interface_.set_pvt(flowgraph_->get_pvt());
cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);
bool enable_FPGA = configuration_->property("Channel.enable_FPGA", false);
if (enable_FPGA == true)
{
@ -764,22 +765,23 @@ void ControlThread::apply_action(unsigned int what)
}
}
std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, arma::vec LLH)
std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, const arma::vec &LLH)
{
//1. Compute rx ECEF position from LLH WGS84
// 1. Compute rx ECEF position from LLH WGS84
arma::vec LLH_rad = arma::vec{degtorad(LLH(0)), degtorad(LLH(1)), LLH(2)};
arma::mat C_tmp = arma::zeros(3, 3);
arma::vec r_eb_e = arma::zeros(3, 1);
arma::vec v_eb_e = arma::zeros(3, 1);
Geo_to_ECEF(LLH_rad, arma::vec{0, 0, 0}, C_tmp, r_eb_e, v_eb_e, C_tmp);
//2. Compute rx GPS time from UTC time
// 2. Compute rx GPS time from UTC time
gtime_t utc_gtime;
utc_gtime.time = rx_utc_time;
utc_gtime.sec = 0;
gtime_t gps_gtime = utc2gpst(utc_gtime);
//2. loop throught all the available ephemeris or almanac and compute satellite positions and elevations
// 3. loop through all the available ephemeris or almanac and compute satellite positions and elevations
// store visible satellites in a vector of pairs <int,Gnss_Satellite> to associate an elevation to the each satellite
std::vector<std::pair<int, Gnss_Satellite>> available_satellites;
@ -805,7 +807,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
//push sat
// push sat
if (El > 0)
{
std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@ -827,7 +829,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
//push sat
// push sat
if (El > 0)
{
std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@ -848,7 +850,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
arma::vec r_sat_eb_e = arma::vec{r_sat[0], r_sat[1], r_sat[2]};
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
//push sat
// push sat
if (El > 0)
{
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@ -870,7 +872,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
arma::vec dx = r_sat_eb_e - r_eb_e;
topocent(&Az, &El, &dist_m, r_eb_e, dx);
std::cout << "Using Galileo Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
// push sat
if (El > 0)
{
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
@ -879,11 +881,11 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
}
}
//sort the visible satellites in ascending order of elevation
// sort the visible satellites in ascending order of elevation
std::sort(available_satellites.begin(), available_satellites.end(), [](const std::pair<int, Gnss_Satellite> &a, const std::pair<int, Gnss_Satellite> &b) { // use lambda. Cleaner and easier to read
return a.first < b.first;
});
//std::reverse(available_satellites.begin(), available_satellites.end());
// std::reverse(available_satellites.begin(), available_satellites.end());
return available_satellites;
}

2
src/core/receiver/control_thread.h
View File

@ -148,8 +148,8 @@ private:
* Compute elevations for the specified time and position for all the available satellites in ephemeris and almanac queues
* returns a vector filled with the available satellites ordered from high elevation to low elevation angle.
*/
std::vector<std::pair<int, Gnss_Satellite>> get_visible_sats(time_t rx_utc_time, const arma::vec& LLH);
std::vector<std::pair<int, Gnss_Satellite>> get_visible_sats(time_t rx_utc_time, arma::vec LLH);
/*
* Read initial GNSS assistance from SUPL server or local XML files
*/