mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-15 19:55:47 +00:00
Code Issues Releases Wiki Activity

Fix NMEA printer

Browse Source
This commit is contained in:
Carles Fernandez 2018-11-07 17:02:34 +01:00
commit 6f811dec15
3 changed files with 29 additions and 356 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

369
src/algorithms/PVT/libs/nmea_printer.cc
View File

@ -34,6 +34,7 @@
*/
#include "nmea_printer.h"
#include "rtklib_solution.h"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem/operations.hpp> // for create_directories, exists
#include <boost/filesystem/path.hpp> // for path, operator<<
@ -376,99 +377,10 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
std::string Nmea_Printer::get_GPRMC()
{
// Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10
bool valid_fix = d_PVT_data->is_valid_position();
// ToDo: Compute speed and course over ground
double speed_over_ground_knots = 0;
double course_over_ground_deg = 0;
// boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
std::stringstream sentence_str;
// GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
std::string sentence_header;
sentence_header = "$GPRMC,";
sentence_str << sentence_header;
// UTC Time: hhmmss.sss
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
// Status: A: data valid, V: data NOT valid
if (valid_fix == true)
{
sentence_str << ",A";
}
else
{
sentence_str << ",V";
};
if (print_avg_pos == true)
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
}
else
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
}
// Speed over ground (knots)
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.precision(2);
sentence_str << speed_over_ground_knots;
// course over ground (degrees)
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.precision(2);
sentence_str << course_over_ground_deg;
// Date ddmmyy
boost::gregorian::date sentence_date = d_PVT_data->get_position_UTC_time().date();
unsigned int year = sentence_date.year();
unsigned int day = sentence_date.day();
unsigned int month = sentence_date.month();
sentence_str << ",";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << day;
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << month;
std::stringstream year_strs;
year_strs << std::dec << year;
sentence_str << std::dec << year_strs.str().substr(2);
// Magnetic Variation (degrees)
// ToDo: Implement magnetic compass
sentence_str << ",";
// Magnetic Variation (E or W)
// ToDo: Implement magnetic compass
sentence_str << ",";
// Checksum
char checksum;
std::string tmpstr;
tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1));
sentence_str << "*";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
sentence_str << "\r\n";
unsigned char buff[1024] = {0};
outnmea_rmc(buff, &d_PVT_data->pvt_sol);
sentence_str << buff;
return sentence_str.str();
}
@ -477,82 +389,10 @@ std::string Nmea_Printer::get_GPGSA()
{
// $GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
// GSA-GNSS DOP and Active Satellites
bool valid_fix = d_PVT_data->is_valid_position();
int n_sats_used = d_PVT_data->get_num_valid_observations();
double pdop = d_PVT_data->get_pdop();
double hdop = d_PVT_data->get_hdop();
double vdop = d_PVT_data->get_vdop();
std::stringstream sentence_str;
std::string sentence_header;
sentence_header = "$GPGSA,";
sentence_str << sentence_header;
// mode1:
// (M) Manual-forced to operate in 2D or 3D mode
// (A) Automatic-allowed to automatically switch 2D/3D
std::string mode1 = "M";
sentence_str << mode1;
// mode2:
// 1 fix not available
// 2 fix 2D
// 3 fix 3D
if (valid_fix == true)
{
sentence_str << ",3";
}
else
{
sentence_str << ",1";
};
// Used satellites
for (int i = 0; i < 12; i++)
{
sentence_str << ",";
if (i < n_sats_used)
{
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << d_PVT_data->get_visible_satellites_ID(i);
}
}
// PDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << pdop;
// HDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << hdop;
// VDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << vdop;
// Checksum
char checksum;
std::string tmpstr;
tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1));
sentence_str << "*";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
sentence_str << "\r\n";
unsigned char buff[1024] = {0};
outnmea_gsa(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
sentence_str << buff;
return sentence_str.str();
}
@ -560,199 +400,22 @@ std::string Nmea_Printer::get_GPGSA()
std::string Nmea_Printer::get_GPGSV()
{
// GSV-GNSS Satellites in View
// Notice that NMEA 2.1 only supports 12 channels
int n_sats_used = d_PVT_data->get_num_valid_observations();
std::stringstream sentence_str;
std::stringstream frame_str;
std::string sentence_header;
sentence_header = "$GPGSV,";
char checksum;
std::string tmpstr;
// 1st step: How many GPGSV frames we need? (up to 3)
// Each frame contains up to 4 satellites
int n_frames;
n_frames = std::ceil((static_cast<double>(n_sats_used)) / 4.0);
// generate the frames
int current_satellite = 0;
for (int i = 1; i < (n_frames + 1); i++)
{
frame_str.str("");
frame_str << sentence_header;
// number of messages
frame_str << n_frames;
// message number
frame_str << ",";
frame_str << i;
// total number of satellites in view
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << n_sats_used;
// satellites info
for (int j = 0; j < 4; j++)
{
// write satellite info
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << d_PVT_data->get_visible_satellites_ID(current_satellite);
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_El(current_satellite));
frame_str << ",";
frame_str.width(3);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_Az(current_satellite));
frame_str << ",";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::dec << static_cast<int>(d_PVT_data->get_visible_satellites_CN0_dB(current_satellite));
current_satellite++;
if (current_satellite == n_sats_used)
{
break;
}
}
// frame checksum
tmpstr = frame_str.str();
checksum = checkSum(tmpstr.substr(1));
frame_str << "*";
frame_str.width(2);
frame_str.fill('0');
frame_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
frame_str << "\r\n";
//add frame to sentence
sentence_str << frame_str.str();
}
return sentence_str.str();
// $GPGSV,2,1,07,07,79,048,42,02,51,062,43,26,36,256,42,27,27,138,42*71
// Notice that NMEA 2.1 only supports 12 channels
std::stringstream sentence_str;
unsigned char buff[1024] = {0};
outnmea_gsv(buff, &d_PVT_data->pvt_sol, d_PVT_data->pvt_ssat);
sentence_str << buff;
return sentence_str.str();
}
std::string Nmea_Printer::get_GPGGA()
{
// boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
bool valid_fix = d_PVT_data->is_valid_position();
int n_channels = d_PVT_data->get_num_valid_observations(); //d_nchannels
double hdop = d_PVT_data->get_hdop();
double MSL_altitude;
if (d_PVT_data->is_averaging() == true)
{
MSL_altitude = d_PVT_data->get_avg_height();
}
else
{
MSL_altitude = d_PVT_data->get_height();
}
std::stringstream sentence_str;
// GPGGA (Global Positioning System Fixed Data)
std::string sentence_header;
sentence_header = "$GPGGA,";
sentence_str << sentence_header;
// UTC Time: hhmmss.sss
sentence_str << get_UTC_NMEA_time(d_PVT_data->get_position_UTC_time());
if (d_PVT_data->is_averaging() == true)
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_avg_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_avg_longitude());
}
else
{
// Latitude ddmm.mmmm,(N or S)
sentence_str << "," << latitude_to_hm(d_PVT_data->get_latitude());
// longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->get_longitude());
}
// Position fix indicator
// 0 - Fix not available or invalid
// 1 - GPS SPS Mode, fix valid
// 2 - Differential GPS, SPS Mode, fix valid
// 3-5 - Not supported
// 6 - Dead Reckoning Mode, fix valid
// ToDo: Update PVT module to identify the fix mode
if (valid_fix == true)
{
sentence_str << ",1";
}
else
{
sentence_str << ",0";
}
// Number of satellites used in PVT
sentence_str << ",";
if (n_channels < 10)
{
sentence_str << '0' << n_channels;
}
else
{
sentence_str << n_channels;
}
// HDOP
sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2);
sentence_str.precision(1);
sentence_str.fill('0');
sentence_str << hdop;
// MSL Altitude
sentence_str << ",";
sentence_str.precision(1);
sentence_str << MSL_altitude;
sentence_str << ",M";
// Geoid-to-ellipsoid separation. Ellipsoid altitude = MSL Altitude + Geoid Separation.
// ToDo: Compute this value
sentence_str << ",";
sentence_str << "0.0";
sentence_str << ",M";
// Age of Diff. Corr. (Seconds) Null fields when DGPS is not used
// Diff. Ref. Station ID (0000)
// ToDo: Implement this fields for Differential GPS
sentence_str << ",";
sentence_str << "0.0,0000";
// Checksum
char checksum;
std::string tmpstr;
tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1));
sentence_str << "*";
sentence_str.width(2);
sentence_str.fill('0');
sentence_str << std::hex << static_cast<int>(checksum);
// end NMEA sentence
sentence_str << "\r\n";
unsigned char buff[1024] = {0};
outnmea_gga(buff, &d_PVT_data->pvt_sol);
sentence_str << buff;
return sentence_str.str();
// $GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
}

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

@ -53,6 +53,7 @@
#include "rtklib_solver.h"
#include "rtklib_conversions.h"
#include "rtklib_solution.h"
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"
#include "GLONASS_L1_L2_CA.h"
@ -74,7 +75,11 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
rtk_ = rtk;
for (unsigned int i = 0; i < 4; i++) dop_[i] = 0.0;
pvt_sol = {{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};
ssat_t ssat0 = {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}}}, {{}, {}}};
for (unsigned int i = 0; i < MAXSAT; i++)
{
pvt_ssat[i] = ssat0;
}
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
@ -794,7 +799,11 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
unsigned int used_sats = 0;
for (unsigned int i = 0; i < MAXSAT; i++)
{
if (rtk_.ssat[i].vs == 1) used_sats++;
if (rtk_.ssat[i].vs == 1)
{
pvt_ssat[i] = rtk_.ssat[i];
used_sats++;
}
}
std::vector<double> azel;
@ -809,8 +818,8 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
index_aux++;
}
}
if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
if (index_aux > 0) dops(index_aux, azel.data(), 0.0, dop_);
this->set_valid_position(true);
arma::vec rx_position_and_time(4);
rx_position_and_time(0) = pvt_sol.rr[0]; // [m]

1
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -86,6 +86,7 @@ private:
public:
sol_t pvt_sol;
ssat_t pvt_ssat[MAXSAT];
rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, bool flag_dump_to_mat, rtk_t& rtk);
~rtklib_solver();