mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-06-05 08:04:09 +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
parent
45d7220dae
commit
598512529f
@ -66,10 +66,19 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
|
|||||||
d_dump_filename = dump_filename;
|
d_dump_filename = dump_filename;
|
||||||
std::string dump_ls_pvt_filename;
|
std::string dump_ls_pvt_filename;
|
||||||
dump_ls_pvt_filename=dump_filename;
|
dump_ls_pvt_filename=dump_filename;
|
||||||
|
|
||||||
|
//initialize kml_printer
|
||||||
std::string kml_dump_filename;
|
std::string kml_dump_filename;
|
||||||
kml_dump_filename = d_dump_filename;
|
kml_dump_filename = d_dump_filename;
|
||||||
kml_dump_filename.append(".kml");
|
kml_dump_filename.append(".kml");
|
||||||
d_kml_dump.set_headers(kml_dump_filename);
|
d_kml_dump.set_headers(kml_dump_filename);
|
||||||
|
|
||||||
|
//initialize nmea_printer
|
||||||
|
std::string nmea_dump_filename;
|
||||||
|
nmea_dump_filename = d_dump_filename;
|
||||||
|
nmea_dump_filename.append(".nmea");
|
||||||
|
d_nmea_printer=new Nmea_Printer(nmea_dump_filename);
|
||||||
|
|
||||||
d_dump_filename.append("_raw.dat");
|
d_dump_filename.append("_raw.dat");
|
||||||
dump_ls_pvt_filename.append("_ls_pvt.dat");
|
dump_ls_pvt_filename.append("_ls_pvt.dat");
|
||||||
d_averaging_depth = averaging_depth;
|
d_averaging_depth = averaging_depth;
|
||||||
@ -117,6 +126,7 @@ gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc()
|
|||||||
d_kml_dump.close_file();
|
d_kml_dump.close_file();
|
||||||
delete d_ls_pvt;
|
delete d_ls_pvt;
|
||||||
delete rp;
|
delete rp;
|
||||||
|
delete d_nmea_printer;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -204,6 +214,8 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
|
|||||||
if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,d_tx_time,d_flag_averaging) == true)
|
if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,d_tx_time,d_flag_averaging) == true)
|
||||||
{
|
{
|
||||||
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
|
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
|
||||||
|
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
|
||||||
|
|
||||||
if (!b_rinex_header_writen) // & we have utc data in nav message!
|
if (!b_rinex_header_writen) // & we have utc data in nav message!
|
||||||
{
|
{
|
||||||
rp->rinex_nav_header(rp->navFile, d_last_nav_msg);
|
rp->rinex_nav_header(rp->navFile, d_last_nav_msg);
|
||||||
|
|||||||
@ -38,6 +38,7 @@
|
|||||||
#include <boost/thread/thread.hpp>
|
#include <boost/thread/thread.hpp>
|
||||||
#include "concurrent_queue.h"
|
#include "concurrent_queue.h"
|
||||||
#include "gps_navigation_message.h"
|
#include "gps_navigation_message.h"
|
||||||
|
#include "nmea_printer.h"
|
||||||
#include "kml_printer.h"
|
#include "kml_printer.h"
|
||||||
#include "rinex_printer.h"
|
#include "rinex_printer.h"
|
||||||
#include "gps_l1_ca_ls_pvt.h"
|
#include "gps_l1_ca_ls_pvt.h"
|
||||||
@ -81,6 +82,8 @@ private:
|
|||||||
|
|
||||||
Kml_Printer d_kml_dump;
|
Kml_Printer d_kml_dump;
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
|||||||
@ -188,7 +188,10 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
|||||||
//--- Correct satellite position (do to earth rotation) --------
|
//--- Correct satellite position (do to earth rotation) --------
|
||||||
|
|
||||||
Rot_X = e_r_corr(traveltime, X.col(i)); //armadillo
|
Rot_X = e_r_corr(traveltime, X.col(i)); //armadillo
|
||||||
//--- Find the elevation angel of the satellite ----------------
|
//--- Find the elevation angle of the satellite ----------------
|
||||||
|
|
||||||
|
topocent(&d_visible_satellites_Az[i],&d_visible_satellites_El[i],&d_visible_satellites_Distance[i],pos.subvec(0,2), Rot_X - pos.subvec(0,2));
|
||||||
|
|
||||||
//[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
|
//[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
|
||||||
|
|
||||||
}
|
}
|
||||||
@ -221,11 +224,13 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
|
|||||||
//-- compute the Dilution Of Precision values
|
//-- compute the Dilution Of Precision values
|
||||||
arma::mat Q;
|
arma::mat Q;
|
||||||
Q = arma::inv(arma::htrans(A)*A);
|
Q = arma::inv(arma::htrans(A)*A);
|
||||||
|
//std::cout<<Q<<std::endl;
|
||||||
|
|
||||||
d_GDOP = sqrt(arma::trace(Q)); // GDOP
|
d_GDOP = sqrt(arma::trace(Q)); // GDOP
|
||||||
d_PDOP = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); // PDOP
|
d_PDOP = sqrt(Q(0,0) + Q(1,1) + Q(2,2)); // PDOP
|
||||||
d_HDOP = sqrt(Q(1,1) + Q(2,2)); // HDOP
|
d_HDOP = sqrt(Q(0,0) + Q(1,1)); // HDOP
|
||||||
d_VDOP = sqrt(Q(3,3)); // VDOP
|
d_VDOP = sqrt(Q(2,2)); // VDOP
|
||||||
d_TDOP = sqrt(Q(4,4)); // TDOP
|
d_TDOP = sqrt(Q(3,3)); // TDOP
|
||||||
}catch(std::exception e)
|
}catch(std::exception e)
|
||||||
{
|
{
|
||||||
d_GDOP = -1;
|
d_GDOP = -1;
|
||||||
@ -250,6 +255,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
|||||||
double GPS_corrected_time = 0;
|
double GPS_corrected_time = 0;
|
||||||
double utc = 0;
|
double utc = 0;
|
||||||
|
|
||||||
|
d_flag_averaging=flag_averaging;
|
||||||
|
|
||||||
int valid_obs=0; //valid observations counter
|
int valid_obs=0; //valid observations counter
|
||||||
for (int i=0; i<d_nchannels; i++)
|
for (int i=0; i<d_nchannels; i++)
|
||||||
{
|
{
|
||||||
@ -280,6 +287,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
|||||||
DLOG(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X
|
DLOG(INFO) << "ECEF satellite SV ID=" << d_ephemeris[i].i_satellite_PRN <<" X=" << d_ephemeris[i].d_satpos_X
|
||||||
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
|
<< " [m] Y=" << d_ephemeris[i].d_satpos_Y << " [m] Z=" << d_ephemeris[i].d_satpos_Z << " [m]" << std::endl;
|
||||||
obs(i) = gnss_pseudoranges_iter->second.Pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
|
obs(i) = gnss_pseudoranges_iter->second.Pseudorange_m + d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
|
||||||
|
d_visible_satellites_IDs[valid_obs]=d_ephemeris[i].i_satellite_PRN;
|
||||||
|
d_visible_satellites_CN0_dB[valid_obs]=gnss_pseudoranges_iter->second.CN0_dB_hz;
|
||||||
valid_obs++;
|
valid_obs++;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
@ -296,7 +305,11 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
|||||||
obs(i) = 1; // to avoid algorithm problems (divide by zero)
|
obs(i) = 1; // to avoid algorithm problems (divide by zero)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
d_valid_observations = valid_obs;
|
||||||
|
|
||||||
DLOG(INFO) <<"PVT: valid observations="<<valid_obs<<std::endl;
|
DLOG(INFO) <<"PVT: valid observations="<<valid_obs<<std::endl;
|
||||||
|
|
||||||
if (valid_obs>=4)
|
if (valid_obs>=4)
|
||||||
{
|
{
|
||||||
arma::vec mypos;
|
arma::vec mypos;
|
||||||
@ -471,59 +484,230 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
|
|||||||
d_height_m = h;
|
d_height_m = h;
|
||||||
}
|
}
|
||||||
|
|
||||||
//void gps_l1_ca_ls_pvt::topocent(traveltime, X_sat)
|
void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
|
||||||
//{
|
{
|
||||||
/*
|
//function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z)
|
||||||
%function [Az, El, D] = topocent(X, dx)
|
//%TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude,
|
||||||
%TOPOCENT Transformation of vector dx into topocentric coordinate
|
//% height given Cartesian coordinates X,Y,Z, and reference ellipsoid
|
||||||
% system with origin at X.
|
//% values semi-major axis (a) and the inverse of flattening (finv).
|
||||||
% Both parameters are 3 by 1 vectors.
|
//%
|
||||||
%
|
//%[dphi, dlambda, h] = togeod(a, finv, X, Y, Z);
|
||||||
%[Az, El, D] = topocent(X, dx);
|
//%
|
||||||
%
|
//% The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
|
||||||
% Inputs:
|
//% The output units of angular quantities will be in decimal degrees
|
||||||
% X - vector origin corrdinates (in ECEF system [X; Y; Z;])
|
//% (15.5 degrees not 15 deg 30 min). The output units of h will be the
|
||||||
% dx - vector ([dX; dY; dZ;]).
|
//% same as the units of X,Y,Z,a.
|
||||||
%
|
//%
|
||||||
% Outputs:
|
//% Inputs:
|
||||||
% D - vector length. Units like units of the input
|
//% a - semi-major axis of the reference ellipsoid
|
||||||
% Az - azimuth from north positive clockwise, degrees
|
//% finv - inverse of flattening of the reference ellipsoid
|
||||||
% El - elevation angle, degrees
|
//% X,Y,Z - Cartesian coordinates
|
||||||
|
//%
|
||||||
|
//% Outputs:
|
||||||
|
//% dphi - latitude
|
||||||
|
//% dlambda - longitude
|
||||||
|
//% h - height above reference ellipsoid
|
||||||
|
//
|
||||||
|
//% Copyright (C) 1987 C. Goad, Columbus, Ohio
|
||||||
|
//% Reprinted with permission of author, 1996
|
||||||
|
//% Fortran code translated into MATLAB
|
||||||
|
//% Kai Borre 03-30-96
|
||||||
|
//%
|
||||||
|
//% CVS record:
|
||||||
|
//% $Id: togeod.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
|
||||||
|
//%==========================================================================
|
||||||
|
//
|
||||||
|
*h = 0;
|
||||||
|
double tolsq = 1.e-10;
|
||||||
|
int maxit=10;
|
||||||
|
// compute radians-to-degree factor
|
||||||
|
double rtd;
|
||||||
|
rtd = 180/GPS_PI;
|
||||||
|
|
||||||
|
// compute square of eccentricity
|
||||||
|
double esq;
|
||||||
|
if (finv < 1.0E-20)
|
||||||
|
{
|
||||||
|
esq = 0;
|
||||||
|
}else
|
||||||
|
{
|
||||||
|
esq = (2 - 1/finv) / finv;
|
||||||
|
}
|
||||||
|
|
||||||
|
double oneesq;
|
||||||
|
|
||||||
|
oneesq = 1 - esq;
|
||||||
|
|
||||||
|
// first guess
|
||||||
|
// P is distance from spin axis
|
||||||
|
double P;
|
||||||
|
P = sqrt(X*X+Y*Y);
|
||||||
|
//direct calculation of longitude
|
||||||
|
//
|
||||||
|
if (P > 1.0E-20)
|
||||||
|
{
|
||||||
|
*dlambda = atan2(Y,X) * rtd;
|
||||||
|
}else
|
||||||
|
{
|
||||||
|
*dlambda = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (*dlambda < 0)
|
||||||
|
{
|
||||||
|
*dlambda = *dlambda + 360.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// r is distance from origin (0,0,0)
|
||||||
|
double r;
|
||||||
|
r = sqrt(P*P + Z*Z);
|
||||||
|
|
||||||
|
double sinphi;
|
||||||
|
|
||||||
|
if (r > 1.0E-20)
|
||||||
|
{
|
||||||
|
sinphi = Z/r;
|
||||||
|
} else
|
||||||
|
{
|
||||||
|
sinphi = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
*dphi = asin(sinphi);
|
||||||
|
|
||||||
|
// initial value of height = distance from origin minus
|
||||||
|
// approximate distance from origin to surface of ellipsoid
|
||||||
|
if (r < 1.0E-20)
|
||||||
|
{
|
||||||
|
*h = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
*h = r - a*(1-sinphi*sinphi/finv);
|
||||||
|
|
||||||
|
|
||||||
dtr = pi/180;
|
// iterate
|
||||||
|
double cosphi;
|
||||||
|
double N_phi;
|
||||||
|
double dP;
|
||||||
|
double dZ;
|
||||||
|
for (int i=0; i<maxit; i++)
|
||||||
|
{
|
||||||
|
sinphi = sin(*dphi);
|
||||||
|
cosphi = cos(*dphi);
|
||||||
|
|
||||||
[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
|
// compute radius of curvature in prime vertical direction
|
||||||
|
N_phi = a/sqrt(1-esq*sinphi*sinphi);
|
||||||
|
|
||||||
|
// compute residuals in P and Z
|
||||||
|
dP = P - (N_phi + (*h)) * cosphi;
|
||||||
|
dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
|
||||||
|
//
|
||||||
|
// update height and latitude
|
||||||
|
*h = *h + (sinphi*dZ + cosphi*dP);
|
||||||
|
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
|
||||||
|
|
||||||
|
// test for convergence
|
||||||
|
if ((dP*dP + dZ*dZ) < tolsq)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Not Converged--Warn user
|
||||||
|
// if (i == (maxit-1))
|
||||||
|
// fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
|
||||||
|
// ' iterations\n'], i);
|
||||||
|
// end
|
||||||
|
}
|
||||||
|
//
|
||||||
|
*dphi = (*dphi) * rtd;
|
||||||
|
|
||||||
|
}
|
||||||
|
void gps_l1_ca_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x, arma::vec x_sat)
|
||||||
|
{
|
||||||
|
|
||||||
|
//%function [Az, El, D] = topocent(X, dx)
|
||||||
|
//%TOPOCENT Transformation of vector dx into topocentric coordinate
|
||||||
|
//% system with origin at X.
|
||||||
|
//% Both parameters are 3 by 1 vectors.
|
||||||
|
//%
|
||||||
|
//%[Az, El, D] = topocent(X, dx);
|
||||||
|
//%
|
||||||
|
//% Inputs:
|
||||||
|
//% X - vector origin corrdinates (in ECEF system [X; Y; Z;])
|
||||||
|
//% dx - vector ([dX; dY; dZ;]).
|
||||||
|
//%
|
||||||
|
//% Outputs:
|
||||||
|
//% D - vector length. Units like units of the input
|
||||||
|
//% Az - azimuth from north positive clockwise, degrees
|
||||||
|
//% El - elevation angle, degrees
|
||||||
|
|
||||||
|
double dtr;
|
||||||
|
double lambda;
|
||||||
|
double phi;
|
||||||
|
double cl;
|
||||||
|
double sl;
|
||||||
|
double cb;
|
||||||
|
double sb;
|
||||||
|
|
||||||
|
double h;
|
||||||
|
|
||||||
|
dtr = GPS_PI/180.0;
|
||||||
|
|
||||||
|
//[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
|
||||||
|
|
||||||
|
togeod(&phi, &lambda, &h,6378137.0, 298.257223563, x(0), x(1), x(2));
|
||||||
|
|
||||||
cl = cos(lambda * dtr);
|
cl = cos(lambda * dtr);
|
||||||
sl = sin(lambda * dtr);
|
sl = sin(lambda * dtr);
|
||||||
cb = cos(phi * dtr);
|
cb = cos(phi * dtr);
|
||||||
sb = sin(phi * dtr);
|
sb = sin(phi * dtr);
|
||||||
|
|
||||||
F = [-sl -sb*cl cb*cl;
|
arma::mat F=arma::zeros(3,3);
|
||||||
cl -sb*sl cb*sl;
|
|
||||||
0 cb sb];
|
|
||||||
|
|
||||||
local_vector = F' * dx;
|
F(0,0)=-sl;
|
||||||
E = local_vector(1);
|
F(0,1)=-sb*cl;
|
||||||
N = local_vector(2);
|
F(0,2)=cb*cl;
|
||||||
U = local_vector(3);
|
|
||||||
|
|
||||||
hor_dis = sqrt(E^2 + N^2);
|
F(1,0)=cl;
|
||||||
|
F(1,1)=-sb*sl;
|
||||||
|
F(1,2)=cb*sl;
|
||||||
|
|
||||||
if hor_dis < 1.e-20
|
F(2,0)=0;
|
||||||
Az = 0;
|
F(2,1)=cb;
|
||||||
El = 90;
|
F(2,2)=sb;
|
||||||
|
|
||||||
|
arma::vec local_vector;
|
||||||
|
|
||||||
|
local_vector = arma::htrans(F) * x_sat;
|
||||||
|
|
||||||
|
double E;
|
||||||
|
double N;
|
||||||
|
double U;
|
||||||
|
|
||||||
|
E = local_vector(0);
|
||||||
|
N = local_vector(1);
|
||||||
|
U = local_vector(2);
|
||||||
|
|
||||||
|
double hor_dis;
|
||||||
|
|
||||||
|
hor_dis = sqrt(E*E + N*N);
|
||||||
|
|
||||||
|
if (hor_dis < 1.0E-20)
|
||||||
|
{
|
||||||
|
*Az = 0;
|
||||||
|
*El = 90;
|
||||||
|
}
|
||||||
else
|
else
|
||||||
Az = atan2(E, N)/dtr;
|
{
|
||||||
El = atan2(U, hor_dis)/dtr;
|
*Az = atan2(E, N)/dtr;
|
||||||
end
|
*El = atan2(U, hor_dis)/dtr;
|
||||||
|
}
|
||||||
|
|
||||||
if Az < 0
|
if (*Az < 0)
|
||||||
Az = Az + 360;
|
{
|
||||||
end
|
*Az = *Az + 360.0;
|
||||||
|
}
|
||||||
|
|
||||||
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
|
|
||||||
%%%%%%%%% end topocent.m %%%%%%%%%
|
*D = sqrt(x_sat(0)*x_sat(0) + x_sat(1)*x_sat(1) + x_sat(2)*x_sat(2));
|
||||||
*/
|
|
||||||
//}
|
}
|
||||||
|
|||||||
@ -48,6 +48,8 @@
|
|||||||
|
|
||||||
#include "gnss_synchro.h"
|
#include "gnss_synchro.h"
|
||||||
|
|
||||||
|
#define PVT_MAX_CHANNELS 24
|
||||||
|
|
||||||
/*!
|
/*!
|
||||||
* \brief This class implements a simple PVT Least Squares solution
|
* \brief This class implements a simple PVT Least Squares solution
|
||||||
*/
|
*/
|
||||||
@ -56,9 +58,17 @@ class gps_l1_ca_ls_pvt
|
|||||||
private:
|
private:
|
||||||
arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w);
|
arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w);
|
||||||
arma::vec e_r_corr(double traveltime, arma::vec X_sat);
|
arma::vec e_r_corr(double traveltime, arma::vec X_sat);
|
||||||
//void topocent();
|
void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec x_sat);
|
||||||
|
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
|
||||||
public:
|
public:
|
||||||
int d_nchannels; //! Number of available channels for positioning
|
int d_nchannels; //! Number of available channels for positioning
|
||||||
|
int d_valid_observations; //! Number of valid pseudorrange observations (valid satellites)
|
||||||
|
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites
|
||||||
|
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //! Array with the LOS Elevation of the valid satellites
|
||||||
|
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //! Array with the LOS Azimuth of the valid satellites
|
||||||
|
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //! Array with the LOS Distance of the valid satellites
|
||||||
|
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites
|
||||||
|
|
||||||
Gps_Navigation_Message* d_ephemeris;
|
Gps_Navigation_Message* d_ephemeris;
|
||||||
double d_GPS_current_time;
|
double d_GPS_current_time;
|
||||||
boost::posix_time::ptime d_position_UTC_time;
|
boost::posix_time::ptime d_position_UTC_time;
|
||||||
@ -92,6 +102,8 @@ public:
|
|||||||
double d_TDOP;
|
double d_TDOP;
|
||||||
|
|
||||||
bool d_flag_dump_enabled;
|
bool d_flag_dump_enabled;
|
||||||
|
bool d_flag_averaging;
|
||||||
|
|
||||||
std::string d_dump_filename;
|
std::string d_dump_filename;
|
||||||
std::ofstream d_dump_file;
|
std::ofstream d_dump_file;
|
||||||
|
|
||||||
|
|||||||
@ -3,3 +3,4 @@ project : build-dir ../../../../build ;
|
|||||||
obj rinex_printer : rinex_printer.cc ;
|
obj rinex_printer : rinex_printer.cc ;
|
||||||
obj gps_l1_ca_ls_pvt : gps_l1_ca_ls_pvt.cc ;
|
obj gps_l1_ca_ls_pvt : gps_l1_ca_ls_pvt.cc ;
|
||||||
obj kml_printer : kml_printer.cc ;
|
obj kml_printer : kml_printer.cc ;
|
||||||
|
obj nmea_printer : nmea_printer.cc ;
|
||||||
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
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
80
src/algorithms/PVT/libs/nmea_printer.h
Normal file
80
src/algorithms/PVT/libs/nmea_printer.h
Normal file
@ -0,0 +1,80 @@
|
|||||||
|
/*!
|
||||||
|
* \file kml_printer.h
|
||||||
|
* \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/>.
|
||||||
|
*
|
||||||
|
* -------------------------------------------------------------------------
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef GNSS_SDR_NMEA_PRINTER_H_
|
||||||
|
#define GNSS_SDR_NMEA_PRINTER_H_
|
||||||
|
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include "gps_l1_ca_ls_pvt.h"
|
||||||
|
|
||||||
|
class Nmea_Printer
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/*!
|
||||||
|
* \brief Default constructor.
|
||||||
|
*/
|
||||||
|
Nmea_Printer(std::string filename);
|
||||||
|
|
||||||
|
/*!
|
||||||
|
* \brief Print NMEA PVT and satellite info to the initialized device
|
||||||
|
*/
|
||||||
|
bool Print_Nmea_Line(gps_l1_ca_ls_pvt* position, bool print_average_values);
|
||||||
|
|
||||||
|
/*!
|
||||||
|
* \brief Default destructor.
|
||||||
|
*/
|
||||||
|
~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
|
||||||
|
|
||||||
|
gps_l1_ca_ls_pvt* d_PVT_data;
|
||||||
|
|
||||||
|
std::string get_GPGGA();
|
||||||
|
std::string get_GPGSV();
|
||||||
|
std::string get_GPGSA();
|
||||||
|
std::string get_GPRMC();
|
||||||
|
std::string get_UTC_NMEA_time(boost::posix_time::ptime d_position_UTC_time);
|
||||||
|
std::string longitude_to_hm(double longitude);
|
||||||
|
std::string latitude_to_hm(double lat);
|
||||||
|
char checkSum(std::string sentence);
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
@ -37,6 +37,7 @@ exe gnss-sdr : main.cc
|
|||||||
../algorithms/observables/gnuradio_blocks//gps_l1_ca_observables_cc
|
../algorithms/observables/gnuradio_blocks//gps_l1_ca_observables_cc
|
||||||
../algorithms/PVT/libs//rinex_printer
|
../algorithms/PVT/libs//rinex_printer
|
||||||
../algorithms/PVT/libs//kml_printer
|
../algorithms/PVT/libs//kml_printer
|
||||||
|
../algorithms/PVT/libs//nmea_printer
|
||||||
../algorithms/PVT/libs//gps_l1_ca_ls_pvt
|
../algorithms/PVT/libs//gps_l1_ca_ls_pvt
|
||||||
../algorithms/output_filter/adapters//file_output_filter
|
../algorithms/output_filter/adapters//file_output_filter
|
||||||
../algorithms/output_filter/adapters//null_sink_output_filter
|
../algorithms/output_filter/adapters//null_sink_output_filter
|
||||||
|
|||||||
@ -34,6 +34,7 @@ exe run_tests : test_main.cc
|
|||||||
../algorithms/observables/gnuradio_blocks//gps_l1_ca_observables_cc
|
../algorithms/observables/gnuradio_blocks//gps_l1_ca_observables_cc
|
||||||
../algorithms/PVT/libs//rinex_printer
|
../algorithms/PVT/libs//rinex_printer
|
||||||
../algorithms/PVT/libs//kml_printer
|
../algorithms/PVT/libs//kml_printer
|
||||||
|
../algorithms/PVT/libs//nmea_printer
|
||||||
../algorithms/PVT/libs//gps_l1_ca_ls_pvt
|
../algorithms/PVT/libs//gps_l1_ca_ls_pvt
|
||||||
../algorithms/output_filter/adapters//file_output_filter
|
../algorithms/output_filter/adapters//file_output_filter
|
||||||
../algorithms/output_filter/adapters//null_sink_output_filter
|
../algorithms/output_filter/adapters//null_sink_output_filter
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user