mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Adding Tele Command status for reporting the Position, Speed and Course over ground and Time

This commit is contained in:
Javier 2018-11-06 14:39:57 +01:00
parent 18db62dfd3
commit ea3db59fd7
12 changed files with 204 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,6 +49,21 @@ namespace bc = boost::integer;
using google::LogMessage;
bool RtklibPvt::get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time)
{
return pvt_->get_latest_PVT(longitude_deg,
latitude_deg,
height_m,
ground_speed_kmh,
course_over_ground_deg,
UTC_time);
}
void RtklibPvt::clear_ephemeris()
{
pvt_->clear_ephemeris();

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

@ -86,6 +86,13 @@ public:
return sizeof(gr_complex);
}
bool get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time);
private:
rtklib_pvt_cc_sptr pvt_;
rtk_t rtk;

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

@ -47,6 +47,7 @@
#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;
@ -893,9 +894,36 @@ bool rtklib_pvt_cc::load_gnss_synchro_map_xml(const std::string file_name)
}
bool rtklib_pvt_cc::get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time)
{
gr::thread::scoped_lock lock(d_setlock);
if (d_ls_pvt->is_valid_position())
{
*latitude_deg = d_ls_pvt->get_latitude();
*longitude_deg = d_ls_pvt->get_longitude();
*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());
return true;
}
else
{
return false;
}
}
int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_items,
gr_vector_void_star& output_items __attribute__((unused)))
{
gr::thread::scoped_lock l(d_setlock);
for (int32_t epoch = 0; epoch < noutput_items; epoch++)
{
bool flag_display_pvt = false;

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

@ -60,7 +60,7 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t n_channels,
rtk_t& rtk);
/*!
* \brief This class implements a block that computes the PVT solution with Galileo E1 signals
* \brief This class implements a block that computes the PVT solution using the RTKLIB integrated library
*/
class rtklib_pvt_cc : public gr::sync_block
{
@ -159,6 +159,17 @@ public:
*/
void clear_ephemeris();
/*!
* \brief Get the latest Position WGS84 [deg], Ground Velocity, Course over Ground, and UTC Time, if available
*/
bool get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time);
~rtklib_pvt_cc(); //!< Default destructor
int work(int noutput_items, gr_vector_const_void_star& input_items,

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

@ -43,6 +43,8 @@ Pvt_Solution::Pvt_Solution()
d_latitude_d = 0.0;
d_longitude_d = 0.0;
d_height_m = 0.0;
d_speed_over_ground_m_s = 0.0;
d_course_over_ground_d = 0.0;
d_avg_latitude_d = 0.0;
d_avg_longitude_d = 0.0;
d_avg_height_m = 0.0;
@ -126,6 +128,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
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
return 0;
}
@ -516,6 +519,25 @@ double Pvt_Solution::get_height() const
return d_height_m;
}
double Pvt_Solution::get_speed_over_ground() const
{
return d_speed_over_ground_m_s;
}
void Pvt_Solution::set_speed_over_ground(double speed_m_s)
{
d_speed_over_ground_m_s = speed_m_s;
}
void Pvt_Solution::set_course_over_ground(double cog_deg)
{
d_course_over_ground_d = cog_deg;
}
double Pvt_Solution::get_course_over_ground() const
{
return d_course_over_ground_d;
}
double Pvt_Solution::get_avg_latitude() const
{

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

@ -49,9 +49,11 @@ class 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]
double d_speed_over_ground_m_s; // RX speed over ground [m/s]
double d_course_over_ground_d; // RX course over ground [deg]
double d_avg_latitude_d; // Averaged latitude in degrees
double d_avg_longitude_d; // Averaged longitude in degrees
@ -86,6 +88,12 @@ public:
double get_longitude() const; //!< Get RX position Longitude WGS84 [deg]
double get_height() const; //!< Get RX position height WGS84 [m]
double get_speed_over_ground() const; //!< Get RX speed over ground [m/s]
void set_speed_over_ground(double speed_m_s); //!< Set RX speed over ground [m/s]
double get_course_over_ground() const; //!< Get RX course over ground [deg]
void set_course_over_ground(double cog_deg); //!< Set RX course over ground [deg]
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]

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

@ -827,6 +827,22 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
rx_position_and_time(3) = pvt_sol.dtr[0] / GPS_C_m_s; // the receiver clock offset is expressed in [meters], so we convert it into [s]
}
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
//compute Ground speed and COG
double ground_speed_ms = 0.0;
double pos[3];
double enuv[3];
ecef2pos(pvt_sol.rr, pos);
ecef2enu(pos, &pvt_sol.rr[3], enuv);
this->set_speed_over_ground(norm_rtk(enuv, 2));
double new_cog;
if (ground_speed_ms >= 1.0)
{
new_cog = atan2(enuv[0], enuv[1]) * R2D;
if (new_cog < 0.0) new_cog += 360.0;
this->set_course_over_ground(new_cog);
}
//observable fix:
//double offset_s = this->get_time_offset_s();
//this->set_time_offset_s(offset_s + (rx_position_and_time(3) / GPS_C_m_s)); // accumulate the rx time error for the next iteration [meters]->[seconds]

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

@ -61,6 +61,13 @@ public:
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 bool get_latest_PVT(double* longitude_deg,
double* latitude_deg,
double* height_m,
double* ground_speed_kmh,
double* course_over_ground_deg,
time_t* UTC_time) = 0;
};
#endif /* GNSS_SDR_PVT_INTERFACE_H_ */

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

@ -161,6 +161,7 @@ int ControlThread::run()
sysv_queue_thread_ = boost::thread(&ControlThread::sysv_queue_listener, this);
//start the telecommand listener thread
cmd_interface_.set_pvt(flowgraph_->get_pvt());
cmd_interface_thread_ = boost::thread(&ControlThread::telecommand_listener, this);
@ -718,25 +719,33 @@ void ControlThread::apply_action(unsigned int what)
switch (what)
{
case 0:
DLOG(INFO) << "Received action STOP";
LOG(INFO) << "Received action STOP";
stop_ = true;
applied_actions_++;
break;
case 1:
DLOG(INFO) << "Received action RESTART";
LOG(INFO) << "Received action RESTART";
stop_ = true;
restart_ = true;
applied_actions_++;
break;
case 11:
LOG(INFO) << "Receiver action COLDSTART";
//delete all ephemeris and almanac information from maps (also the PVT map queue)
pvt_ptr = flowgraph_->get_pvt();
pvt_ptr->clear_ephemeris();
//todo: reorder the satellite queues to the receiver default startup order.
//This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
break;
case 12:
DLOG(INFO) << "Receiver action HOTSTART";
LOG(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";
LOG(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();
@ -750,7 +759,7 @@ void ControlThread::apply_action(unsigned int what)
//start again the satellite acquisitions (done in chained applyaction to flowgraph)
break;
default:
DLOG(INFO) << "Unrecognized action.";
LOG(INFO) << "Unrecognized action.";
break;
}
}
@ -775,6 +784,13 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
std::vector<std::pair<int, Gnss_Satellite>> available_satellites;
std::shared_ptr<PvtInterface> pvt_ptr = flowgraph_->get_pvt();
struct tm tstruct;
char buf[80];
tstruct = *gmtime(&rx_utc_time);
strftime(buf, sizeof(buf), "%d/%m/%Y %H:%M:%S ", &tstruct);
std::string str_time = std::string(buf);
std::cout << "Get visible satellites at " << str_time
<< " UTC, assuming RX position " << LLH(0) << " [deg], " << LLH(1) << " [deg], " << LLH(2) << " [m]" << std::endl;
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)
@ -789,10 +805,10 @@ 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);
std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
std::cout << "Using GPS Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
}
@ -811,10 +827,10 @@ 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);
std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
std::cout << "Using Galileo Ephemeris: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
}
@ -832,10 +848,10 @@ 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);
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
//push sat
if (El > 0)
{
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("GPS"), it->second.i_satellite_PRN))));
}
@ -857,6 +873,7 @@ std::vector<std::pair<int, Gnss_Satellite>> ControlThread::get_visible_sats(time
//push sat
if (El > 0)
{
std::cout << "Using GPS Almanac: Sat " << it->second.i_satellite_PRN << " Az: " << Az << " El: " << El << std::endl;
available_satellites.push_back(std::pair<int, Gnss_Satellite>(floor(El),
(Gnss_Satellite(std::string("Galileo"), it->second.i_satellite_PRN))));
}

10
src/core/receiver/gnss_flowgraph.cc
View File

@ -1103,11 +1103,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
acq_channels_count_ = 0; //all channels are in stanby now
break;
case 11: //request coldstart mode
LOG(INFO) << "TC request coldstart";
//todo: delete all ephemeris and almanac information from maps (also the PVT map queue)
//todo: reorder the satellite queues to the receiver default startup order.
//This is required to allow repeatability. Otherwise the satellite search order will depend on the last tracked satellites
LOG(INFO) << "TC request flowgraph coldstart";
//start again the satellite acquisitions
for (unsigned int i = 0; i < channels_count_; i++)
{
@ -1136,7 +1132,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
}
break;
case 12: //request hotstart mode
LOG(INFO) << "TC request hotstart";
LOG(INFO) << "TC request flowgraph hotstart";
for (unsigned int i = 0; i < channels_count_; i++)
{
unsigned int ch_index = (who + i + 1) % channels_count_;
@ -1164,7 +1160,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what)
}
break;
case 13: //request warmstart mode
LOG(INFO) << "TC request warmstart";
LOG(INFO) << "TC request flowgraph warmstart";
//start again the satellite acquisitions
for (unsigned int i = 0; i < channels_count_; i++)
{

56
src/core/receiver/tcp_cmd_interface.cc
View File

@ -32,8 +32,13 @@
#include "tcp_cmd_interface.h"
#include "control_message_factory.h"
#include <functional>
#include <sstream>
void TcpCmdInterface::set_pvt(std::shared_ptr<PvtInterface> PVT_sptr)
{
PVT_sptr_ = PVT_sptr;
}
time_t TcpCmdInterface::get_utc_time()
{
return receiver_utc_time_;
@ -78,10 +83,51 @@ std::string TcpCmdInterface::standby(const std::vector<std::string> &commandLine
std::string TcpCmdInterface::status(const std::vector<std::string> &commandLine)
{
std::string response;
std::stringstream str_stream;
//todo: implement the receiver status report
response = "Not implemented\n";
return response;
// str_stream << "-------------------------------------------------------\n";
// str_stream << "ch | sys | sig | mode | Tlm | Eph | Doppler | CN0 |\n";
// str_stream << " | | | | | | [Hz] | [dB - Hz] |\n";
// str_stream << "-------------------------------------------------------\n";
// int n_ch = 10;
// for (int n = 0; n < n_ch; n++)
// {
// str_stream << n << "GPS | L1CA | TRK | YES | YES | 23412.4 | 44.3 |\n";
// }
// str_stream << "--------------------------------------------------------\n";
double longitude_deg, latitude_deg, height_m, ground_speed_kmh, course_over_ground_deg;
time_t UTC_time;
if (PVT_sptr_->get_latest_PVT(&longitude_deg,
&latitude_deg,
&height_m,
&ground_speed_kmh,
&course_over_ground_deg,
&UTC_time) == true)
{
struct tm tstruct;
char buf1[80];
tstruct = *gmtime(&UTC_time);
strftime(buf1, sizeof(buf1), "%d/%m/%Y %H:%M:%S", &tstruct);
std::string str_time = std::string(buf1);
str_stream << "- Receiver UTC Time: " << str_time << std::endl;
str_stream << std::setprecision(9);
str_stream << "- Receiver Position WGS84 [Lat, Long, H]: "
<< latitude_deg << ", "
<< longitude_deg << ", ";
str_stream << std::setprecision(3);
str_stream << height_m << std::endl;
str_stream << std::setprecision(1);
str_stream << "- Receiver Speed over Ground [km/h]: " << ground_speed_kmh << std::endl;
str_stream << "- Receiver Course over ground [deg]: " << course_over_ground_deg << std::endl;
}
else
{
str_stream << "No PVT information available.\n";
}
return str_stream.str();
}
std::string TcpCmdInterface::hotstart(const std::vector<std::string> &commandLine)
@ -277,6 +323,10 @@ void TcpCmdInterface::run_cmd_server(int tcp_port)
response = functions[cmd_vector.at(0)](cmd_vector);
}
}
catch (const std::bad_function_call &ex)
{
response = "ERROR: command not found \n ";
}
catch (const std::exception &ex)
{
response = "ERROR: command execution error: " + std::string(ex.what()) + "\n";

6
src/core/receiver/tcp_cmd_interface.h
View File

@ -31,6 +31,7 @@
#ifndef GNSS_SDR_TCPCMDINTERFACE_H_
#define GNSS_SDR_TCPCMDINTERFACE_H_
#include "pvt_interface.h"
#include <functional>
#include <iostream>
#include <string>
@ -43,7 +44,7 @@
#include <gnuradio/message.h>
#include <gnuradio/msg_queue.h>
#include <armadillo>
#include "time.h"
#include <time.h>
class TcpCmdInterface
{
@ -61,6 +62,7 @@ public:
*/
arma::vec get_LLH();
void set_pvt(std::shared_ptr<PvtInterface> PVT_sptr);
private:
std::unordered_map<std::string, std::function<std::string(const std::vector<std::string> &)>>
@ -83,6 +85,8 @@ private:
double rx_latitude_;
double rx_longitude_;
double rx_altitude_;
std::shared_ptr<PvtInterface> PVT_sptr_;
};
#endif /* GNSS_SDR_TCPCMDINTERFACE_H_ */