mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-06-27 23:52:53 +00:00
Code cleaning
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@263 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez
parent
ae4a867249
commit
9b62455366
@ -56,53 +56,38 @@ gps_l1_ca_make_pvt_cc(unsigned int n_channels, gr_msg_queue_sptr queue, bool dum
|
||||
*/
|
||||
class gps_l1_ca_pvt_cc : public gr_block
|
||||
{
|
||||
|
||||
private:
|
||||
|
||||
friend gps_l1_ca_pvt_cc_sptr
|
||||
gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname);
|
||||
|
||||
gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname);
|
||||
|
||||
gr_msg_queue_sptr d_queue;
|
||||
bool d_dump;
|
||||
bool b_rinex_header_writen;
|
||||
Rinex_Printer *rp;
|
||||
|
||||
unsigned int d_nchannels;
|
||||
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
|
||||
int d_averaging_depth;
|
||||
bool d_flag_averaging;
|
||||
int d_output_rate_ms;
|
||||
int d_display_rate_ms;
|
||||
long unsigned int d_sample_counter;
|
||||
|
||||
Kml_Printer d_kml_dump;
|
||||
|
||||
Nmea_Printer *d_nmea_printer;
|
||||
|
||||
concurrent_queue<Gps_Navigation_Message> *d_nav_queue; // Navigation ephemeris queue
|
||||
Gps_Navigation_Message d_last_nav_msg; // Last navigation message
|
||||
|
||||
double d_ephemeris_clock_s;
|
||||
double d_ephemeris_timestamp_ms;
|
||||
double d_tx_time;
|
||||
gps_l1_ca_ls_pvt *d_ls_pvt;
|
||||
|
||||
std::map<int,Gps_Navigation_Message> nav_data_map;
|
||||
|
||||
public:
|
||||
|
||||
~gps_l1_ca_pvt_cc (); //!< Default destructor
|
||||
|
||||
/*!
|
||||
* \brief Set the queue for getting navigation messages from the GpsL1CaTelemetryDecoder
|
||||
*/
|
||||
void set_navigation_queue(concurrent_queue<Gps_Navigation_Message> *nav_queue){d_nav_queue=nav_queue;}
|
||||
|
||||
int general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing
|
||||
};
|
||||
|
||||
@ -47,8 +47,8 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool
|
||||
d_dump_filename = dump_filename;
|
||||
d_flag_dump_enabled = flag_dump_to_file;
|
||||
d_averaging_depth = 0;
|
||||
d_GPS_current_time=0;;
|
||||
b_valid_position=false;
|
||||
d_GPS_current_time = 0;
|
||||
b_valid_position = false;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_flag_dump_enabled == true)
|
||||
{
|
||||
@ -152,7 +152,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
||||
A(i, j) = 0.0; //Armadillo
|
||||
}
|
||||
omc(i, 0) = 0.0;
|
||||
az(0, i)= 0.0;
|
||||
az(0, i) = 0.0;
|
||||
}
|
||||
el = az;
|
||||
arma::mat X = satpos;
|
||||
|
||||
@ -38,18 +38,15 @@ bool Kml_Printer::set_headers(std::string filename)
|
||||
{
|
||||
time_t rawtime;
|
||||
struct tm * timeinfo;
|
||||
|
||||
time ( &rawtime );
|
||||
timeinfo = localtime ( &rawtime );
|
||||
kml_file.open(filename.c_str());
|
||||
if (kml_file.is_open())
|
||||
{
|
||||
DLOG(INFO) << "KML printer writing on " << filename.c_str();
|
||||
|
||||
// Set iostream numeric format and precision
|
||||
kml_file.setf(kml_file.fixed,kml_file.floatfield);
|
||||
kml_file<<std::setprecision(14);
|
||||
|
||||
kml_file << std::setprecision(14);
|
||||
kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
|
||||
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
|
||||
<< " <Document>" << std::endl
|
||||
@ -83,6 +80,7 @@ bool Kml_Printer::set_headers(std::string filename)
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool Kml_Printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_values)
|
||||
{
|
||||
double latitude;
|
||||
@ -90,15 +88,15 @@ bool Kml_Printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v
|
||||
double height;
|
||||
if (print_average_values == false)
|
||||
{
|
||||
latitude=position->d_latitude_d;
|
||||
longitude=position->d_longitude_d;
|
||||
height=position->d_height_m;
|
||||
latitude = position->d_latitude_d;
|
||||
longitude = position->d_longitude_d;
|
||||
height = position->d_height_m;
|
||||
}
|
||||
else
|
||||
{
|
||||
latitude=position->d_avg_latitude_d;
|
||||
longitude=position->d_avg_longitude_d;
|
||||
height=position->d_avg_height_m;
|
||||
latitude = position->d_avg_latitude_d;
|
||||
longitude = position->d_avg_longitude_d;
|
||||
height = position->d_avg_height_m;
|
||||
}
|
||||
|
||||
if (kml_file.is_open())
|
||||
@ -118,11 +116,11 @@ bool Kml_Printer::close_file()
|
||||
{
|
||||
if (kml_file.is_open())
|
||||
{
|
||||
kml_file<<"</coordinates>" << std::endl
|
||||
<<"</LineString>" << std::endl
|
||||
<<"</Placemark>" << std::endl
|
||||
<<"</Document>" << std::endl
|
||||
<<"</kml>";
|
||||
kml_file << "</coordinates>" << std::endl
|
||||
<< "</LineString>" << std::endl
|
||||
<< "</Placemark>" << std::endl
|
||||
<< "</Document>" << std::endl
|
||||
<< "</kml>";
|
||||
kml_file.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -46,17 +46,11 @@
|
||||
class Kml_Printer
|
||||
{
|
||||
private:
|
||||
|
||||
std::ofstream kml_file;
|
||||
|
||||
public:
|
||||
|
||||
bool set_headers(std::string filename);
|
||||
|
||||
bool print_position(gps_l1_ca_ls_pvt* position, bool print_average_values);
|
||||
|
||||
bool close_file();
|
||||
|
||||
Kml_Printer();
|
||||
~Kml_Printer();
|
||||
};
|
||||
|
||||
@ -48,27 +48,31 @@ using google::LogMessage;
|
||||
|
||||
Nmea_Printer::Nmea_Printer(std::string filename, bool flag_nmea_tty_port, std::string nmea_dump_devname)
|
||||
{
|
||||
nmea_filename=filename;
|
||||
nmea_filename = filename;
|
||||
nmea_file_descriptor.open(nmea_filename.c_str(), std::ios::out);
|
||||
if (nmea_file_descriptor.is_open())
|
||||
{
|
||||
DLOG(INFO) << "NMEA printer writing on " << nmea_filename.c_str();
|
||||
}
|
||||
|
||||
nmea_devname=nmea_dump_devname;
|
||||
if (flag_nmea_tty_port==true)
|
||||
nmea_devname = nmea_dump_devname;
|
||||
if (flag_nmea_tty_port == true)
|
||||
{
|
||||
|
||||
nmea_dev_descriptor=init_serial(nmea_devname.c_str());
|
||||
if (nmea_dev_descriptor!=-1)
|
||||
nmea_dev_descriptor = init_serial(nmea_devname.c_str());
|
||||
if (nmea_dev_descriptor != -1)
|
||||
{
|
||||
DLOG(INFO) << "NMEA printer writing on " << nmea_devname.c_str();
|
||||
}
|
||||
}else{
|
||||
nmea_dev_descriptor=-1;
|
||||
}
|
||||
else
|
||||
{
|
||||
nmea_dev_descriptor = -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Nmea_Printer::~Nmea_Printer()
|
||||
{
|
||||
if (nmea_file_descriptor.is_open())
|
||||
@ -79,13 +83,15 @@ Nmea_Printer::~Nmea_Printer()
|
||||
}
|
||||
|
||||
|
||||
int Nmea_Printer::init_serial (std::string serial_device) {
|
||||
|
||||
|
||||
int Nmea_Printer::init_serial (std::string serial_device)
|
||||
{
|
||||
/*!
|
||||
* Opens the serial device and sets the deffault baud rate for a NMEA transmission (9600,8,N,1)
|
||||
* Opens the serial device and sets the default baud rate for a NMEA transmission (9600,8,N,1)
|
||||
*/
|
||||
int fd = 0;
|
||||
struct termios options;
|
||||
|
||||
long BAUD;
|
||||
long DATABITS;
|
||||
long STOPBITS;
|
||||
@ -115,6 +121,8 @@ int Nmea_Printer::init_serial (std::string serial_device) {
|
||||
return fd;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Nmea_Printer::close_serial ()
|
||||
{
|
||||
if (nmea_dev_descriptor != -1)
|
||||
@ -123,40 +131,41 @@ void Nmea_Printer::close_serial ()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool Nmea_Printer::Print_Nmea_Line(gps_l1_ca_ls_pvt* pvt_data, bool print_average_values)
|
||||
{
|
||||
|
||||
std::string GPRMC;
|
||||
std::string GPGGA;
|
||||
std::string GPGSA;
|
||||
std::string GPGSV;
|
||||
|
||||
// set the new PVT data
|
||||
d_PVT_data=pvt_data;
|
||||
d_PVT_data = pvt_data;
|
||||
|
||||
// generate the NMEA sentences
|
||||
|
||||
//GPRMC
|
||||
GPRMC=get_GPRMC();
|
||||
GPRMC = get_GPRMC();
|
||||
//GPGGA (Global Positioning System Fixed Data)
|
||||
GPGGA=get_GPGGA();
|
||||
GPGGA = get_GPGGA();
|
||||
//GPGSA
|
||||
GPGSA=get_GPGSA();
|
||||
GPGSA = get_GPGSA();
|
||||
//GPGSV
|
||||
GPGSV=get_GPGSV();
|
||||
GPGSV = get_GPGSV();
|
||||
|
||||
// write to log file
|
||||
try{
|
||||
try
|
||||
{
|
||||
//GPRMC
|
||||
nmea_file_descriptor<<GPRMC;
|
||||
nmea_file_descriptor << GPRMC;
|
||||
//GPGGA (Global Positioning System Fixed Data)
|
||||
nmea_file_descriptor<<GPGGA;
|
||||
nmea_file_descriptor << GPGGA;
|
||||
//GPGSA
|
||||
nmea_file_descriptor<<GPGSA;
|
||||
nmea_file_descriptor << GPGSA;
|
||||
//GPGSV
|
||||
nmea_file_descriptor<<GPGSV;
|
||||
|
||||
}catch(std::exception ex)
|
||||
nmea_file_descriptor << GPGSV;
|
||||
}
|
||||
catch(std::exception ex)
|
||||
{
|
||||
DLOG(INFO) << "NMEA printer can not write on output file" << nmea_filename.c_str();;
|
||||
}
|
||||
@ -164,36 +173,42 @@ bool Nmea_Printer::Print_Nmea_Line(gps_l1_ca_ls_pvt* pvt_data, bool print_averag
|
||||
//write to serial device
|
||||
if (nmea_dev_descriptor!=-1)
|
||||
{
|
||||
try{
|
||||
try
|
||||
{
|
||||
int n_bytes_written;
|
||||
//GPRMC
|
||||
n_bytes_written=write(nmea_dev_descriptor, GPRMC.c_str(), GPRMC.length());
|
||||
n_bytes_written = write(nmea_dev_descriptor, GPRMC.c_str(), GPRMC.length());
|
||||
//GPGGA (Global Positioning System Fixed Data)
|
||||
n_bytes_written=write(nmea_dev_descriptor, GPGGA.c_str(), GPGGA.length());
|
||||
n_bytes_written = write(nmea_dev_descriptor, GPGGA.c_str(), GPGGA.length());
|
||||
//GPGSA
|
||||
n_bytes_written=write(nmea_dev_descriptor, GPGSA.c_str(), GPGSA.length());
|
||||
n_bytes_written = write(nmea_dev_descriptor, GPGSA.c_str(), GPGSA.length());
|
||||
//GPGSV
|
||||
n_bytes_written=write(nmea_dev_descriptor, GPGSV.c_str(), GPGSV.length());
|
||||
|
||||
}catch(std::exception ex)
|
||||
n_bytes_written = write(nmea_dev_descriptor, GPGSV.c_str(), GPGSV.length());
|
||||
}
|
||||
catch(std::exception ex)
|
||||
{
|
||||
DLOG(INFO) << "NMEA printer can not write on serial device" << nmea_filename.c_str();;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
char Nmea_Printer::checkSum(std::string sentence) {
|
||||
|
||||
|
||||
char Nmea_Printer::checkSum(std::string sentence)
|
||||
{
|
||||
char check = 0;
|
||||
// iterate over the string, XOR each byte with the total sum:
|
||||
for (unsigned int c = 0; c < sentence.length(); c++) {
|
||||
for (unsigned int c = 0; c < sentence.length(); c++)
|
||||
{
|
||||
check = char(check ^ sentence.at(c));
|
||||
}
|
||||
// return the result
|
||||
return check;
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::string Nmea_Printer::latitude_to_hm(double lat)
|
||||
{
|
||||
bool north;
|
||||
@ -201,7 +216,9 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
|
||||
{
|
||||
north = false;
|
||||
lat = -lat ;
|
||||
}else{
|
||||
}
|
||||
else
|
||||
{
|
||||
north = true;
|
||||
}
|
||||
|
||||
@ -215,17 +232,21 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
|
||||
out_string << deg;
|
||||
out_string.width(6);
|
||||
out_string.precision(4);
|
||||
out_string<<mins;
|
||||
out_string << mins;
|
||||
|
||||
if (north==true)
|
||||
if (north == true)
|
||||
{
|
||||
out_string << ",N";
|
||||
}else{
|
||||
}
|
||||
else
|
||||
{
|
||||
out_string << ",S";
|
||||
}
|
||||
return out_string.str();
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::string Nmea_Printer::longitude_to_hm(double longitude)
|
||||
{
|
||||
bool east;
|
||||
@ -233,10 +254,11 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
|
||||
{
|
||||
east = false;
|
||||
longitude = -longitude ;
|
||||
}else{
|
||||
east=true;
|
||||
}
|
||||
|
||||
else
|
||||
{
|
||||
east = true;
|
||||
}
|
||||
int deg = (int)longitude;
|
||||
double mins = longitude - (double)deg;
|
||||
mins *= 60.0 ;
|
||||
@ -247,15 +269,16 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
|
||||
out_string << deg;
|
||||
out_string.width(6);
|
||||
out_string.precision(4);
|
||||
out_string<<mins;
|
||||
out_string << mins;
|
||||
|
||||
if (east==true)
|
||||
{
|
||||
out_string << ",E";
|
||||
}else{
|
||||
}
|
||||
else
|
||||
{
|
||||
out_string << ",W";
|
||||
}
|
||||
|
||||
return out_string.str();
|
||||
}
|
||||
|
||||
@ -266,16 +289,16 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
|
||||
//UTC Time: hhmmss.sss
|
||||
std::stringstream sentence_str;
|
||||
|
||||
boost::posix_time::time_duration td=d_position_UTC_time.time_of_day();
|
||||
boost::posix_time::time_duration td = d_position_UTC_time.time_of_day();
|
||||
int utc_hours;
|
||||
int utc_mins;
|
||||
int utc_seconds;
|
||||
int utc_milliseconds;
|
||||
|
||||
utc_hours=td.hours();
|
||||
utc_mins=td.minutes();
|
||||
utc_seconds=td.seconds();
|
||||
utc_milliseconds=td.total_milliseconds()-td.total_seconds()*1000;
|
||||
utc_hours = td.hours();
|
||||
utc_mins = td.minutes();
|
||||
utc_seconds = td.seconds();
|
||||
utc_milliseconds = td.total_milliseconds() - td.total_seconds()*1000;
|
||||
|
||||
if (utc_hours < 10) sentence_str << "0"; // two digits for hours
|
||||
sentence_str << utc_hours;
|
||||
@ -295,22 +318,25 @@ std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_
|
||||
{
|
||||
sentence_str << ".0"; // three digits for ms
|
||||
sentence_str << utc_milliseconds;
|
||||
}else
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << "."; // three digits for ms
|
||||
sentence_str << utc_milliseconds;
|
||||
}
|
||||
return sentence_str.str();
|
||||
}
|
||||
|
||||
|
||||
|
||||
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->b_valid_position;
|
||||
bool valid_fix = d_PVT_data->b_valid_position;
|
||||
|
||||
// ToDo: Compute speed and course over ground
|
||||
double speed_over_ground_knots=0;
|
||||
double course_over_ground_deg=0;
|
||||
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();
|
||||
|
||||
@ -318,148 +344,141 @@ std::string Nmea_Printer::get_GPRMC()
|
||||
|
||||
//GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
|
||||
std::string sentence_header;
|
||||
sentence_header="$GPRMC,";
|
||||
sentence_str<<sentence_header;
|
||||
sentence_header = "$GPRMC,";
|
||||
sentence_str << sentence_header;
|
||||
|
||||
//UTC Time: hhmmss.sss
|
||||
sentence_str<<get_UTC_NMEA_time(d_PVT_data->d_position_UTC_time);
|
||||
sentence_str << get_UTC_NMEA_time(d_PVT_data->d_position_UTC_time);
|
||||
|
||||
//Status: A: data valid, V: data NOT valid
|
||||
|
||||
if (valid_fix==true)
|
||||
if (valid_fix == true)
|
||||
{
|
||||
sentence_str<<",A";
|
||||
}else{
|
||||
sentence_str<<",V";
|
||||
sentence_str << ",A";
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << ",V";
|
||||
};
|
||||
|
||||
if (d_PVT_data->d_flag_averaging==true)
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
|
||||
sentence_str<<","<<latitude_to_hm(d_PVT_data->d_avg_latitude_d);
|
||||
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->d_avg_latitude_d);
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_avg_longitude_d);
|
||||
}else{
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->d_avg_longitude_d);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
|
||||
sentence_str<<","<<latitude_to_hm(d_PVT_data->d_latitude_d);
|
||||
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->d_latitude_d);
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_longitude_d);
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->d_longitude_d);
|
||||
}
|
||||
|
||||
//Speed over ground (knots)
|
||||
sentence_str<<",";
|
||||
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 << ",";
|
||||
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->d_position_UTC_time.date();
|
||||
unsigned int year=sentence_date.year();
|
||||
unsigned int day=sentence_date.day();
|
||||
unsigned int month=sentence_date.month();
|
||||
unsigned int year = sentence_date.year();
|
||||
unsigned int day = sentence_date.day();
|
||||
unsigned int month = sentence_date.month();
|
||||
|
||||
sentence_str<<",";
|
||||
sentence_str << ",";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str<<day;
|
||||
sentence_str << day;
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str<<month;
|
||||
sentence_str << month;
|
||||
|
||||
std::stringstream year_strs;
|
||||
year_strs<<std::dec<<year;
|
||||
sentence_str<<std::dec<<year_strs.str().substr(2);
|
||||
year_strs << std::dec << year;
|
||||
sentence_str << std::dec << year_strs.str().substr(2);
|
||||
|
||||
//Magnetic Variation (degrees)
|
||||
// ToDo: Implement magnetic compass
|
||||
sentence_str<<",";
|
||||
sentence_str << ",";
|
||||
|
||||
//Magnetic Variation (E or W)
|
||||
// ToDo: Implement magnetic compass
|
||||
sentence_str<<",";
|
||||
sentence_str << ",";
|
||||
|
||||
// Checksum
|
||||
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
tmpstr=sentence_str.str();
|
||||
|
||||
tmpstr = sentence_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
sentence_str<<"*";
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str<<std::hex<<(int)checksum;
|
||||
sentence_str << std::hex << (int)checksum;
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str<<"\r\n";
|
||||
|
||||
sentence_str << "\r\n";
|
||||
return sentence_str.str();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
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->b_valid_position;
|
||||
|
||||
int n_sats_used=d_PVT_data->d_valid_observations;
|
||||
|
||||
double pdop=d_PVT_data->d_PDOP;
|
||||
double hdop=d_PVT_data->d_HDOP;
|
||||
double vdop=d_PVT_data->d_VDOP;
|
||||
|
||||
bool valid_fix = d_PVT_data->b_valid_position;
|
||||
int n_sats_used = d_PVT_data->d_valid_observations;
|
||||
double pdop = d_PVT_data->d_PDOP;
|
||||
double hdop = d_PVT_data->d_HDOP;
|
||||
double vdop = d_PVT_data->d_VDOP;
|
||||
|
||||
std::stringstream sentence_str;
|
||||
std::string sentence_header;
|
||||
sentence_header="$GPGSA,";
|
||||
sentence_str<<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";
|
||||
std::string mode1 = "M";
|
||||
sentence_str << mode1;
|
||||
|
||||
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";
|
||||
sentence_str << ",3";
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << ",1";
|
||||
};
|
||||
|
||||
// Used satellites
|
||||
for (int i=0; i<12;i++)
|
||||
for (int i=0; i<12; i++)
|
||||
{
|
||||
sentence_str<<",";
|
||||
if (i<n_sats_used)
|
||||
sentence_str << ",";
|
||||
if (i < n_sats_used)
|
||||
{
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str<<d_PVT_data->d_visible_satellites_IDs[i];
|
||||
sentence_str << d_PVT_data->d_visible_satellites_IDs[i];
|
||||
}
|
||||
}
|
||||
|
||||
// PDOP
|
||||
sentence_str<<",";
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.width(2);
|
||||
sentence_str.precision(1);
|
||||
@ -472,9 +491,8 @@ std::string Nmea_Printer::get_GPGSA()
|
||||
sentence_str.precision(1);
|
||||
sentence_str.fill('0');
|
||||
sentence_str << hdop;
|
||||
|
||||
//VDOP
|
||||
sentence_str<<",";
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.width(2);
|
||||
sentence_str.precision(1);
|
||||
@ -482,130 +500,128 @@ std::string Nmea_Printer::get_GPGSA()
|
||||
sentence_str << vdop;
|
||||
|
||||
// Checksum
|
||||
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
tmpstr=sentence_str.str();
|
||||
|
||||
tmpstr = sentence_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
sentence_str<<"*";
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str<<std::hex<<(int)checksum;
|
||||
sentence_str << std::hex << (int)checksum;
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str<<"\r\n";
|
||||
|
||||
return sentence_str.str();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
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->d_valid_observations;
|
||||
|
||||
std::stringstream sentence_str;
|
||||
std::stringstream frame_str;
|
||||
std::string sentence_header;
|
||||
sentence_header="$GPGSV,";
|
||||
|
||||
sentence_header = "$GPGSV,";
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
|
||||
// 1st step: How many GPGSV frames we need? (up to 3)
|
||||
// Each frame countains up to 4 satellites
|
||||
// Each frame contains up to 4 satellites
|
||||
int n_frames;
|
||||
n_frames=std::ceil(((double)n_sats_used)/4.0);
|
||||
n_frames = std::ceil(((double)n_sats_used) / 4.0);
|
||||
|
||||
// generate the frames
|
||||
int current_satellite=0;
|
||||
for (int i=1;i<(n_frames+1);i++)
|
||||
int current_satellite = 0;
|
||||
for (int i=1; i<(n_frames+1); i++)
|
||||
{
|
||||
|
||||
frame_str.str("");
|
||||
|
||||
frame_str<<sentence_header;
|
||||
frame_str << sentence_header;
|
||||
|
||||
// number of messages
|
||||
frame_str<<n_frames;
|
||||
frame_str << n_frames;
|
||||
|
||||
// message number
|
||||
frame_str<<",";
|
||||
frame_str<<i;
|
||||
frame_str << ",";
|
||||
frame_str << i;
|
||||
|
||||
// total number of satellites in view
|
||||
frame_str<<",";
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str<<std::dec<<n_sats_used;
|
||||
frame_str << std::dec << n_sats_used;
|
||||
|
||||
//satellites info
|
||||
for (int j=0;j<4;j++)
|
||||
for (int j=0; j<4; j++)
|
||||
{
|
||||
// write satellite info
|
||||
frame_str<<",";
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str<<std::dec<<d_PVT_data->d_visible_satellites_IDs[current_satellite];
|
||||
frame_str << std::dec << d_PVT_data->d_visible_satellites_IDs[current_satellite];
|
||||
|
||||
frame_str<<",";
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str<<std::dec<<(int)d_PVT_data->d_visible_satellites_El[current_satellite];
|
||||
frame_str << std::dec << (int)d_PVT_data->d_visible_satellites_El[current_satellite];
|
||||
|
||||
frame_str<<",";
|
||||
frame_str << ",";
|
||||
frame_str.width(3);
|
||||
frame_str.fill('0');
|
||||
frame_str<<std::dec<<(int)d_PVT_data->d_visible_satellites_Az[current_satellite];
|
||||
frame_str << std::dec << (int)d_PVT_data->d_visible_satellites_Az[current_satellite];
|
||||
|
||||
frame_str<<",";
|
||||
frame_str << ",";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str<<std::dec<<(int)d_PVT_data->d_visible_satellites_CN0_dB[current_satellite];
|
||||
frame_str << std::dec << (int)d_PVT_data->d_visible_satellites_CN0_dB[current_satellite];
|
||||
|
||||
current_satellite++;
|
||||
|
||||
if (current_satellite==n_sats_used)
|
||||
if (current_satellite == n_sats_used)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// frame checksum
|
||||
tmpstr=frame_str.str();
|
||||
tmpstr = frame_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
frame_str<<"*";
|
||||
frame_str << "*";
|
||||
frame_str.width(2);
|
||||
frame_str.fill('0');
|
||||
frame_str<<std::hex<<(int)checksum;
|
||||
frame_str << std::hex << (int)checksum;
|
||||
|
||||
// end NMEA sentence
|
||||
frame_str<<"\r\n";
|
||||
frame_str << "\r\n";
|
||||
|
||||
//add frame to sentence
|
||||
sentence_str<<frame_str.str();
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
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->b_valid_position;
|
||||
|
||||
int n_channels=d_PVT_data->d_valid_observations;//d_nchannels
|
||||
double hdop=d_PVT_data->d_HDOP;
|
||||
bool valid_fix = d_PVT_data->b_valid_position;
|
||||
int n_channels = d_PVT_data->d_valid_observations;//d_nchannels
|
||||
double hdop = d_PVT_data->d_HDOP;
|
||||
double MSL_altitude;
|
||||
|
||||
if (d_PVT_data->d_flag_averaging==true)
|
||||
if (d_PVT_data->d_flag_averaging == true)
|
||||
{
|
||||
MSL_altitude=d_PVT_data->d_avg_height_m;
|
||||
}else{
|
||||
}
|
||||
else
|
||||
{
|
||||
MSL_altitude=d_PVT_data->d_height_m;
|
||||
}
|
||||
|
||||
@ -613,31 +629,25 @@ std::string Nmea_Printer::get_GPGGA()
|
||||
|
||||
//GPGGA (Global Positioning System Fixed Data)
|
||||
std::string sentence_header;
|
||||
sentence_header="$GPGGA,";
|
||||
|
||||
sentence_str<<sentence_header;
|
||||
sentence_header = "$GPGGA,";
|
||||
sentence_str << sentence_header;
|
||||
|
||||
//UTC Time: hhmmss.sss
|
||||
sentence_str << get_UTC_NMEA_time(d_PVT_data->d_position_UTC_time);
|
||||
|
||||
sentence_str<<get_UTC_NMEA_time(d_PVT_data->d_position_UTC_time);
|
||||
|
||||
if (d_PVT_data->d_flag_averaging==true)
|
||||
if (d_PVT_data->d_flag_averaging == true)
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
|
||||
sentence_str<<","<<latitude_to_hm(d_PVT_data->d_avg_latitude_d);
|
||||
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->d_avg_latitude_d);
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_avg_longitude_d);
|
||||
}else{
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->d_avg_longitude_d);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Latitude ddmm.mmmm,(N or S)
|
||||
|
||||
sentence_str<<","<<latitude_to_hm(d_PVT_data->d_latitude_d);
|
||||
|
||||
sentence_str << "," << latitude_to_hm(d_PVT_data->d_latitude_d);
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_longitude_d);
|
||||
sentence_str << "," << longitude_to_hm(d_PVT_data->d_longitude_d);
|
||||
}
|
||||
|
||||
// Position fix indicator
|
||||
@ -648,24 +658,28 @@ std::string Nmea_Printer::get_GPGGA()
|
||||
// 6 - Dead Reckoning Mode, fix valid
|
||||
// ToDo: Update PVT module to identify the fix mode
|
||||
|
||||
if (valid_fix==true)
|
||||
if (valid_fix == true)
|
||||
{
|
||||
sentence_str<<",1";
|
||||
}else{
|
||||
sentence_str<<",0";
|
||||
sentence_str << ",1";
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << ",0";
|
||||
}
|
||||
|
||||
// Number of satellites used in PVT
|
||||
sentence_str<<",";
|
||||
if (n_channels<10)
|
||||
sentence_str << ",";
|
||||
if (n_channels < 10)
|
||||
{
|
||||
sentence_str<<'0'<<n_channels;
|
||||
}else{
|
||||
sentence_str<<n_channels;
|
||||
sentence_str << '0' << n_channels;
|
||||
}
|
||||
else
|
||||
{
|
||||
sentence_str << n_channels;
|
||||
}
|
||||
|
||||
// HDOP
|
||||
sentence_str<<",";
|
||||
sentence_str << ",";
|
||||
sentence_str.setf(std::ios::fixed, std::ios::floatfield);
|
||||
sentence_str.width(2);
|
||||
sentence_str.precision(1);
|
||||
@ -673,40 +687,36 @@ std::string Nmea_Printer::get_GPGGA()
|
||||
sentence_str << hdop;
|
||||
|
||||
// MSL Altitude
|
||||
sentence_str<<",";
|
||||
sentence_str << ",";
|
||||
sentence_str.precision(1);
|
||||
sentence_str<<MSL_altitude;
|
||||
sentence_str<<",M";
|
||||
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";
|
||||
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";
|
||||
sentence_str << ",";
|
||||
sentence_str << "0.0,0000";
|
||||
|
||||
// Checksum
|
||||
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
tmpstr=sentence_str.str();
|
||||
|
||||
tmpstr = sentence_str.str();
|
||||
checksum = checkSum(tmpstr.substr(1));
|
||||
sentence_str<<"*";
|
||||
sentence_str << "*";
|
||||
sentence_str.width(2);
|
||||
sentence_str.fill('0');
|
||||
sentence_str<<std::hex<<(int)checksum;
|
||||
sentence_str << std::hex <<(int)checksum;
|
||||
|
||||
// end NMEA sentence
|
||||
sentence_str<<"\r\n";
|
||||
|
||||
sentence_str << "\r\n";
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
@ -59,18 +59,14 @@ public:
|
||||
*/
|
||||
~Nmea_Printer();
|
||||
|
||||
|
||||
private:
|
||||
std::string nmea_filename ; //<! String with the NMEA log filename
|
||||
std::ofstream nmea_file_descriptor ; //<! Output file stream for NMEA log file
|
||||
std::string nmea_devname;
|
||||
int nmea_dev_descriptor ; //<! NMEA serial device descriptor (i.e. COM port)
|
||||
|
||||
gps_l1_ca_ls_pvt* d_PVT_data;
|
||||
|
||||
int init_serial (std::string serial_device); //serial port control
|
||||
void close_serial ();
|
||||
|
||||
std::string get_GPGGA();
|
||||
std::string get_GPGSV();
|
||||
std::string get_GPGSA();
|
||||
@ -79,7 +75,6 @@ private:
|
||||
std::string longitude_to_hm(double longitude);
|
||||
std::string latitude_to_hm(double lat);
|
||||
char checkSum(std::string sentence);
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -14,7 +14,7 @@
|
||||
* 1) The carrier-phase measurement at one or both carriers (actually being a
|
||||
* measurement on the beat frequency between the received carrier of the
|
||||
* satellite signal and a receiver-generated reference frequency).
|
||||
* 2) The pseudorange (code) measuremen , equivalent to the difference
|
||||
* 2) The pseudorange (code) measurement , equivalent to the difference
|
||||
* of the time of reception (expressed in the time frame of the receiver)
|
||||
* and the time of transmission (expressed in the time frame of the satellite)
|
||||
* of a distinct satellite signal.
|
||||
@ -66,8 +66,6 @@
|
||||
*/
|
||||
class Rinex_Printer
|
||||
{
|
||||
|
||||
|
||||
public:
|
||||
/*!
|
||||
* \brief Default constructor. Creates GPS Navigation and Observables RINEX files and their headers
|
||||
@ -106,30 +104,19 @@ public:
|
||||
* \brief Writes observables into the RINEX file
|
||||
*/
|
||||
void log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav_msg, std::map<int,double> pseudoranges);
|
||||
|
||||
std::map<std::string,std::string> satelliteSystem; //<! GPS, GLONASS, SBAS payload, Galileo or Compass
|
||||
std::map<std::string,std::string> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH
|
||||
std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors
|
||||
|
||||
|
||||
std::string stringVersion; //<! RINEX version (2.10/2.11 or 3.01)
|
||||
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
int version ; // RINEX version (2 for 2.10/2.11 and 3 for 3.01)
|
||||
|
||||
int numberTypesObservations; // Number of available types of observable in the system. Should be public?
|
||||
|
||||
|
||||
/*
|
||||
* Generation of RINEX signal strength indicators
|
||||
*/
|
||||
int signalStrength(double snr);
|
||||
|
||||
|
||||
/* Creates RINEX file names according to the naming convention
|
||||
*
|
||||
* See http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf
|
||||
@ -426,7 +413,6 @@ inline std::string& Rinex_Printer::sci2for(std::string& aStr,
|
||||
const std::string::size_type expLen,
|
||||
const bool checkSwitch)
|
||||
{
|
||||
|
||||
std::string::size_type idx = aStr.find('.', startPos);
|
||||
int expAdd = 0;
|
||||
std::string exp;
|
||||
@ -505,19 +491,17 @@ inline std::string& Rinex_Printer::sci2for(std::string& aStr,
|
||||
aStr.insert((std::string::size_type)1, 1, '0');
|
||||
}
|
||||
|
||||
|
||||
return aStr;
|
||||
|
||||
} // end sci2for
|
||||
|
||||
|
||||
|
||||
inline std::string asString(const long double x, const std::string::size_type precision)
|
||||
{
|
||||
{
|
||||
std::ostringstream ss;
|
||||
ss << std::fixed << std::setprecision(precision) << x ;
|
||||
return ss.str();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user