mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-10-30 23:03:05 +00:00
Added NMEA 2.1 protocol for PVT dump.
This is an experimental release that only supports FILE dump operations. git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@235 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas
617
src/algorithms/PVT/libs/nmea_printer.cc
Normal file
617
src/algorithms/PVT/libs/nmea_printer.cc
Normal file
@@ -0,0 +1,617 @@
|
||||
/*!
|
||||
* \file kml_printer.cc
|
||||
* \brief Implementation of a NMEA 2.1 printer for GNSS-SDR
|
||||
* This class provides a implementation of a subset of the NMEA-0183 standard for interfacing
|
||||
* marine electronic devices as defined by the National Marine Electronics Association (NMEA).
|
||||
* See http://www.nmea.org/ for the NMEA 183 standard
|
||||
*
|
||||
* \author Javier Arribas, 2012. jarribas(at)cttc.es
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
#include <gflags/gflags.h>
|
||||
#include "boost/date_time/posix_time/posix_time.hpp"
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "nmea_printer.h"
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
//DEFINE_string(NMEA_version, "2.1", "Specifies the NMEA version (2.1)");
|
||||
|
||||
Nmea_Printer::Nmea_Printer(std::string 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_Printer::~Nmea_Printer()
|
||||
{
|
||||
if (nmea_file_descriptor.is_open())
|
||||
{
|
||||
nmea_file_descriptor.close();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool Nmea_Printer::Print_Nmea_Line(gps_l1_ca_ls_pvt* pvt_data, bool print_average_values)
|
||||
{
|
||||
|
||||
// set the new PVT data
|
||||
d_PVT_data=pvt_data;
|
||||
|
||||
try{
|
||||
//GPRMC
|
||||
nmea_file_descriptor<<get_GPRMC();
|
||||
//GPGGA (Global Positioning System Fixed Data)
|
||||
nmea_file_descriptor<<get_GPGGA();
|
||||
//GPGSA
|
||||
nmea_file_descriptor<<get_GPGSA();
|
||||
//GPGSV
|
||||
nmea_file_descriptor<<get_GPGSV();
|
||||
|
||||
}catch(std::exception ex)
|
||||
{
|
||||
DLOG(INFO) << "NMEA printer can not write on output file" << nmea_filename.c_str();;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
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++) {
|
||||
check = char(check ^ sentence.at(c));
|
||||
}
|
||||
// return the result
|
||||
return check;
|
||||
}
|
||||
|
||||
std::string Nmea_Printer::latitude_to_hm(double lat)
|
||||
{
|
||||
bool north;
|
||||
if (lat < 0.0)
|
||||
{
|
||||
north = false;
|
||||
lat = -lat ;
|
||||
}else{
|
||||
north = true;
|
||||
}
|
||||
|
||||
int deg = (int)lat;
|
||||
double mins = lat - (double)deg;
|
||||
mins *= 60.0 ;
|
||||
std::ostringstream out_string;
|
||||
out_string.setf(std::ios::fixed, std::ios::floatfield);
|
||||
out_string.fill('0');
|
||||
out_string.width(2);
|
||||
out_string << deg;
|
||||
out_string.width(6);
|
||||
out_string.precision(4);
|
||||
out_string<<mins;
|
||||
|
||||
if (north==true)
|
||||
{
|
||||
out_string << ",N";
|
||||
}else{
|
||||
out_string << ",S";
|
||||
}
|
||||
return out_string.str();
|
||||
}
|
||||
|
||||
std::string Nmea_Printer::longitude_to_hm(double longitude)
|
||||
{
|
||||
bool east;
|
||||
if (longitude < 0.0)
|
||||
{
|
||||
east = false;
|
||||
longitude = -longitude ;
|
||||
}else{
|
||||
east=true;
|
||||
}
|
||||
|
||||
int deg = (int)longitude;
|
||||
double mins = longitude - (double)deg;
|
||||
mins *= 60.0 ;
|
||||
std::ostringstream out_string;
|
||||
out_string.setf(std::ios::fixed, std::ios::floatfield);
|
||||
out_string.width(3);
|
||||
out_string.fill('0');
|
||||
out_string << deg;
|
||||
out_string.width(6);
|
||||
out_string.precision(4);
|
||||
out_string<<mins;
|
||||
|
||||
if (east==true)
|
||||
{
|
||||
out_string << ",E";
|
||||
}else{
|
||||
out_string << ",W";
|
||||
}
|
||||
|
||||
return out_string.str();
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::string Nmea_Printer::get_UTC_NMEA_time(boost::posix_time::ptime d_position_UTC_time)
|
||||
{
|
||||
//UTC Time: hhmmss.sss
|
||||
std::stringstream sentence_str;
|
||||
|
||||
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;
|
||||
|
||||
if (utc_hours < 10) sentence_str << "0"; // two digits for hours
|
||||
sentence_str << utc_hours;
|
||||
|
||||
if (utc_mins < 10) sentence_str << "0"; // two digits for minutes
|
||||
sentence_str << utc_mins;
|
||||
|
||||
if (utc_seconds < 10) sentence_str << "0"; // two digits for seconds
|
||||
sentence_str << utc_seconds;
|
||||
|
||||
if (utc_milliseconds < 10)
|
||||
{
|
||||
sentence_str << ".00"; // three digits for ms
|
||||
sentence_str << utc_milliseconds;
|
||||
}
|
||||
else if (utc_milliseconds < 100)
|
||||
{
|
||||
sentence_str << ".0"; // three digits for ms
|
||||
sentence_str << utc_milliseconds;
|
||||
}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;
|
||||
|
||||
// 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->d_position_UTC_time);
|
||||
|
||||
//Status: A: data valid, V: data NOT valid
|
||||
|
||||
if (valid_fix==true)
|
||||
{
|
||||
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);
|
||||
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
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);
|
||||
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_longitude_d);
|
||||
}
|
||||
|
||||
//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->d_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<<(int)checksum;
|
||||
|
||||
// end NMEA sentence
|
||||
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;
|
||||
|
||||
|
||||
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->d_visible_satellites_IDs[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<<(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,";
|
||||
|
||||
char checksum;
|
||||
std::string tmpstr;
|
||||
// 1st step: How many GPGSV frames we need? (up to 3)
|
||||
// Each frame countains up to 4 satellites
|
||||
int n_frames;
|
||||
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++)
|
||||
{
|
||||
|
||||
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->d_visible_satellites_IDs[current_satellite];
|
||||
|
||||
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<<",";
|
||||
frame_str.width(3);
|
||||
frame_str.fill('0');
|
||||
frame_str<<std::dec<<(int)d_PVT_data->d_visible_satellites_Az[current_satellite];
|
||||
|
||||
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];
|
||||
|
||||
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<<(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
|
||||
|
||||
}
|
||||
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;
|
||||
double MSL_altitude;
|
||||
|
||||
if (d_PVT_data->d_flag_averaging==true)
|
||||
{
|
||||
MSL_altitude=d_PVT_data->d_avg_height_m;
|
||||
}else{
|
||||
MSL_altitude=d_PVT_data->d_height_m;
|
||||
}
|
||||
|
||||
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->d_position_UTC_time);
|
||||
|
||||
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);
|
||||
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
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);
|
||||
|
||||
// longitude dddmm.mmmm,(E or W)
|
||||
|
||||
sentence_str<<","<<longitude_to_hm(d_PVT_data->d_longitude_d);
|
||||
}
|
||||
|
||||
// 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<<(int)checksum;
|
||||
|
||||
// end NMEA sentence
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user