mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 20:20:35 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into fix-nmea
This commit is contained in:
commit
9bf8f14a54
@ -48,6 +48,12 @@ namespace bc = boost::integer;
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
|
||||
void RtklibPvt::clear_ephemeris()
|
||||
{
|
||||
pvt_->clear_ephemeris();
|
||||
}
|
||||
|
||||
RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
@ -515,6 +521,22 @@ RtklibPvt::~RtklibPvt()
|
||||
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)
|
||||
{
|
||||
|
@ -63,6 +63,13 @@ public:
|
||||
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 disconnect(gr::top_block_sptr top_block) override;
|
||||
gr::basic_block_sptr get_left_block() override;
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
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,
|
||||
const Pvt_Conf& conf_,
|
||||
|
@ -31,7 +31,7 @@
|
||||
#ifndef GNSS_SDR_RTKLIB_PVT_CC_H
|
||||
#define GNSS_SDR_RTKLIB_PVT_CC_H
|
||||
|
||||
|
||||
#include "gps_ephemeris.h"
|
||||
#include "nmea_printer.h"
|
||||
#include "kml_printer.h"
|
||||
#include "gpx_printer.h"
|
||||
@ -142,11 +142,22 @@ public:
|
||||
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
|
||||
|
||||
|
@ -59,7 +59,6 @@ bool ChannelFsm::Event_stop_channel()
|
||||
switch (d_state)
|
||||
{
|
||||
case 0: //already in stanby
|
||||
return true;
|
||||
break;
|
||||
case 1: //acquisition
|
||||
d_state = 0;
|
||||
@ -70,8 +69,9 @@ bool ChannelFsm::Event_stop_channel()
|
||||
stop_tracking();
|
||||
break;
|
||||
default:
|
||||
return true;
|
||||
break;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ChannelFsm::Event_start_acquisition()
|
||||
|
@ -39,6 +39,7 @@ set(GNSS_SPLIBS_SOURCES
|
||||
conjugate_sc.cc
|
||||
conjugate_ic.cc
|
||||
gnss_sdr_create_directory.cc
|
||||
geofunctions.cc
|
||||
)
|
||||
|
||||
set(GNSS_SPLIBS_HEADERS
|
||||
|
@ -291,3 +291,55 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
|
||||
|
||||
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;
|
||||
}
|
||||
|
@ -38,10 +38,16 @@
|
||||
#include "gps_cnav_ephemeris.h"
|
||||
#include "glonass_gnav_ephemeris.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 Gps_Ephemeris& gps_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
|
||||
* \param glonass_gnav_eph GLONASS GNAV Ephemeris structure
|
||||
|
@ -38,6 +38,10 @@
|
||||
#define GNSS_SDR_PVT_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.
|
||||
@ -52,6 +56,11 @@ 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;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_PVT_INTERFACE_H_ */
|
||||
|
@ -35,6 +35,7 @@
|
||||
#include "control_thread.h"
|
||||
#include "concurrent_queue.h"
|
||||
#include "concurrent_map.h"
|
||||
#include "pvt_interface.h"
|
||||
#include "control_message_factory.h"
|
||||
#include "file_configuration.h"
|
||||
#include "gnss_flowgraph.h"
|
||||
@ -49,6 +50,10 @@
|
||||
#include "gps_almanac.h"
|
||||
#include "glonass_gnav_ephemeris.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/chrono.hpp>
|
||||
#include <glog/logging.h>
|
||||
@ -693,6 +698,10 @@ void ControlThread::process_control_messages()
|
||||
}
|
||||
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);
|
||||
}
|
||||
processed_control_messages_++;
|
||||
@ -704,6 +713,8 @@ void ControlThread::process_control_messages()
|
||||
|
||||
void ControlThread::apply_action(unsigned int what)
|
||||
{
|
||||
std::shared_ptr<PvtInterface> pvt_ptr;
|
||||
std::vector<std::pair<int, Gnss_Satellite>> visible_satellites;
|
||||
switch (what)
|
||||
{
|
||||
case 0:
|
||||
@ -717,12 +728,148 @@ void ControlThread::apply_action(unsigned int what)
|
||||
restart_ = true;
|
||||
applied_actions_++;
|
||||
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:
|
||||
DLOG(INFO) << "Unrecognized action.";
|
||||
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()
|
||||
{
|
||||
|
@ -35,16 +35,17 @@
|
||||
#ifndef GNSS_SDR_CONTROL_THREAD_H_
|
||||
#define GNSS_SDR_CONTROL_THREAD_H_
|
||||
|
||||
#include "gnss_satellite.h"
|
||||
#include "control_message_factory.h"
|
||||
#include "gnss_sdr_supl_client.h"
|
||||
#include "tcp_cmd_interface.h"
|
||||
#include "gnss_flowgraph.h"
|
||||
#include "configuration_interface.h"
|
||||
#include <boost/thread.hpp>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
class GNSSFlowgraph;
|
||||
class ConfigurationInterface;
|
||||
#include <armadillo>
|
||||
|
||||
|
||||
/*!
|
||||
@ -143,6 +144,12 @@ private:
|
||||
*/
|
||||
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
|
||||
*/
|
||||
|
@ -1137,10 +1137,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
break;
|
||||
case 12: //request hotstart mode
|
||||
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++)
|
||||
{
|
||||
unsigned int ch_index = (who + i + 1) % channels_count_;
|
||||
@ -1169,12 +1165,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
|
||||
break;
|
||||
case 13: //request warmstart mode
|
||||
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
|
||||
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)
|
||||
{
|
||||
if (running_)
|
||||
|
@ -41,6 +41,11 @@
|
||||
#include "gnss_signal.h"
|
||||
#include "gnss_sdr_sample_counter.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/msg_queue.h>
|
||||
#include <list>
|
||||
@ -55,10 +60,6 @@
|
||||
#include "gnss_sdr_fpga_sample_counter.h"
|
||||
#endif
|
||||
|
||||
class GNSSBlockInterface;
|
||||
class ChannelInterface;
|
||||
class ConfigurationInterface;
|
||||
class GNSSBlockFactory;
|
||||
|
||||
/*! \brief This class represents a GNSS flow graph.
|
||||
*
|
||||
@ -129,6 +130,19 @@ public:
|
||||
*/
|
||||
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:
|
||||
void init(); // Populates the SV PRN list available for acquisition and tracking
|
||||
void set_signals_list();
|
||||
|
@ -34,6 +34,15 @@
|
||||
#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 response;
|
||||
@ -78,8 +87,25 @@ std::string TcpCmdInterface::status(const std::vector<std::string> &commandLine)
|
||||
std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine)
|
||||
{
|
||||
std::string response;
|
||||
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS
|
||||
//todo: store it in a member variable
|
||||
if (commandLine.size() > 5)
|
||||
{
|
||||
//read commandline time parameter
|
||||
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)
|
||||
{
|
||||
@ -90,14 +116,37 @@ std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLin
|
||||
{
|
||||
response = "ERROR\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
response = "ERROR: time parameter not found, please use hotstart %d/%m/%Y %H:%M:%S\n";
|
||||
}
|
||||
return response;
|
||||
}
|
||||
|
||||
std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLine)
|
||||
{
|
||||
std::string response;
|
||||
//todo: read and parse the command line parameter: dd/mm/yyyy HH:MM:SS
|
||||
//todo: store it in a member variable
|
||||
if (commandLine.size() > 5)
|
||||
{
|
||||
std::string tmp_str;
|
||||
//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)
|
||||
{
|
||||
@ -108,6 +157,12 @@ std::string TcpCmdInterface::warmstart(const std::vector<std::string> &commandLi
|
||||
{
|
||||
response = "ERROR\n";
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
response = "ERROR: time parameter not found, please use warmstart %d/%m/%Y %H:%M:%S Lat Long H\n";
|
||||
}
|
||||
return response;
|
||||
}
|
||||
|
||||
@ -154,6 +209,10 @@ TcpCmdInterface::TcpCmdInterface()
|
||||
register_functions();
|
||||
keep_running_ = true;
|
||||
control_queue_ = nullptr;
|
||||
rx_latitude_ = 0;
|
||||
rx_longitude_ = 0;
|
||||
rx_altitude_ = 0;
|
||||
receiver_utc_time_ = 0;
|
||||
}
|
||||
|
||||
|
||||
|
@ -42,7 +42,8 @@
|
||||
#include <cstdint>
|
||||
#include <gnuradio/message.h>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
|
||||
#include <armadillo>
|
||||
#include "time.h"
|
||||
|
||||
class TcpCmdInterface
|
||||
{
|
||||
@ -51,6 +52,14 @@ public:
|
||||
virtual ~TcpCmdInterface();
|
||||
void run_cmd_server(int tcp_port);
|
||||
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:
|
||||
@ -68,6 +77,12 @@ private:
|
||||
|
||||
gr::msg_queue::sptr control_queue_;
|
||||
bool keep_running_;
|
||||
|
||||
time_t receiver_utc_time_;
|
||||
|
||||
double rx_latitude_;
|
||||
double rx_longitude_;
|
||||
double rx_altitude_;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */
|
||||
|
@ -50,6 +50,7 @@ include_directories(
|
||||
${CMAKE_SOURCE_DIR}/src/core/system_parameters
|
||||
${CMAKE_SOURCE_DIR}/src/core/interfaces
|
||||
${CMAKE_SOURCE_DIR}/src/core/receiver
|
||||
${CMAKE_SOURCE_DIR}/src/core/monitor
|
||||
${CMAKE_SOURCE_DIR}/src/core/libs
|
||||
${CMAKE_SOURCE_DIR}/src/core/libs/supl
|
||||
${CMAKE_SOURCE_DIR}/src/core/libs/supl/asn-rrlp
|
||||
|
@ -18,7 +18,6 @@
|
||||
|
||||
|
||||
set(SYSTEM_TESTING_LIB_SOURCES
|
||||
geofunctions.cc
|
||||
spirent_motion_csv_dump_reader.cc
|
||||
rtklib_solver_dump_reader.cc
|
||||
)
|
||||
|
Loading…
Reference in New Issue
Block a user