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

Code cleaning

Browse Source 
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 2012-10-28 11:38:50 +00:00
parent ae4a867249
commit 9b62455366
9 changed files with 1131 additions and 1165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h
View File

@ -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 class gps_l1_ca_pvt_cc : public gr_block
{ {
private: private:
friend gps_l1_ca_pvt_cc_sptr 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_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); 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; gr_msg_queue_sptr d_queue;
bool d_dump; bool d_dump;
bool b_rinex_header_writen; bool b_rinex_header_writen;
Rinex_Printer *rp; Rinex_Printer *rp;
unsigned int d_nchannels; unsigned int d_nchannels;
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;
int d_averaging_depth; int d_averaging_depth;
bool d_flag_averaging; bool d_flag_averaging;
int d_output_rate_ms; int d_output_rate_ms;
int d_display_rate_ms; int d_display_rate_ms;
long unsigned int d_sample_counter; long unsigned int d_sample_counter;
Kml_Printer d_kml_dump; Kml_Printer d_kml_dump;
Nmea_Printer *d_nmea_printer; Nmea_Printer *d_nmea_printer;
concurrent_queue<Gps_Navigation_Message> *d_nav_queue; // Navigation ephemeris queue concurrent_queue<Gps_Navigation_Message> *d_nav_queue; // Navigation ephemeris queue
Gps_Navigation_Message d_last_nav_msg; // Last navigation message Gps_Navigation_Message d_last_nav_msg; // Last navigation message
double d_ephemeris_clock_s; double d_ephemeris_clock_s;
double d_ephemeris_timestamp_ms; double d_ephemeris_timestamp_ms;
double d_tx_time; double d_tx_time;
gps_l1_ca_ls_pvt *d_ls_pvt; gps_l1_ca_ls_pvt *d_ls_pvt;
std::map<int,Gps_Navigation_Message> nav_data_map; std::map<int,Gps_Navigation_Message> nav_data_map;
public: public:
~gps_l1_ca_pvt_cc (); //!< Default destructor ~gps_l1_ca_pvt_cc (); //!< Default destructor
/*! /*!
* \brief Set the queue for getting navigation messages from the GpsL1CaTelemetryDecoder * \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;} 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, 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 gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing
}; };

6
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@ -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_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file; d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth = 0; d_averaging_depth = 0;
d_GPS_current_time=0;; d_GPS_current_time = 0;
b_valid_position=false; b_valid_position = false;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true) 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 A(i, j) = 0.0; //Armadillo
} }
omc(i, 0) = 0.0; omc(i, 0) = 0.0;
az(0, i)= 0.0; az(0, i) = 0.0;
} }
el = az; el = az;
arma::mat X = satpos; arma::mat X = satpos;

28
src/algorithms/PVT/libs/kml_printer.cc
View File

@ -38,18 +38,15 @@ bool Kml_Printer::set_headers(std::string filename)
{ {
time_t rawtime; time_t rawtime;
struct tm * timeinfo; struct tm * timeinfo;
time ( &rawtime ); time ( &rawtime );
timeinfo = localtime ( &rawtime ); timeinfo = localtime ( &rawtime );
kml_file.open(filename.c_str()); kml_file.open(filename.c_str());
if (kml_file.is_open()) if (kml_file.is_open())
{ {
DLOG(INFO) << "KML printer writing on " << filename.c_str(); DLOG(INFO) << "KML printer writing on " << filename.c_str();
// Set iostream numeric format and precision // Set iostream numeric format and precision
kml_file.setf(kml_file.fixed,kml_file.floatfield); 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_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl << "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
<< " <Document>" << 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) bool Kml_Printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_values)
{ {
double latitude; double latitude;
@ -90,15 +88,15 @@ bool Kml_Printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v
double height; double height;
if (print_average_values == false) if (print_average_values == false)
{ {
latitude=position->d_latitude_d; latitude = position->d_latitude_d;
longitude=position->d_longitude_d; longitude = position->d_longitude_d;
height=position->d_height_m; height = position->d_height_m;
} }
else else
{ {
latitude=position->d_avg_latitude_d; latitude = position->d_avg_latitude_d;
longitude=position->d_avg_longitude_d; longitude = position->d_avg_longitude_d;
height=position->d_avg_height_m; height = position->d_avg_height_m;
} }
if (kml_file.is_open()) if (kml_file.is_open())
@ -118,11 +116,11 @@ bool Kml_Printer::close_file()
{ {
if (kml_file.is_open()) if (kml_file.is_open())
{ {
kml_file<<"</coordinates>" << std::endl kml_file << "</coordinates>" << std::endl
<<"</LineString>" << std::endl << "</LineString>" << std::endl
<<"</Placemark>" << std::endl << "</Placemark>" << std::endl
<<"</Document>" << std::endl << "</Document>" << std::endl
<<"</kml>"; << "</kml>";
kml_file.close(); kml_file.close();
return true; return true;
} }

6
src/algorithms/PVT/libs/kml_printer.h
View File

@ -46,17 +46,11 @@
class Kml_Printer class Kml_Printer
{ {
private: private:
std::ofstream kml_file; std::ofstream kml_file;
public: public:
bool set_headers(std::string filename); bool set_headers(std::string filename);
bool print_position(gps_l1_ca_ls_pvt* position, bool print_average_values); bool print_position(gps_l1_ca_ls_pvt* position, bool print_average_values);
bool close_file(); bool close_file();
Kml_Printer(); Kml_Printer();
~Kml_Printer(); ~Kml_Printer();
}; };

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

@ -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_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); nmea_file_descriptor.open(nmea_filename.c_str(), std::ios::out);
if (nmea_file_descriptor.is_open()) if (nmea_file_descriptor.is_open())
{ {
DLOG(INFO) << "NMEA printer writing on " << nmea_filename.c_str(); DLOG(INFO) << "NMEA printer writing on " << nmea_filename.c_str();
} }
nmea_devname=nmea_dump_devname; nmea_devname = nmea_dump_devname;
if (flag_nmea_tty_port==true) if (flag_nmea_tty_port == true)
{ {
nmea_dev_descriptor = init_serial(nmea_devname.c_str());
nmea_dev_descriptor=init_serial(nmea_devname.c_str()); if (nmea_dev_descriptor != -1)
if (nmea_dev_descriptor!=-1)
{ {
DLOG(INFO) << "NMEA printer writing on " << nmea_devname.c_str(); DLOG(INFO) << "NMEA printer writing on " << nmea_devname.c_str();
} }
}else{ }
nmea_dev_descriptor=-1; else
{
nmea_dev_descriptor = -1;
} }
} }
Nmea_Printer::~Nmea_Printer() Nmea_Printer::~Nmea_Printer()
{ {
if (nmea_file_descriptor.is_open()) 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; int fd = 0;
struct termios options; struct termios options;
long BAUD; long BAUD;
long DATABITS; long DATABITS;
long STOPBITS; long STOPBITS;
@ -115,6 +121,8 @@ int Nmea_Printer::init_serial (std::string serial_device) {
return fd; return fd;
} }
void Nmea_Printer::close_serial () void Nmea_Printer::close_serial ()
{ {
if (nmea_dev_descriptor != -1) 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) bool Nmea_Printer::Print_Nmea_Line(gps_l1_ca_ls_pvt* pvt_data, bool print_average_values)
{ {
std::string GPRMC; std::string GPRMC;
std::string GPGGA; std::string GPGGA;
std::string GPGSA; std::string GPGSA;
std::string GPGSV; std::string GPGSV;
// set the new PVT data // set the new PVT data
d_PVT_data=pvt_data; d_PVT_data = pvt_data;
// generate the NMEA sentences // generate the NMEA sentences
//GPRMC //GPRMC
GPRMC=get_GPRMC(); GPRMC = get_GPRMC();
//GPGGA (Global Positioning System Fixed Data) //GPGGA (Global Positioning System Fixed Data)
GPGGA=get_GPGGA(); GPGGA = get_GPGGA();
//GPGSA //GPGSA
GPGSA=get_GPGSA(); GPGSA = get_GPGSA();
//GPGSV //GPGSV
GPGSV=get_GPGSV(); GPGSV = get_GPGSV();
// write to log file // write to log file
try{ try
{
//GPRMC //GPRMC
nmea_file_descriptor<<GPRMC; nmea_file_descriptor << GPRMC;
//GPGGA (Global Positioning System Fixed Data) //GPGGA (Global Positioning System Fixed Data)
nmea_file_descriptor<<GPGGA; nmea_file_descriptor << GPGGA;
//GPGSA //GPGSA
nmea_file_descriptor<<GPGSA; nmea_file_descriptor << GPGSA;
//GPGSV //GPGSV
nmea_file_descriptor<<GPGSV; nmea_file_descriptor << GPGSV;
}
}catch(std::exception ex) catch(std::exception ex)
{ {
DLOG(INFO) << "NMEA printer can not write on output file" << nmea_filename.c_str();; 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 //write to serial device
if (nmea_dev_descriptor!=-1) if (nmea_dev_descriptor!=-1)
{ {
try{ try
{
int n_bytes_written; int n_bytes_written;
//GPRMC //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) //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 //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 //GPGSV
n_bytes_written=write(nmea_dev_descriptor, GPGSV.c_str(), GPGSV.length()); n_bytes_written = write(nmea_dev_descriptor, GPGSV.c_str(), GPGSV.length());
}
}catch(std::exception ex) catch(std::exception ex)
{ {
DLOG(INFO) << "NMEA printer can not write on serial device" << nmea_filename.c_str();; DLOG(INFO) << "NMEA printer can not write on serial device" << nmea_filename.c_str();;
} }
} }
return true; return true;
} }
char Nmea_Printer::checkSum(std::string sentence) {
char Nmea_Printer::checkSum(std::string sentence)
{
char check = 0; char check = 0;
// iterate over the string, XOR each byte with the total sum: // 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)); check = char(check ^ sentence.at(c));
} }
// return the result // return the result
return check; return check;
} }
std::string Nmea_Printer::latitude_to_hm(double lat) std::string Nmea_Printer::latitude_to_hm(double lat)
{ {
bool north; bool north;
@ -201,7 +216,9 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
{ {
north = false; north = false;
lat = -lat ; lat = -lat ;
}else{ }
else
{
north = true; north = true;
} }
@ -215,17 +232,21 @@ std::string Nmea_Printer::latitude_to_hm(double lat)
out_string << deg; out_string << deg;
out_string.width(6); out_string.width(6);
out_string.precision(4); out_string.precision(4);
out_string<<mins; out_string << mins;
if (north==true) if (north == true)
{ {
out_string << ",N"; out_string << ",N";
}else{ }
else
{
out_string << ",S"; out_string << ",S";
} }
return out_string.str(); return out_string.str();
} }
std::string Nmea_Printer::longitude_to_hm(double longitude) std::string Nmea_Printer::longitude_to_hm(double longitude)
{ {
bool east; bool east;
@ -233,10 +254,11 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
{ {
east = false; east = false;
longitude = -longitude ; longitude = -longitude ;
}else{
east=true;
} }
else
{
east = true;
}
int deg = (int)longitude; int deg = (int)longitude;
double mins = longitude - (double)deg; double mins = longitude - (double)deg;
mins *= 60.0 ; mins *= 60.0 ;
@ -247,15 +269,16 @@ std::string Nmea_Printer::longitude_to_hm(double longitude)
out_string << deg; out_string << deg;
out_string.width(6); out_string.width(6);
out_string.precision(4); out_string.precision(4);
out_string<<mins; out_string << mins;
if (east==true) if (east==true)
{ {
out_string << ",E"; out_string << ",E";
}else{ }
else
{
out_string << ",W"; out_string << ",W";
} }
return out_string.str(); 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 //UTC Time: hhmmss.sss
std::stringstream sentence_str; 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_hours;
int utc_mins; int utc_mins;
int utc_seconds; int utc_seconds;
int utc_milliseconds; int utc_milliseconds;
utc_hours=td.hours(); utc_hours = td.hours();
utc_mins=td.minutes(); utc_mins = td.minutes();
utc_seconds=td.seconds(); utc_seconds = td.seconds();
utc_milliseconds=td.total_milliseconds()-td.total_seconds()*1000; utc_milliseconds = td.total_milliseconds() - td.total_seconds()*1000;
if (utc_hours < 10) sentence_str << "0"; // two digits for hours if (utc_hours < 10) sentence_str << "0"; // two digits for hours
sentence_str << utc_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 << ".0"; // three digits for ms
sentence_str << utc_milliseconds; sentence_str << utc_milliseconds;
}else }
else
{ {
sentence_str << "."; // three digits for ms sentence_str << "."; // three digits for ms
sentence_str << utc_milliseconds; sentence_str << utc_milliseconds;
} }
return sentence_str.str(); return sentence_str.str();
} }
std::string Nmea_Printer::get_GPRMC() std::string Nmea_Printer::get_GPRMC()
{ {
// Sample -> $GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,*10 // 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 // ToDo: Compute speed and course over ground
double speed_over_ground_knots=0; double speed_over_ground_knots = 0;
double course_over_ground_deg=0; double course_over_ground_deg = 0;
//boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time(); //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) //GPRMC (RMC-Recommended,Minimum Specific GNSS Data)
std::string sentence_header; std::string sentence_header;
sentence_header="$GPRMC,"; sentence_header = "$GPRMC,";
sentence_str<<sentence_header; sentence_str << sentence_header;
//UTC Time: hhmmss.sss //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 //Status: A: data valid, V: data NOT valid
if (valid_fix==true) if (valid_fix == true)
{ {
sentence_str<<",A"; sentence_str << ",A";
}else{ }
sentence_str<<",V"; else
{
sentence_str << ",V";
}; };
if (d_PVT_data->d_flag_averaging==true) if (d_PVT_data->d_flag_averaging==true)
{ {
// Latitude ddmm.mmmm,(N or S) // 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) // longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->d_avg_longitude_d);
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_avg_longitude_d); }
}else{ else
{
// Latitude ddmm.mmmm,(N or S) // 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) // 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) //Speed over ground (knots)
sentence_str<<","; sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield); sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.precision(2); sentence_str.precision(2);
sentence_str << speed_over_ground_knots; sentence_str << speed_over_ground_knots;
//course over ground (degrees) //course over ground (degrees)
sentence_str<<","; sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield); sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.precision(2); sentence_str.precision(2);
sentence_str << course_over_ground_deg; sentence_str << course_over_ground_deg;
// Date ddmmyy // Date ddmmyy
boost::gregorian::date sentence_date = d_PVT_data->d_position_UTC_time.date(); boost::gregorian::date sentence_date = d_PVT_data->d_position_UTC_time.date();
unsigned int year=sentence_date.year(); unsigned int year = sentence_date.year();
unsigned int day=sentence_date.day(); unsigned int day = sentence_date.day();
unsigned int month=sentence_date.month(); unsigned int month = sentence_date.month();
sentence_str<<","; sentence_str << ",";
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str<<day; sentence_str << day;
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str<<month; sentence_str << month;
std::stringstream year_strs; std::stringstream year_strs;
year_strs<<std::dec<<year; year_strs << std::dec << year;
sentence_str<<std::dec<<year_strs.str().substr(2); sentence_str << std::dec << year_strs.str().substr(2);
//Magnetic Variation (degrees) //Magnetic Variation (degrees)
// ToDo: Implement magnetic compass // ToDo: Implement magnetic compass
sentence_str<<","; sentence_str << ",";
//Magnetic Variation (E or W) //Magnetic Variation (E or W)
// ToDo: Implement magnetic compass // ToDo: Implement magnetic compass
sentence_str<<","; sentence_str << ",";
// Checksum // Checksum
char checksum; char checksum;
std::string tmpstr; std::string tmpstr;
tmpstr=sentence_str.str(); tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1)); checksum = checkSum(tmpstr.substr(1));
sentence_str<<"*"; sentence_str << "*";
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str<<std::hex<<(int)checksum; sentence_str << std::hex << (int)checksum;
// end NMEA sentence // end NMEA sentence
sentence_str<<"\r\n"; sentence_str << "\r\n";
return sentence_str.str(); return sentence_str.str();
} }
std::string Nmea_Printer::get_GPGSA() std::string Nmea_Printer::get_GPGSA()
{ {
//$GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33 //$GPGSA,A,3,07,02,26,27,09,04,15, , , , , ,1.8,1.0,1.5*33
// GSA-GNSS DOP and Active Satellites // GSA-GNSS DOP and Active Satellites
bool valid_fix = d_PVT_data->b_valid_position;
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;
int n_sats_used=d_PVT_data->d_valid_observations; double hdop = d_PVT_data->d_HDOP;
double vdop = d_PVT_data->d_VDOP;
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::stringstream sentence_str;
std::string sentence_header; std::string sentence_header;
sentence_header="$GPGSA,"; sentence_header = "$GPGSA,";
sentence_str<<sentence_header; sentence_str << sentence_header;
// mode1: // mode1:
// (M) Manual-forced to operate in 2D or 3D mode // (M) Manual-forced to operate in 2D or 3D mode
// (A) Automatic-allowed to automatically switch 2D/3D // (A) Automatic-allowed to automatically switch 2D/3D
std::string mode1="M"; std::string mode1 = "M";
sentence_str << mode1;
sentence_str<<mode1;
// mode2: // mode2:
// 1 fix not available // 1 fix not available
// 2 fix 2D // 2 fix 2D
// 3 fix 3D // 3 fix 3D
if (valid_fix==true) if (valid_fix==true)
{ {
sentence_str<<",3"; sentence_str << ",3";
}else{ }
sentence_str<<",1"; else
{
sentence_str << ",1";
}; };
// Used satellites // Used satellites
for (int i=0; i<12;i++) for (int i=0; i<12; i++)
{ {
sentence_str<<","; sentence_str << ",";
if (i<n_sats_used) if (i < n_sats_used)
{ {
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); 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 // PDOP
sentence_str<<","; sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield); sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2); sentence_str.width(2);
sentence_str.precision(1); sentence_str.precision(1);
@ -472,9 +491,8 @@ std::string Nmea_Printer::get_GPGSA()
sentence_str.precision(1); sentence_str.precision(1);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str << hdop; sentence_str << hdop;
//VDOP //VDOP
sentence_str<<","; sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield); sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2); sentence_str.width(2);
sentence_str.precision(1); sentence_str.precision(1);
@ -482,130 +500,128 @@ std::string Nmea_Printer::get_GPGSA()
sentence_str << vdop; sentence_str << vdop;
// Checksum // Checksum
char checksum; char checksum;
std::string tmpstr; std::string tmpstr;
tmpstr=sentence_str.str(); tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1)); checksum = checkSum(tmpstr.substr(1));
sentence_str<<"*"; sentence_str << "*";
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str<<std::hex<<(int)checksum; sentence_str << std::hex << (int)checksum;
// end NMEA sentence // end NMEA sentence
sentence_str<<"\r\n"; sentence_str<<"\r\n";
return sentence_str.str(); return sentence_str.str();
} }
std::string Nmea_Printer::get_GPGSV() std::string Nmea_Printer::get_GPGSV()
{ {
// GSV-GNSS Satellites in View // GSV-GNSS Satellites in View
// Notice that NMEA 2.1 only supports 12 channels // Notice that NMEA 2.1 only supports 12 channels
int n_sats_used=d_PVT_data->d_valid_observations; int n_sats_used=d_PVT_data->d_valid_observations;
std::stringstream sentence_str; std::stringstream sentence_str;
std::stringstream frame_str; std::stringstream frame_str;
std::string sentence_header; std::string sentence_header;
sentence_header="$GPGSV,"; sentence_header = "$GPGSV,";
char checksum; char checksum;
std::string tmpstr; std::string tmpstr;
// 1st step: How many GPGSV frames we need? (up to 3) // 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; 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 // generate the frames
int current_satellite=0; int current_satellite = 0;
for (int i=1;i<(n_frames+1);i++) for (int i=1; i<(n_frames+1); i++)
{ {
frame_str.str(""); frame_str.str("");
frame_str << sentence_header;
frame_str<<sentence_header;
// number of messages // number of messages
frame_str<<n_frames; frame_str << n_frames;
// message number // message number
frame_str<<","; frame_str << ",";
frame_str<<i; frame_str << i;
// total number of satellites in view // total number of satellites in view
frame_str<<","; frame_str << ",";
frame_str.width(2); frame_str.width(2);
frame_str.fill('0'); frame_str.fill('0');
frame_str<<std::dec<<n_sats_used; frame_str << std::dec << n_sats_used;
//satellites info //satellites info
for (int j=0;j<4;j++) for (int j=0; j<4; j++)
{ {
// write satellite info // write satellite info
frame_str<<","; frame_str << ",";
frame_str.width(2); frame_str.width(2);
frame_str.fill('0'); 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.width(2);
frame_str.fill('0'); 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.width(3);
frame_str.fill('0'); 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.width(2);
frame_str.fill('0'); 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++; current_satellite++;
if (current_satellite==n_sats_used) if (current_satellite == n_sats_used)
{ {
break; break;
} }
} }
// frame checksum // frame checksum
tmpstr=frame_str.str(); tmpstr = frame_str.str();
checksum = checkSum(tmpstr.substr(1)); checksum = checkSum(tmpstr.substr(1));
frame_str<<"*"; frame_str << "*";
frame_str.width(2); frame_str.width(2);
frame_str.fill('0'); frame_str.fill('0');
frame_str<<std::hex<<(int)checksum; frame_str << std::hex << (int)checksum;
// end NMEA sentence // end NMEA sentence
frame_str<<"\r\n"; frame_str << "\r\n";
//add frame to sentence //add frame to sentence
sentence_str<<frame_str.str(); sentence_str << frame_str.str();
} }
return sentence_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 //$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() std::string Nmea_Printer::get_GPGGA()
{ {
//boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time(); //boost::posix_time::ptime d_position_UTC_time=boost::posix_time::microsec_clock::universal_time();
bool valid_fix = d_PVT_data->b_valid_position;
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;
int n_channels=d_PVT_data->d_valid_observations;//d_nchannels
double hdop=d_PVT_data->d_HDOP;
double MSL_altitude; 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; MSL_altitude=d_PVT_data->d_avg_height_m;
}else{ }
else
{
MSL_altitude=d_PVT_data->d_height_m; MSL_altitude=d_PVT_data->d_height_m;
} }
@ -613,31 +629,25 @@ std::string Nmea_Printer::get_GPGGA()
//GPGGA (Global Positioning System Fixed Data) //GPGGA (Global Positioning System Fixed Data)
std::string sentence_header; std::string sentence_header;
sentence_header="$GPGGA,"; sentence_header = "$GPGGA,";
sentence_str << sentence_header;
sentence_str<<sentence_header;
//UTC Time: hhmmss.sss //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) // 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) // longitude dddmm.mmmm,(E or W)
sentence_str << "," << longitude_to_hm(d_PVT_data->d_avg_longitude_d);
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_avg_longitude_d); }
}else{ else
{
// Latitude ddmm.mmmm,(N or S) // 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) // 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 // Position fix indicator
@ -648,24 +658,28 @@ std::string Nmea_Printer::get_GPGGA()
// 6 - Dead Reckoning Mode, fix valid // 6 - Dead Reckoning Mode, fix valid
// ToDo: Update PVT module to identify the fix mode // ToDo: Update PVT module to identify the fix mode
if (valid_fix==true) if (valid_fix == true)
{ {
sentence_str<<",1"; sentence_str << ",1";
}else{ }
sentence_str<<",0"; else
{
sentence_str << ",0";
} }
// Number of satellites used in PVT // Number of satellites used in PVT
sentence_str<<","; sentence_str << ",";
if (n_channels<10) if (n_channels < 10)
{ {
sentence_str<<'0'<<n_channels; sentence_str << '0' << n_channels;
}else{ }
sentence_str<<n_channels; else
{
sentence_str << n_channels;
} }
// HDOP // HDOP
sentence_str<<","; sentence_str << ",";
sentence_str.setf(std::ios::fixed, std::ios::floatfield); sentence_str.setf(std::ios::fixed, std::ios::floatfield);
sentence_str.width(2); sentence_str.width(2);
sentence_str.precision(1); sentence_str.precision(1);
@ -673,40 +687,36 @@ std::string Nmea_Printer::get_GPGGA()
sentence_str << hdop; sentence_str << hdop;
// MSL Altitude // MSL Altitude
sentence_str<<","; sentence_str << ",";
sentence_str.precision(1); sentence_str.precision(1);
sentence_str<<MSL_altitude; sentence_str << MSL_altitude;
sentence_str<<",M"; sentence_str << ",M";
// Geoid-to-ellipsoid separation. Ellipsoid altitude = MSL Altitude + Geoid Separation. // Geoid-to-ellipsoid separation. Ellipsoid altitude = MSL Altitude + Geoid Separation.
// ToDo: Compute this value // ToDo: Compute this value
sentence_str<<","; sentence_str << ",";
sentence_str<<"0.0"; sentence_str << "0.0";
sentence_str<<",M"; sentence_str << ",M";
// Age of Diff. Corr. (Seconds) Null fields when DGPS is not used // Age of Diff. Corr. (Seconds) Null fields when DGPS is not used
// Diff. Ref. Station ID (0000) // Diff. Ref. Station ID (0000)
// ToDo: Implement this fields for Differential GPS // ToDo: Implement this fields for Differential GPS
sentence_str<<","; sentence_str << ",";
sentence_str<<"0.0,0000"; sentence_str << "0.0,0000";
// Checksum // Checksum
char checksum; char checksum;
std::string tmpstr; std::string tmpstr;
tmpstr=sentence_str.str(); tmpstr = sentence_str.str();
checksum = checkSum(tmpstr.substr(1)); checksum = checkSum(tmpstr.substr(1));
sentence_str<<"*"; sentence_str << "*";
sentence_str.width(2); sentence_str.width(2);
sentence_str.fill('0'); sentence_str.fill('0');
sentence_str<<std::hex<<(int)checksum; sentence_str << std::hex <<(int)checksum;
// end NMEA sentence // end NMEA sentence
sentence_str<<"\r\n"; sentence_str << "\r\n";
return sentence_str.str(); 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 //$GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A
} }

5
src/algorithms/PVT/libs/nmea_printer.h
View File

@ -59,18 +59,14 @@ public:
*/ */
~Nmea_Printer(); ~Nmea_Printer();
private: private:
std::string nmea_filename ; //<! String with the NMEA log filename std::string nmea_filename ; //<! String with the NMEA log filename
std::ofstream nmea_file_descriptor ; //<! Output file stream for NMEA log file std::ofstream nmea_file_descriptor ; //<! Output file stream for NMEA log file
std::string nmea_devname; std::string nmea_devname;
int nmea_dev_descriptor ; //<! NMEA serial device descriptor (i.e. COM port) int nmea_dev_descriptor ; //<! NMEA serial device descriptor (i.e. COM port)
gps_l1_ca_ls_pvt* d_PVT_data; gps_l1_ca_ls_pvt* d_PVT_data;
int init_serial (std::string serial_device); //serial port control int init_serial (std::string serial_device); //serial port control
void close_serial (); void close_serial ();
std::string get_GPGGA(); std::string get_GPGGA();
std::string get_GPGSV(); std::string get_GPGSV();
std::string get_GPGSA(); std::string get_GPGSA();
@ -79,7 +75,6 @@ private:
std::string longitude_to_hm(double longitude); std::string longitude_to_hm(double longitude);
std::string latitude_to_hm(double lat); std::string latitude_to_hm(double lat);
char checkSum(std::string sentence); char checkSum(std::string sentence);
}; };
#endif #endif

22
src/algorithms/PVT/libs/rinex_printer.h
View File

@ -14,7 +14,7 @@
* 1) The carrier-phase measurement at one or both carriers (actually being a * 1) The carrier-phase measurement at one or both carriers (actually being a
* measurement on the beat frequency between the received carrier of the * measurement on the beat frequency between the received carrier of the
* satellite signal and a receiver-generated reference frequency). * 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) * 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) * and the time of transmission (expressed in the time frame of the satellite)
* of a distinct satellite signal. * of a distinct satellite signal.
@ -66,8 +66,6 @@
*/ */
class Rinex_Printer class Rinex_Printer
{ {
public: public:
/*! /*!
* \brief Default constructor. Creates GPS Navigation and Observables RINEX files and their headers * \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 * \brief Writes observables into the RINEX file
*/ */
void log_rinex_obs(std::ofstream& out, Gps_Navigation_Message nav_msg, std::map<int,double> pseudoranges); 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> 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> observationType; //<! PSEUDORANGE, CARRIER_PHASE, DOPPLER, SIGNAL_STRENGTH
std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors std::map<std::string,std::string> observationCode; //<! GNSS observation descriptors
std::string stringVersion; //<! RINEX version (2.10/2.11 or 3.01) std::string stringVersion; //<! RINEX version (2.10/2.11 or 3.01)
private: private:
int version ; // RINEX version (2 for 2.10/2.11 and 3 for 3.01) 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? int numberTypesObservations; // Number of available types of observable in the system. Should be public?
/* /*
* Generation of RINEX signal strength indicators * Generation of RINEX signal strength indicators
*/ */
int signalStrength(double snr); int signalStrength(double snr);
/* Creates RINEX file names according to the naming convention /* Creates RINEX file names according to the naming convention
* *
* See http://igscb.jpl.nasa.gov/igscb/data/format/rinex301.pdf * 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 std::string::size_type expLen,
const bool checkSwitch) const bool checkSwitch)
{ {
std::string::size_type idx = aStr.find('.', startPos); std::string::size_type idx = aStr.find('.', startPos);
int expAdd = 0; int expAdd = 0;
std::string exp; 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'); aStr.insert((std::string::size_type)1, 1, '0');
} }
return aStr; return aStr;
} // end sci2for } // end sci2for
inline std::string asString(const long double x, const std::string::size_type precision) inline std::string asString(const long double x, const std::string::size_type precision)
{ {
std::ostringstream ss; std::ostringstream ss;
ss << std::fixed << std::setprecision(precision) << x ; ss << std::fixed << std::setprecision(precision) << x ;
return ss.str(); return ss.str();
} }