1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 04:30:33 +00:00

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

This commit is contained in:
Carles Fernandez 2018-11-05 18:50:37 +01:00
commit 04a8096179
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
19 changed files with 458 additions and 43 deletions

View File

@ -48,6 +48,12 @@ namespace bc = boost::integer;
using google::LogMessage; using google::LogMessage;
void RtklibPvt::clear_ephemeris()
{
pvt_->clear_ephemeris();
}
RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
std::string role, std::string role,
unsigned int in_streams, unsigned int in_streams,
@ -515,6 +521,22 @@ RtklibPvt::~RtklibPvt()
rtkfree(&rtk); rtkfree(&rtk);
} }
std::map<int, Gps_Ephemeris> RtklibPvt::get_gps_ephemeris()
{
return pvt_->get_gps_ephemeris_map();
}
std::map<int, Galileo_Ephemeris> RtklibPvt::get_galileo_ephemeris()
{
return pvt_->get_galileo_ephemeris_map();
}
std::map<int, Gps_Almanac> RtklibPvt::get_gps_almanac()
{
return pvt_->get_gps_almanac_map();
}
std::map<int, Galileo_Almanac> RtklibPvt::get_galileo_almanac()
{
return pvt_->get_galileo_almanac_map();
}
void RtklibPvt::connect(gr::top_block_sptr top_block) void RtklibPvt::connect(gr::top_block_sptr top_block)
{ {

View File

@ -63,6 +63,13 @@ public:
return "RTKLIB_PVT"; 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 connect(gr::top_block_sptr top_block) override; void connect(gr::top_block_sptr top_block) override;
void disconnect(gr::top_block_sptr top_block) override; void disconnect(gr::top_block_sptr top_block) override;
gr::basic_block_sptr get_left_block() override; gr::basic_block_sptr get_left_block() override;

View File

@ -234,11 +234,32 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg)
} }
std::map<int, Gps_Ephemeris> rtklib_pvt_cc::get_GPS_L1_ephemeris_map() std::map<int, Gps_Ephemeris> rtklib_pvt_cc::get_gps_ephemeris_map()
{ {
return d_ls_pvt->gps_ephemeris_map; return d_ls_pvt->gps_ephemeris_map;
} }
std::map<int, Gps_Almanac> rtklib_pvt_cc::get_gps_almanac_map()
{
return d_ls_pvt->gps_almanac_map;
}
std::map<int, Galileo_Ephemeris> rtklib_pvt_cc::get_galileo_ephemeris_map()
{
return d_ls_pvt->galileo_ephemeris_map;
}
std::map<int, Galileo_Almanac> rtklib_pvt_cc::get_galileo_almanac_map()
{
return d_ls_pvt->galileo_almanac_map;
}
void rtklib_pvt_cc::clear_ephemeris()
{
d_ls_pvt->gps_ephemeris_map.clear();
d_ls_pvt->gps_almanac_map.clear();
d_ls_pvt->galileo_ephemeris_map.clear();
d_ls_pvt->galileo_almanac_map.clear();
}
rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
const Pvt_Conf& conf_, const Pvt_Conf& conf_,

View File

@ -31,7 +31,7 @@
#ifndef GNSS_SDR_RTKLIB_PVT_CC_H #ifndef GNSS_SDR_RTKLIB_PVT_CC_H
#define GNSS_SDR_RTKLIB_PVT_CC_H #define GNSS_SDR_RTKLIB_PVT_CC_H
#include "gps_ephemeris.h"
#include "nmea_printer.h" #include "nmea_printer.h"
#include "kml_printer.h" #include "kml_printer.h"
#include "gpx_printer.h" #include "gpx_printer.h"
@ -142,11 +142,22 @@ public:
rtk_t& rtk); rtk_t& rtk);
/*! /*!
* \brief Get latest set of GPS L1 ephemeris from PVT block * \brief Get latest set of ephemeris from PVT block
* *
* It is used to save the assistance data at the receiver shutdown
*/ */
std::map<int, Gps_Ephemeris> get_GPS_L1_ephemeris_map(); std::map<int, Gps_Ephemeris> get_gps_ephemeris_map();
std::map<int, Gps_Almanac> get_gps_almanac_map();
std::map<int, Galileo_Ephemeris> get_galileo_ephemeris_map();
std::map<int, Galileo_Almanac> get_galileo_almanac_map();
/*!
* \brief Clear all ephemeris information and the almanacs for GPS and Galileo
*
*/
void clear_ephemeris();
~rtklib_pvt_cc(); //!< Default destructor ~rtklib_pvt_cc(); //!< Default destructor

View File

@ -59,7 +59,6 @@ bool ChannelFsm::Event_stop_channel()
switch (d_state) switch (d_state)
{ {
case 0: //already in stanby case 0: //already in stanby
return true;
break; break;
case 1: //acquisition case 1: //acquisition
d_state = 0; d_state = 0;
@ -70,8 +69,9 @@ bool ChannelFsm::Event_stop_channel()
stop_tracking(); stop_tracking();
break; break;
default: default:
return true; break;
} }
return true;
} }
bool ChannelFsm::Event_start_acquisition() bool ChannelFsm::Event_start_acquisition()

View File

@ -39,6 +39,7 @@ set(GNSS_SPLIBS_SOURCES
conjugate_sc.cc conjugate_sc.cc
conjugate_ic.cc conjugate_ic.cc
gnss_sdr_create_directory.cc gnss_sdr_create_directory.cc
geofunctions.cc
) )
set(GNSS_SPLIBS_HEADERS set(GNSS_SPLIBS_HEADERS

View File

@ -116,7 +116,7 @@ geph_t eph_to_rtklib(const Glonass_Gnav_Ephemeris& glonass_gnav_eph, const Glona
eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph) eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph)
{ {
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, eph_t rtklib_sat = {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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
//Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris
rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO; rtklib_sat.sat = gal_eph.i_satellite_PRN + NSATGPS + NSATGLO;
rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1;
@ -174,7 +174,7 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph)
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph) eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph)
{ {
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, eph_t rtklib_sat = {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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
rtklib_sat.sat = gps_eph.i_satellite_PRN; rtklib_sat.sat = gps_eph.i_satellite_PRN;
rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A;
rtklib_sat.M0 = gps_eph.d_M_0; rtklib_sat.M0 = gps_eph.d_M_0;
@ -231,7 +231,7 @@ eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph)
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph) eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
{ {
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, eph_t rtklib_sat = {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, 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, 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, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0};
rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN;
const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170
rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A;
@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
return rtklib_sat; return rtklib_sat;
} }
alm_t alm_to_rtklib(const Gps_Almanac& gps_alm)
{
alm_t rtklib_alm;
rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
rtklib_alm.sat = gps_alm.i_satellite_PRN;
rtklib_alm.svh = gps_alm.i_SV_health;
rtklib_alm.svconf = gps_alm.i_AS_status;
rtklib_alm.week = gps_alm.i_WNa;
rtklib_alm.toa = gpst2time(gps_alm.i_WNa, gps_alm.d_Toa);
rtklib_alm.A = gps_alm.d_sqrt_A * gps_alm.d_sqrt_A;
rtklib_alm.e = gps_alm.d_e_eccentricity;
rtklib_alm.i0 = gps_alm.d_Delta_i + 0.3;
rtklib_alm.OMG0 = gps_alm.d_OMEGA0;
rtklib_alm.OMGd = gps_alm.d_OMEGA_DOT;
rtklib_alm.omg = gps_alm.d_OMEGA;
rtklib_alm.M0 = gps_alm.d_M_0;
rtklib_alm.f0 = gps_alm.d_A_f0;
rtklib_alm.f1 = gps_alm.d_A_f1;
rtklib_alm.toas = gps_alm.d_Toa;
return rtklib_alm;
}
alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm)
{
alm_t rtklib_alm;
rtklib_alm = {0, 0, 0, 0, {0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
rtklib_alm.sat = gal_alm.i_satellite_PRN;
rtklib_alm.svh = gal_alm.E1B_HS;
rtklib_alm.svconf = gal_alm.E1B_HS;
rtklib_alm.week = gal_alm.d_WNa;
rtklib_alm.toa = gpst2time(gal_alm.d_WNa, gal_alm.d_Toa);
rtklib_alm.A = 5440.588203494 + gal_alm.d_Delta_sqrt_A;
rtklib_alm.A = rtklib_alm.A * rtklib_alm.A;
rtklib_alm.e = gal_alm.d_e_eccentricity;
rtklib_alm.i0 = gal_alm.d_Delta_i + 0.31111;
rtklib_alm.OMG0 = gal_alm.d_OMEGA0;
rtklib_alm.OMGd = gal_alm.d_OMEGA_DOT;
rtklib_alm.omg = gal_alm.d_OMEGA;
rtklib_alm.M0 = gal_alm.d_M_0;
rtklib_alm.f0 = gal_alm.d_A_f0;
rtklib_alm.f1 = gal_alm.d_A_f1;
rtklib_alm.toas = gal_alm.d_Toa;
return rtklib_alm;
}

View File

@ -38,10 +38,16 @@
#include "gps_cnav_ephemeris.h" #include "gps_cnav_ephemeris.h"
#include "glonass_gnav_ephemeris.h" #include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_utc_model.h" #include "glonass_gnav_utc_model.h"
#include "gps_almanac.h"
#include "galileo_almanac.h"
eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph); eph_t eph_to_rtklib(const Galileo_Ephemeris& gal_eph);
eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph); eph_t eph_to_rtklib(const Gps_Ephemeris& gps_eph);
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph); eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph);
alm_t alm_to_rtklib(const Gps_Almanac& gps_alm);
alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm);
/*! /*!
* \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart * \brief Transforms a Glonass_Gnav_Ephemeris to its RTKLIB counterpart
* \param glonass_gnav_eph GLONASS GNAV Ephemeris structure * \param glonass_gnav_eph GLONASS GNAV Ephemeris structure

View File

@ -38,6 +38,10 @@
#define GNSS_SDR_PVT_INTERFACE_H_ #define GNSS_SDR_PVT_INTERFACE_H_
#include "gnss_block_interface.h" #include "gnss_block_interface.h"
#include "gps_ephemeris.h"
#include "galileo_ephemeris.h"
#include "gps_almanac.h"
#include "galileo_almanac.h"
/*! /*!
* \brief This class represents an interface to a PVT block. * \brief This class represents an interface to a PVT block.
@ -52,6 +56,11 @@ class PvtInterface : public GNSSBlockInterface
{ {
public: public:
virtual void reset() = 0; 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;
}; };
#endif /* GNSS_SDR_PVT_INTERFACE_H_ */ #endif /* GNSS_SDR_PVT_INTERFACE_H_ */

View File

@ -35,6 +35,7 @@
#include "control_thread.h" #include "control_thread.h"
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "concurrent_map.h" #include "concurrent_map.h"
#include "pvt_interface.h"
#include "control_message_factory.h" #include "control_message_factory.h"
#include "file_configuration.h" #include "file_configuration.h"
#include "gnss_flowgraph.h" #include "gnss_flowgraph.h"
@ -49,6 +50,10 @@
#include "gps_almanac.h" #include "gps_almanac.h"
#include "glonass_gnav_ephemeris.h" #include "glonass_gnav_ephemeris.h"
#include "glonass_gnav_utc_model.h" #include "glonass_gnav_utc_model.h"
#include "geofunctions.h"
#include "rtklib_rtkcmn.h"
#include "rtklib_conversions.h"
#include "rtklib_ephemeris.h"
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <boost/chrono.hpp> #include <boost/chrono.hpp>
#include <glog/logging.h> #include <glog/logging.h>
@ -693,6 +698,10 @@ void ControlThread::process_control_messages()
} }
else else
{ {
if (control_messages_->at(i)->who == 300) //some TC commands require also actions from controlthread
{
apply_action(control_messages_->at(i)->what);
}
flowgraph_->apply_action(control_messages_->at(i)->who, control_messages_->at(i)->what); flowgraph_->apply_action(control_messages_->at(i)->who, control_messages_->at(i)->what);
} }
processed_control_messages_++; processed_control_messages_++;
@ -704,6 +713,8 @@ void ControlThread::process_control_messages()
void ControlThread::apply_action(unsigned int what) void ControlThread::apply_action(unsigned int what)
{ {
std::shared_ptr<PvtInterface> pvt_ptr;
std::vector<std::pair<int, Gnss_Satellite>> visible_satellites;
switch (what) switch (what)
{ {
case 0: case 0:
@ -717,12 +728,148 @@ void ControlThread::apply_action(unsigned int what)
restart_ = true; restart_ = true;
applied_actions_++; applied_actions_++;
break; break;
case 12:
DLOG(INFO) << "Receiver action HOTSTART";
visible_satellites = get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
//reorder the satellite queue to acquire first those visible satellites
flowgraph_->priorize_satellites(visible_satellites);
//start again the satellite acquisitions (done in chained applyaction to flowgraph)
break;
case 13:
DLOG(INFO) << "Receiver action WARMSTART";
//delete all ephemeris and almanac information from maps (also the PVT map queue)
pvt_ptr = flowgraph_->get_pvt();
pvt_ptr->clear_ephemeris();
//load the ephemeris and the almanac from XML files (receiver assistance)
read_assistance_from_XML();
//call here the function that computes the set of visible satellites and its elevation
//for the date and time specified by the warmstart command and the assisted position
get_visible_sats(cmd_interface_.get_utc_time(), cmd_interface_.get_LLH());
//reorder the satellite queue to acquire first those visible satellites
flowgraph_->priorize_satellites(visible_satellites);
//start again the satellite acquisitions (done in chained applyaction to flowgraph)
break;
default: default:
DLOG(INFO) << "Unrecognized action."; DLOG(INFO) << "Unrecognized action.";
break; break;
} }
} }
std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time_t rx_utc_time, arma::vec LLH)
{
//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
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
// 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;
std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
std::map<int, Gps_Ephemeris> gps_eph_map = pvt_ptr->get_gps_ephemeris();
for (std::map<int, Gps_Ephemeris>::iterator it = gps_eph_map.begin(); it != gps_eph_map.end(); ++it)
{
eph_t rtklib_eph = eph_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
&sat_pos_variance_m2);
double Az, El, dist_m;
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);
std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
}
}
std::map<int, Galileo_Ephemeris> gal_eph_map = pvt_ptr->get_galileo_ephemeris();
for (std::map<int, Galileo_Ephemeris>::iterator it = gal_eph_map.begin(); it != gal_eph_map.end(); ++it)
{
eph_t rtklib_eph = eph_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
eph2pos(gps_gtime, &rtklib_eph, &r_sat[0], &clock_bias_s,
&sat_pos_variance_m2);
double Az, El, dist_m;
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);
std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
}
}
std::map<int, Gps_Almanac> gps_alm_map = pvt_ptr->get_gps_almanac();
for (std::map<int, Gps_Almanac>::iterator it = gps_alm_map.begin(); it != gps_alm_map.end(); ++it)
{
alm_t rtklib_alm = alm_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
double Az, El, dist_m;
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);
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
}
}
std::map<int, Galileo_Almanac> gal_alm_map = pvt_ptr->get_galileo_almanac();
for (std::map<int, Galileo_Almanac>::iterator it = gal_alm_map.begin(); it != gal_alm_map.end(); ++it)
{
alm_t rtklib_alm = alm_to_rtklib(it->second);
double r_sat[3];
double clock_bias_s;
double sat_pos_variance_m2;
alm2pos(gps_gtime, &rtklib_alm, &r_sat[0], &clock_bias_s);
double Az, El, dist_m;
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);
std::cout << "Using Galileo Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
}
}
//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());
return available_satellites;
}
void ControlThread::gps_acq_assist_data_collector() void ControlThread::gps_acq_assist_data_collector()
{ {

View File

@ -35,16 +35,17 @@
#ifndef GNSS_SDR_CONTROL_THREAD_H_ #ifndef GNSS_SDR_CONTROL_THREAD_H_
#define GNSS_SDR_CONTROL_THREAD_H_ #define GNSS_SDR_CONTROL_THREAD_H_
#include "gnss_satellite.h"
#include "control_message_factory.h" #include "control_message_factory.h"
#include "gnss_sdr_supl_client.h" #include "gnss_sdr_supl_client.h"
#include "tcp_cmd_interface.h" #include "tcp_cmd_interface.h"
#include "gnss_flowgraph.h"
#include "configuration_interface.h"
#include <boost/thread.hpp> #include <boost/thread.hpp>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
#include <memory> #include <memory>
#include <vector> #include <vector>
#include <armadillo>
class GNSSFlowgraph;
class ConfigurationInterface;
/*! /*!
@ -143,6 +144,12 @@ private:
*/ */
void gps_acq_assist_data_collector(); void gps_acq_assist_data_collector();
/*
* 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, arma::vec LLH);
/* /*
* Read initial GNSS assistance from SUPL server or local XML files * Read initial GNSS assistance from SUPL server or local XML files
*/ */

View File

@ -1137,10 +1137,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
break; break;
case 12: //request hotstart mode case 12: //request hotstart mode
LOG(INFO) << "TC request hotstart"; LOG(INFO) << "TC request hotstart";
//todo: call here the function that computes the set of visible satellites and its elevation
//for the date and time specified by the hotstart command and the last available PVT
//todo: reorder the satellite queue to acquire first those visible satellites
//start again the satellite acquisitions
for (unsigned int i = 0; i < channels_count_; i++) for (unsigned int i = 0; i < channels_count_; i++)
{ {
unsigned int ch_index = (who + i + 1) % channels_count_; unsigned int ch_index = (who + i + 1) % channels_count_;
@ -1169,12 +1165,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
break; break;
case 13: //request warmstart mode case 13: //request warmstart mode
LOG(INFO) << "TC request warmstart"; LOG(INFO) << "TC request warmstart";
//todo: delete all ephemeris and almanac information from maps (also the PVT map queue)
//todo: load the ephemeris and the almanac from XML files (receiver assistance)
//todo: call here the function that computes the set of visible satellites and its elevation
//for the date and time specified by the warmstart command and the assisted position
//todo: reorder the satellite queue to acquire first those visible satellites
//start again the satellite acquisitions //start again the satellite acquisitions
for (unsigned int i = 0; i < channels_count_; i++) for (unsigned int i = 0; i < channels_count_; i++)
{ {
@ -1209,6 +1199,60 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
} }
void GNSSFlowgraph::priorize_satellites(std::vector<std::pair<int, Gnss_Satellite>> visible_satellites)
{
size_t old_size;
Gnss_Signal gs;
for (std::vector<std::pair<int, Gnss_Satellite>>::iterator it = visible_satellites.begin(); it != visible_satellites.end(); ++it)
{
if (it->second.get_system() == "GPS")
{
gs = Gnss_Signal(it->second, "1C");
old_size = available_GPS_1C_signals_.size();
available_GPS_1C_signals_.remove(gs);
if (old_size > available_GPS_1C_signals_.size())
{
available_GPS_1C_signals_.push_front(gs);
}
gs = Gnss_Signal(it->second, "2S");
old_size = available_GPS_2S_signals_.size();
available_GPS_2S_signals_.remove(gs);
if (old_size > available_GPS_2S_signals_.size())
{
available_GPS_2S_signals_.push_front(gs);
}
gs = Gnss_Signal(it->second, "L5");
old_size = available_GPS_L5_signals_.size();
available_GPS_L5_signals_.remove(gs);
if (old_size > available_GPS_L5_signals_.size())
{
available_GPS_L5_signals_.push_front(gs);
}
}
else if (it->second.get_system() == "Galileo")
{
gs = Gnss_Signal(it->second, "1B");
old_size = available_GAL_1B_signals_.size();
available_GAL_1B_signals_.remove(gs);
if (old_size > available_GAL_1B_signals_.size())
{
available_GAL_1B_signals_.push_front(gs);
}
gs = Gnss_Signal(it->second, "5X");
old_size = available_GAL_5X_signals_.size();
available_GAL_5X_signals_.remove(gs);
if (old_size > available_GAL_5X_signals_.size())
{
available_GAL_5X_signals_.push_front(gs);
}
}
}
}
void GNSSFlowgraph::set_configuration(std::shared_ptr<ConfigurationInterface> configuration) void GNSSFlowgraph::set_configuration(std::shared_ptr<ConfigurationInterface> configuration)
{ {
if (running_) if (running_)

View File

@ -41,6 +41,11 @@
#include "gnss_signal.h" #include "gnss_signal.h"
#include "gnss_sdr_sample_counter.h" #include "gnss_sdr_sample_counter.h"
#include "gnss_synchro_monitor.h" #include "gnss_synchro_monitor.h"
#include "gnss_block_interface.h"
#include "pvt_interface.h"
#include "channel_interface.h"
#include "configuration_interface.h"
#include "gnss_block_factory.h"
#include <gnuradio/top_block.h> #include <gnuradio/top_block.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
#include <list> #include <list>
@ -55,10 +60,6 @@
#include "gnss_sdr_fpga_sample_counter.h" #include "gnss_sdr_fpga_sample_counter.h"
#endif #endif
class GNSSBlockInterface;
class ChannelInterface;
class ConfigurationInterface;
class GNSSBlockFactory;
/*! \brief This class represents a GNSS flow graph. /*! \brief This class represents a GNSS flow graph.
* *
@ -129,6 +130,19 @@ public:
*/ */
bool send_telemetry_msg(pmt::pmt_t msg); bool send_telemetry_msg(pmt::pmt_t msg);
/*!
* \brief Returns a smart pointer to the PVT object
*/
std::shared_ptr<PvtInterface> get_pvt()
{
return std::dynamic_pointer_cast<PvtInterface>(pvt_);
}
/*!
* \brief Priorize visible satellites in the specified vector
*/
void priorize_satellites(std::vector<std::pair<int, Gnss_Satellite>> visible_satellites);
private: private:
void init(); // Populates the SV PRN list available for acquisition and tracking void init(); // Populates the SV PRN list available for acquisition and tracking
void set_signals_list(); void set_signals_list();

View File

@ -34,6 +34,15 @@
#include <functional> #include <functional>
time_t TcpCmdInterface::get_utc_time()
{
return receiver_utc_time_;
}
arma::vec TcpCmdInterface::get_LLH()
{
return arma::vec{rx_latitude_, rx_longitude_, rx_altitude_};
}
std::string TcpCmdInterface::reset(const std::vector<std::string> &commandLine) std::string TcpCmdInterface::reset(const std::vector<std::string> &commandLine)
{ {
std::string response; std::string response;
@ -78,17 +87,40 @@ std::string TcpCmdInterface::status(const std::vector<std::string> &commandLine)
std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine) std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine)
{ {
std::string response; std::string response;
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS if (commandLine.size() > 5)
//todo: store it in a member variable
std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (control_queue_ != nullptr)
{ {
control_queue_->handle(cmf->GetQueueMessage(300, 12)); //send the standby message (who=300,what=12) //read commandline time parameter
response = "OK\n"; struct tm tm;
strptime(commandLine.at(1).c_str(), "%d/%m/%Y %H:%M:%S", &tm);
receiver_utc_time_ = timegm(&tm);
//read latitude, longitude, and height
rx_latitude_ = std::stod(commandLine.at(3).c_str());
rx_longitude_ = std::stod(commandLine.at(4).c_str());
rx_altitude_ = std::stod(commandLine.at(5).c_str());
if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_))
{
response = "ERROR: position malformed\n";
}
else
{
std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (control_queue_ != nullptr)
{
control_queue_->handle(cmf->GetQueueMessage(300, 12)); //send the standby message (who=300,what=12)
response = "OK\n";
}
else
{
response = "ERROR\n";
}
}
} }
else else
{ {
response = "ERROR\n"; response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n";
} }
return response; return response;
} }
@ -96,17 +128,40 @@ std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLin
std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLine) std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLine)
{ {
std::string response; std::string response;
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS if (commandLine.size() > 5)
//todo: store it in a member variable
std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (control_queue_ != nullptr)
{ {
control_queue_->handle(cmf->GetQueueMessage(300, 13)); //send the standby message (who=300,what=13) std::string tmp_str;
response = "OK\n"; //read commandline time parameter
struct tm tm;
tmp_str = commandLine.at(1) + commandLine.at(2);
strptime(tmp_str.c_str(), "%d/%m/%Y %H:%M:%S", &tm);
receiver_utc_time_ = timegm(&tm);
//read latitude, longitude, and height
rx_latitude_ = std::stod(commandLine.at(3).c_str());
rx_longitude_ = std::stod(commandLine.at(4).c_str());
rx_altitude_ = std::stod(commandLine.at(5).c_str());
if (std::isnan(rx_latitude_) || std::isnan(rx_longitude_) || std::isnan(rx_altitude_))
{
response = "ERROR: position malformed\n";
}
else
{
std::unique_ptr<ControlMessageFactory> cmf(new ControlMessageFactory());
if (control_queue_ != nullptr)
{
control_queue_->handle(cmf->GetQueueMessage(300, 13)); //send the standby message (who=300,what=13)
response = "OK\n";
}
else
{
response = "ERROR\n";
}
}
} }
else else
{ {
response = "ERROR\n"; response = "ERROR: time parameter not found, please use warmstart %d/%m/%Y %H:%M:%S Lat Long H\n";
} }
return response; return response;
} }
@ -154,6 +209,10 @@ TcpCmdInterface::TcpCmdInterface()
register_functions(); register_functions();
keep_running_ = true; keep_running_ = true;
control_queue_ = nullptr; control_queue_ = nullptr;
rx_latitude_ = 0;
rx_longitude_ = 0;
rx_altitude_ = 0;
receiver_utc_time_ = 0;
} }

View File

@ -42,7 +42,8 @@
#include <cstdint> #include <cstdint>
#include <gnuradio/message.h> #include <gnuradio/message.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
#include <armadillo>
#include "time.h"
class TcpCmdInterface class TcpCmdInterface
{ {
@ -51,6 +52,14 @@ public:
virtual ~TcpCmdInterface(); virtual ~TcpCmdInterface();
void run_cmd_server(int tcp_port); void run_cmd_server(int tcp_port);
void set_msg_queue(gr::msg_queue::sptr control_queue); void set_msg_queue(gr::msg_queue::sptr control_queue);
/*!
* \brief gets the UTC time parsed from the last TC command issued
*/
time_t get_utc_time();
/*!
* \brief gets the Latitude, Longitude and Altitude vector from the last TC command issued
*/
arma::vec get_LLH();
private: private:
@ -68,6 +77,12 @@ private:
gr::msg_queue::sptr control_queue_; gr::msg_queue::sptr control_queue_;
bool keep_running_; bool keep_running_;
time_t receiver_utc_time_;
double rx_latitude_;
double rx_longitude_;
double rx_altitude_;
}; };
#endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */ #endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */

View File

@ -50,6 +50,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/core/system_parameters ${CMAKE_SOURCE_DIR}/src/core/system_parameters
${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/interfaces
${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/core/receiver
${CMAKE_SOURCE_DIR}/src/core/monitor
${CMAKE_SOURCE_DIR}/src/core/libs ${CMAKE_SOURCE_DIR}/src/core/libs
${CMAKE_SOURCE_DIR}/src/core/libs/supl ${CMAKE_SOURCE_DIR}/src/core/libs/supl
${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp ${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp

View File

@ -18,7 +18,6 @@
set(SYSTEM_TESTING_LIB_SOURCES set(SYSTEM_TESTING_LIB_SOURCES
geofunctions.cc
spirent_motion_csv_dump_reader.cc spirent_motion_csv_dump_reader.cc
rtklib_solver_dump_reader.cc rtklib_solver_dump_reader.cc
) )