diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 439d43604..49b48d64d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -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; std::string dump_ls_pvt_filename; dump_ls_pvt_filename=dump_filename; + + //initialize kml_printer std::string kml_dump_filename; kml_dump_filename = d_dump_filename; kml_dump_filename.append(".kml"); 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"); dump_ls_pvt_filename.append("_ls_pvt.dat"); d_averaging_depth = averaging_depth; @@ -117,6 +126,7 @@ gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc() d_kml_dump.close_file(); delete d_ls_pvt; 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) { 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! { rp->rinex_nav_header(rp->navFile, d_last_nav_msg); diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h index 3f913e746..82b646682 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h @@ -38,6 +38,7 @@ #include #include "concurrent_queue.h" #include "gps_navigation_message.h" +#include "nmea_printer.h" #include "kml_printer.h" #include "rinex_printer.h" #include "gps_l1_ca_ls_pvt.h" @@ -81,6 +82,8 @@ private: Kml_Printer d_kml_dump; + Nmea_Printer *d_nmea_printer; + concurrent_queue *d_nav_queue; // Navigation ephemeris queue Gps_Navigation_Message d_last_nav_msg; // Last navigation message diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc index e2fb97245..a50c31656 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -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) -------- 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, :)); } @@ -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 arma::mat Q; Q = arma::inv(arma::htrans(A)*A); + //std::cout< gnss_pseudoranges_map, double GPS_corrected_time = 0; double utc = 0; + d_flag_averaging=flag_averaging; + int valid_obs=0; //valid observations counter for (int i=0; i gnss_pseudoranges_map, 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; 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++; } else @@ -296,7 +305,11 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, obs(i) = 1; // to avoid algorithm problems (divide by zero) } } + + d_valid_observations = valid_obs; + DLOG(INFO) <<"PVT: valid observations="<=4) { 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; } -//void gps_l1_ca_ls_pvt::topocent(traveltime, 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 +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) + //%TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude, + //% height given Cartesian coordinates X,Y,Z, and reference ellipsoid + //% values semi-major axis (a) and the inverse of flattening (finv). + //% + //%[dphi, dlambda, h] = togeod(a, finv, X, Y, Z); + //% + //% The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc) + //% The output units of angular quantities will be in decimal degrees + //% (15.5 degrees not 15 deg 30 min). The output units of h will be the + //% same as the units of X,Y,Z,a. + //% + //% Inputs: + //% a - semi-major axis of the reference ellipsoid + //% finv - inverse of flattening of the reference ellipsoid + //% 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. + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#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< $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<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<<","<d_avg_latitude_d); + + // longitude dddmm.mmmm,(E or W) + + sentence_str<<","<d_avg_longitude_d); + }else{ + // Latitude ddmm.mmmm,(N or S) + + sentence_str<<","<d_latitude_d); + + // longitude dddmm.mmmm,(E or W) + + sentence_str<<","<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<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<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<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<d_visible_satellites_IDs[current_satellite]; + + frame_str<<","; + frame_str.width(2); + frame_str.fill('0'); + frame_str<d_visible_satellites_El[current_satellite]; + + frame_str<<","; + frame_str.width(3); + frame_str.fill('0'); + frame_str<d_visible_satellites_Az[current_satellite]; + + frame_str<<","; + frame_str.width(2); + frame_str.fill('0'); + frame_str<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<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<d_position_UTC_time); + + if (d_PVT_data->d_flag_averaging==true) + { + // Latitude ddmm.mmmm,(N or S) + + sentence_str<<","<d_avg_latitude_d); + + // longitude dddmm.mmmm,(E or W) + + sentence_str<<","<d_avg_longitude_d); + }else{ + // Latitude ddmm.mmmm,(N or S) + + sentence_str<<","<d_latitude_d); + + // longitude dddmm.mmmm,(E or W) + + sentence_str<<","<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'<. + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_NMEA_PRINTER_H_ +#define GNSS_SDR_NMEA_PRINTER_H_ + + +#include +#include +#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 ; //