mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 07:13:03 +00:00
Code Issues Releases Wiki Activity

- Major changes:

Browse Source 
- The executable file and the default configuration file is now changed from "./install/mercurio" and "./conf/mercurio.conf" to "./install/gnss-sdr" and "./conf/gnss-sdr.conf", respectively.
        - Configuration file structure changed to define in a single entry the internal sampling frequency (after the signal conditioner). NOTICE that this change affects the all the adapters (acquisition, tracking, telemetry_decoder, observables, and PVT). All the adapters are now modified to work with this feature.
        - Moved several in-line GPS L1 CA parameters (a.k.a magic numbers..) to ./src/core/system_parameters/GPS_L1_CA.h definition file.
        - Tracking blocks now uses DOUBLE values in their outputs.
        - Observables and PVT now are separated. PVT and their associated libraries are moved to ./src/algorithms/PVT
        - Temporarily disabled the RINEX output (I am working on that!)
        - GNSS-SDR screen output now gives extended debug information of the receiver status and events. In the future, this output will be redirected to a log file.

- Bug fixes:
        - FILE_SIGNAL_SOURCE now works correctly when the user configures GNSS-SDR to process the entire file.
        - GPS_L1_CA_DLL_PLL now computes correctly the PRN start values.
        - GPS_L1_CA_DLL_FLL_PLL now computes correctly the PRN start values.
        - Several modifications in GPS_L1_CA_Telemetry_Decoder, GPS_L1_CA_Observables, and GPS_L1_CA_PVT modules to fix the GPS position computation.

- New features
        - Tracking blocks perform a signal integrity check against NaN outliers before the correlation process.
        - Tracking and PVT binary dump options are now documented and we provide MATLAB libraries and sample files to read it. Available in ./utils/matlab" and "./utils/matlab/libs"
        - Observables output rate can be configured. This option enables the GPS L1 CA PVT computation at a maximum rate of 1ms.
        - GPS_L1_CA_PVT now can perform a moving average Latitude, Longitude, and Altitude output for each of the Observables output. It is configurable using the configuration file.
        - Added Google Earth compatible Keyhole Markup Language (KML) output writer class (./src/algorithms/PVT/libs/kml_printer.h and ./src/algorithms/PVT/libs/kml_printer.cc ). You can see the receiver position directly using Google Earth.
        - We provide a master configuration file which includes an in-line documentation with all the new (and old) options. It can be found in ./conf/master.conf

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@84 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas
2011-12-07 17:59:34 +00:00
parent 57f7a128a5
commit 69b8ac00dc
102 changed files with 4756 additions and 914 deletions
Download Patch File Download Diff File Expand all files Collapse all files

489
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc Normal file
View File

@@ -0,0 +1,489 @@
/*!
* \file gps_l1_ca_ls_pvt.cc
* \brief Least Squares Position, Velocity, and Time (PVT) solver, based on
* K.Borre Matlab receiver.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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/>.
*
* -------------------------------------------------------------------------
*/
/*!
* Using ITPP
*/
//#include <itpp/itbase.h>
//#include <itpp/stat/misc_stat.h>
//#include <itpp/base/matfunc.h>
//using namespace itpp;
/*!
* Using armadillo
*/
#include "armadillo"
//using namespace arma;
#include "gps_l1_ca_ls_pvt.h"
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file)
{
// init empty ephemerids for all the available GNSS channels
d_nchannels=nchannels;
d_ephemeris=new gps_navigation_message[nchannels];
d_dump_filename=dump_filename;
d_flag_dump_enabled=flag_dump_to_file;
d_averaging_depth=0;
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"PVT lib dump enabled Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "Exception opening PVT lib dump file "<<e.what()<<"\r\n";
}
}
}
}
void gps_l1_ca_ls_pvt::set_averaging_depth(int depth)
{
d_averaging_depth=depth;
}
gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt()
{
d_dump_file.close();
delete[] d_ephemeris;
}
arma::vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, arma::vec X_sat) {
/*
%E_R_CORR Returns rotated satellite ECEF coordinates due to Earth
%rotation during signal travel time
%
%X_sat_rot = e_r_corr(traveltime, X_sat);
%
% Inputs:
% travelTime - signal travel time
% X_sat - satellite's ECEF coordinates
%
% Outputs:
% X_sat_rot - rotated satellite's coordinates (ECEF)
*/
double Omegae_dot = 7.292115147e-5; // rad/sec
//--- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = Omegae_dot * traveltime;
//--- Make a rotation matrix -----------------------------------------------
arma::mat R3=arma::zeros(3,3);
R3(0, 0)= cos(omegatau);
R3(0, 1)= sin(omegatau);
R3(0, 2)= 0.0;
R3(1, 0)=-sin(omegatau);
R3(1, 1)= cos(omegatau);
R3(1, 2)=0.0;
R3(2, 0)= 0.0;
R3(2, 1)= 0.0;
R3(2, 2)= 1;
//--- Do the rotation ------------------------------------------------------
arma::vec X_sat_rot;
X_sat_rot = R3 * X_sat;
return X_sat_rot;
}
arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w) {
//Function calculates the Least Square Solution.
//pos= leastSquarePos(satpos, obs, w);
// Inputs:
// satpos - Satellites positions (in ECEF system: [X; Y; Z;]
// obs - Observations - the pseudorange measurements to each
// satellite
// w - weigths vector
// Outputs:
// pos - receiver position and receiver clock error
// (in ECEF system: [X, Y, Z, dt])
//////////////////////////////////////////////////////////////////////////
// el - Satellites elevation angles (degrees)
// az - Satellites azimuth angles (degrees)
// dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
//=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config
//double dtr = GPS_PI / 180.0;
int nmbOfSatellites;
//nmbOfSatellites = satpos.cols(); //ITPP
nmbOfSatellites = satpos.n_cols; //Armadillo
arma::vec pos = "0.0 0.0 0.0 0.0";
arma::mat A;
arma::mat omc;
arma::mat az;
arma::mat el;
A=arma::zeros(nmbOfSatellites,4);
omc=arma::zeros(nmbOfSatellites,1);
az=arma::zeros(1,nmbOfSatellites);
el=arma::zeros(1,nmbOfSatellites);
for (int i = 0; i < nmbOfSatellites; i++) {
for (int j = 0; j < 4; j++) {
//A.set(i, j, 0.0); //ITPP
A(i,j)=0.0; //Armadillo
}
omc(i, 0)=0.0;
az(0, i)=0.0;
}
el = az;
arma::mat X = satpos;
arma::vec Rot_X;
double rho2;
double traveltime;
double trop;
arma::mat mat_tmp;
arma::vec x;
//=== Iteratively find receiver position ===================================
for (int iter = 0; iter < nmbOfIterations; iter++) {
for (int i = 0; i < nmbOfSatellites; i++) {
if (iter == 0) {
//--- Initialize variables at the first iteration --------------
//Rot_X=X.get_col(i); //ITPP
Rot_X=X.col(i); //Armadillo
trop = 0.0;
} else {
//--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0))*(X(0, i) - pos(0)) + (X(1, i) - pos(1))*(X(1, i) - pos(1))+ (X(2,i) - pos(2))*(X(2,i) - pos(2));
traveltime = sqrt(rho2) / GPS_C_m_s;
//--- Correct satellite position (do to earth rotation) --------
//Rot_X = e_r_corr(traveltime, X.get_col(i)); //ITPP
Rot_X = e_r_corr(traveltime, X.col(i)); //armadillo
//--- Find the elevation angel of the satellite ----------------
//[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
}
//--- Apply the corrections ----------------------------------------
//omc(i) = (obs(i) - norm(Rot_X - pos(3)) - pos(4) - trop); //ITPP
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0,2),2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix ---------------------------------------
//ITPP
//A.set(i, 0, (-(Rot_X(0) - pos(0))) / obs(i));
//A.set(i, 1, (-(Rot_X(1) - pos(1))) / obs(i));
//A.set(i, 2, (-(Rot_X(2) - pos(2))) / obs(i));
//A.set(i, 3, 1.0);
//Armadillo
A(i,0)=(-(Rot_X(0) - pos(0))) / obs(i);
A(i,1)=(-(Rot_X(1) - pos(1))) / obs(i);
A(i,2)=(-(Rot_X(2) - pos(2))) / obs(i);
A(i,3)=1.0;
}
// These lines allow the code to exit gracefully in case of any errors
//if (rank(A) != 4) {
// pos.clear();
// return pos;
//}
//--- Find position update ---------------------------------------------
// x = ls_solve_od(w*A,w*omc); // ITPP
x = arma::solve(w*A,w*omc); // Armadillo
//--- Apply position update --------------------------------------------
pos = pos + x;
}
return pos;
}
bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_map,double GPS_current_time,bool flag_averaging)
{
std::map<int,gnss_pseudorange>::iterator gnss_pseudoranges_iter;
//ITPP
//mat W=eye(d_nchannels); //channels weights matrix
//vec obs=zeros(d_nchannels); // pseudoranges observation vector
//mat satpos=zeros(3,d_nchannels); //satellite positions matrix
// Armadillo
arma::mat W=arma::eye(d_nchannels,d_nchannels); //channels weights matrix
arma::vec obs=arma::zeros(d_nchannels); // pseudoranges observation vector
arma::mat satpos=arma::zeros(3,d_nchannels); //satellite positions matrix
int valid_obs=0; //valid observations counter
for (int i=0; i<d_nchannels; i++)
{
if (d_ephemeris[i].satellite_validation()==true)
{
gnss_pseudoranges_iter=gnss_pseudoranges_map.find(d_ephemeris[i].d_satellite_PRN);
if (gnss_pseudoranges_iter!=gnss_pseudoranges_map.end())
{
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(i,i)=1;
// compute the GPS master clock
d_ephemeris[i].master_clock(GPS_current_time);
// compute the satellite current ECEF position
d_ephemeris[i].satpos();
// compute the clock error including relativistic effects
d_ephemeris[i].relativistic_clock_correction(GPS_current_time);
satpos(0,i)=d_ephemeris[i].d_satpos_X;
satpos(1,i)=d_ephemeris[i].d_satpos_Y;
satpos(2,i)=d_ephemeris[i].d_satpos_Z;
std::cout<<"ECEF satellite SV ID="<<d_ephemeris[i].d_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]\r\n";
obs(i)=gnss_pseudoranges_iter->second.pseudorange_m+d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
valid_obs++;
}else{
// no valid pseudorange for the current channel
W(i,i)=0; // channel de-activated
obs(i)=1; // to avoid algorithm problems (divide by zero)
}
}else{
// no valid ephemeris for the current channel
W(i,i)=0; // channel de-activated
obs(i)=1; // to avoid algorithm problems (divide by zero)
}
}
std::cout<<"PVT: valid observations="<<valid_obs<<std::endl;
if (valid_obs>=4)
{
arma::vec mypos;
mypos=leastSquarePos(satpos,obs,W);
std::cout << "Position at TOW="<<GPS_current_time<<" is ECEF (X,Y,Z) = " << mypos << std::endl;
cart2geo(mypos(0), mypos(1), mypos(2), 4);
std::cout << "Position at TOW="<<GPS_current_time<<" is Lat = " << d_latitude_d << " [º] Long ="<< d_longitude_d <<" [º] Height="<<d_height_m<<" [m]\r\n";
// ######## LOG FILE #########
if(d_flag_dump_enabled==true) {
// MULTIPLEXED FILE RECORDING - Record results to file
try {
double tmp_double;
// PVT GPS time
tmp_double=GPS_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double=mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double=mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double=mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double=mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double=d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double=d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double=d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing PVT lib dump file "<<e.what()<<"\r\n";
}
}
// MOVING AVERAGE PVT
if (flag_averaging==true)
{
if (d_hist_longitude_d.size()==(unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d=0;
d_avg_longitude_d=0;
d_avg_height_m=0;
for (unsigned int i=0;i<d_hist_longitude_d.size();i++)
{
d_avg_latitude_d=d_avg_latitude_d+d_hist_latitude_d.at(i);
d_avg_longitude_d=d_avg_longitude_d+d_hist_longitude_d.at(i);
d_avg_height_m=d_avg_height_m+d_hist_height_m.at(i);
}
d_avg_latitude_d=d_avg_latitude_d/(double)d_averaging_depth;
d_avg_longitude_d=d_avg_longitude_d/(double)d_averaging_depth;
d_avg_height_m=d_avg_height_m/(double)d_averaging_depth;
return true; //indicates that the returned position is valid
}else{
//int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d=d_latitude_d;
d_avg_longitude_d=d_longitude_d;
d_avg_height_m=d_height_m;
return false;//indicates that the returned position is not valid yet
// output the average, although it will not have the full historic available
// d_avg_latitude_d=0;
// d_avg_longitude_d=0;
// d_avg_height_m=0;
// for (unsigned int i=0;i<d_hist_longitude_d.size();i++)
// {
// d_avg_latitude_d=d_avg_latitude_d+d_hist_latitude_d.at(i);
// d_avg_longitude_d=d_avg_longitude_d+d_hist_longitude_d.at(i);
// d_avg_height_m=d_avg_height_m+d_hist_height_m.at(i);
// }
// d_avg_latitude_d=d_avg_latitude_d/(double)current_depth;
// d_avg_longitude_d=d_avg_longitude_d/(double)current_depth;
// d_avg_height_m=d_avg_height_m/(double)current_depth;
}
}else{
return true;//indicates that the returned position is valid
}
}else{
return false;
}
}
void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
//function [phi, lambda, h] = cart2geo(X, Y, Z, i)
//CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
//coordinates (phi, lambda, h) on a selected reference ellipsoid.
//
//[phi, lambda, h] = cart2geo(X, Y, Z, i);
//
// Choices i of Reference Ellipsoid for Geographical Coordinates
// 0. International Ellipsoid 1924
// 1. International Ellipsoid 1967
// 2. World Geodetic System 1972
// 3. Geodetic Reference System 1980
// 4. World Geodetic System 1984
double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137};
double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda;
lambda = atan2(Y,X);
double ex2;
ex2 = (2-f[elipsoid_selection])*f[elipsoid_selection]/((1-f[elipsoid_selection])*(1-f[elipsoid_selection]));
double c;
c = a[elipsoid_selection]*sqrt(1+ex2);
double phi;
phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection]))*f[elipsoid_selection])));
double h = 0.1;
double oldh = 0;
double N;
int iterations = 0;
do{
oldh = h;
N = c/sqrt(1+ex2*(cos(phi)*cos(phi)));
phi = atan(Z/((sqrt(X*X+Y*Y)*(1-(2-f[elipsoid_selection])*f[elipsoid_selection]*N/(N+h)))));
h = sqrt(X*X+Y*Y)/cos(phi)-N;
iterations = iterations + 1;
if (iterations > 100)
{
std::cout<<"Failed to approximate h with desired precision. h-oldh= "<<h-oldh<<std::endl;
break;
}
}while (abs(h-oldh) > 1.0e-12);
d_latitude_d = phi*180.0/GPS_PI;
d_longitude_d = lambda*180/GPS_PI;
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
dtr = pi/180;
[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
cl = cos(lambda * dtr);
sl = sin(lambda * dtr);
cb = cos(phi * dtr);
sb = sin(phi * dtr);
F = [-sl -sb*cl cb*cl;
cl -sb*sl cb*sl;
0 cb sb];
local_vector = F' * dx;
E = local_vector(1);
N = local_vector(2);
U = local_vector(3);
hor_dis = sqrt(E^2 + N^2);
if hor_dis < 1.e-20
Az = 0;
El = 90;
else
Az = atan2(E, N)/dtr;
El = atan2(U, hor_dis)/dtr;
end
if Az < 0
Az = Az + 360;
end
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%%
*/
//}

98
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h Normal file
View File

@@ -0,0 +1,98 @@
/*!
* \file gps_l1_ca_ls_pvt.h
* \brief Least Squares Position, Velocity, and Time (PVT) solver, based on
* K.Borre Matlab receiver.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 GPS_L1_CA_LS_PVT_H_
#define GPS_L1_CA_LS_PVT_H_
#include <fstream>
#include <string>
#include <iostream>
#include <sstream>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>
#include <map>
#include <algorithm>
#include "gps_navigation_message.h"
#include "GPS_L1_CA.h"
//#include <itpp/itbase.h>
//#include <itpp/stat/misc_stat.h>
//#include <itpp/base/matfunc.h>
#include "armadillo"
//using namespace arma;
//using namespace itpp;
class gps_l1_ca_ls_pvt
{
private:
arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w);
arma::vec e_r_corr(double traveltime, arma::vec X_sat);
//void cart2geo();
//void topocent();
public:
int d_nchannels;
gps_navigation_message* d_ephemeris;
double d_pseudoranges_time_ms;
double d_latitude_d;
double d_longitude_d;
double d_height_m;
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
int d_averaging_depth;
double d_avg_latitude_d;
double d_avg_longitude_d;
double d_avg_height_m;
double d_x_m;
double d_y_m;
double d_z_m;
bool d_flag_dump_enabled;
std::string d_dump_filename;
std::ofstream d_dump_file;
void set_averaging_depth(int depth);
gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~gps_l1_ca_ls_pvt();
bool get_PVT(std::map<int,gnss_pseudorange> pseudoranges,double GPS_current_time,bool flag_averaging);
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
};
#endif

5
src/algorithms/PVT/libs/jamfile.jam Normal file
View File

@@ -0,0 +1,5 @@
project : build-dir ../../../../build ;
obj rinex_2_1_printer : rinex_2_1_printer.cc ;
obj gps_l1_ca_ls_pvt : gps_l1_ca_ls_pvt.cc ;
obj kml_printer : kml_printer.cc ;

126
src/algorithms/PVT/libs/kml_printer.cc Normal file
View File

@@ -0,0 +1,126 @@
/*!
* \file kml_printer.cc
* \brief Prints PVT information to a GoogleEarth kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "kml_printer.h"
#include <glog/log_severity.h>
#include <glog/logging.h>
#include <time.h>
bool kml_printer::set_headers(std::string filename)
{
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
kml_file.open(filename.c_str());
if (kml_file.is_open())
{
DLOG(INFO)<<"KML printer writting on "<<filename.c_str();
kml_file<<"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\r\n"
<<"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\r\n"
<<" <Document>\r\n"
<<" <name>GNSS Track</name>\r\n"
<<" <description>GNSS-SDR Receiver position log file created at "<<asctime (timeinfo)
<<" </description>\r\n"
<<"<Style id=\"yellowLineGreenPoly\">\r\n"
<<" <LineStyle>\r\n"
<<" <color>7f00ffff</color>\r\n"
<<" <width>1</width>\r\n"
<<" </LineStyle>\r\n"
<<"<PolyStyle>\r\n"
<<" <color>7f00ff00</color>\r\n"
<<"</PolyStyle>\r\n"
<<"</Style>\r\n"
<<"<Placemark>\r\n"
<<"<name>GNSS-SDR PVT</name>\r\n"
<<"<description>GNSS-SDR position log</description>\r\n"
<<"<styleUrl>#yellowLineGreenPoly</styleUrl>\r\n"
<<"<LineString>\r\n"
<<"<extrude>0</extrude>\r\n"
<<"<tessellate>1</tessellate>\r\n"
<<"<altitudeMode>absolute</altitudeMode>\r\n"
<<"<coordinates>\r\n";
return true;
}else{
return false;
}
}
bool kml_printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_values)
{
double latitude;
double longitude;
double height;
if (print_average_values==false)
{
latitude=position->d_latitude_d;
longitude=position->d_longitude_d;
height=position->d_height_m;
}else{
latitude=position->d_avg_latitude_d;
longitude=position->d_avg_longitude_d;
height=position->d_avg_height_m;
}
if (kml_file.is_open())
{
kml_file<<longitude<<","<<latitude<<","<<height<<"\r\n";
return true;
}else
{
return false;
}
}
bool kml_printer::close_file()
{
if (kml_file.is_open())
{
kml_file<<"</coordinates>\r\n"
<<"</LineString>\r\n"
<<"</Placemark>\r\n"
<<"</Document>\r\n"
<<"</kml>";
kml_file.close();
return true;
}else{
return false;
}
}
kml_printer::kml_printer ()
{
}
kml_printer::~kml_printer ()
{
}

59
src/algorithms/PVT/libs/kml_printer.h Normal file
View File

@@ -0,0 +1,59 @@
/*!
* \file kml_printer.h
* \brief Prints PVT information to a GoogleEarth kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 KML_PRINTER_H_
#define KML_PRINTER_H_
#include <iostream>
#include <fstream>
#include "gps_l1_ca_ls_pvt.h"
class kml_printer
{
private:
std::ofstream kml_file;
public:
bool set_headers(std::string filename);
bool print_position(gps_l1_ca_ls_pvt* position,bool print_average_values);
bool close_file();
kml_printer();
~kml_printer();
};
#endif

314
src/algorithms/PVT/libs/rinex_2_1_printer.cc Normal file
View File

@@ -0,0 +1,314 @@
#include "rinex_2_1_printer.h"
void rinex_printer::set_headers(std::string file_name)
{
correccio_primera_obs=1;
fp_rin = fopen((file_name+".09o").c_str(),"wt");
fp_rin_end = ftell(fp_rin);
fp_rin2 = fopen((file_name+".09n").c_str(),"wt");
fp_rin_end2 = ftell(fp_rin2);
// write RINEX headers
Rinex2ObsHeader();
Rinex2NavHeader();
}
rinex_printer::rinex_printer ()
{
}
rinex_printer::~rinex_printer ()
{
fclose(fp_rin);
fclose(fp_rin2);
}
void rinex_printer::Rinex2NavHeader()
{
if(fp_rin2 != NULL)
{
//calculate UTC_TIME
time_t tiempo;
char cad[80];
struct tm *tmPtr;
tiempo = time(NULL);
tmPtr = gmtime(&tiempo);
strftime( cad, 20, "%d-%b-%y %H:%M", tmPtr );
fseek(fp_rin2, fp_rin_end2, SEEK_SET);
//fprintf(fp_rin2,"----|---1|0---|---2|0---|---3|0---|---4|0---|---5|0---|---6|0---|---7|0---|---8|\n\n");
fprintf(fp_rin2," 2.10 NAVIGATION DATA RINEX VERSION / TYPE\n");
fprintf(fp_rin2,"GNSS-SDR-mercurio CTTC %s PGM / RUN BY / DATE\n",cad);
//fprintf(fp_rin2,"CTTC MARKER NAME\n");
//fprintf(fp_rin2,"0000 MARKERNUMBER\n");
fp_rin_end2 = ftell(fp_rin2);
correccio_primera_obs=1;
}
}
void rinex_printer::LogRinex2Nav(gps_navigation_message nav_msg){
if(fp_rin2 != NULL)
{
//double decimalday,daydecimalhour,decimalhour,decimalmin,decimalsec;
//double day,hour,minutes,seconds,enterseconds,a;
double gpstime;
struct tm *tmPtr;
time_t temps;
//1-Calcul data i hora gps
//Calculo el any,mes i dia a partir de l'hora UTC
//calculate UTC_TIME
char cad1[80];
char cad2[80];
//calculate date of gps time:
double setmanes=nav_msg.d_GPS_week+1024;
//Days & weeks between 00h 1 Jan 1970 and 00h 6 Jan 1980
//520 weeks and 12 days.
gpstime=nav_msg.d_master_clock;
temps=(520+setmanes)*7*24*3600+gpstime+17*24*3600;
tmPtr = gmtime(&temps);
strftime( cad1, 20, "%y %m %d", tmPtr );
//std::cout<<"gps_time="<<cad1<<std::endl;
sprintf(cad2,"%2.0f %2.0f %3.1f",(double)tmPtr->tm_hour,(double)tmPtr->tm_min,(double)tmPtr->tm_sec);
if(correccio_primera_obs==1){
//Escriure CORRECCIO DE TEMPS GPS a0 i a1;
fseek(fp_rin2, fp_rin_end2, SEEK_SET);
char correction[256],correction2[256];
double A0,A1;
/* 8.3819D-09 -7.4506D-09 -5.9605D-08 5.9605D-08 ION ALPHA
8.8 064D+04 -3.2768D+04 -1.9661D+05 1.9661D+05 ION BETA
5. 587935447693D-09 8.881784197001D-15 233472 1518 DELTA-UTC: A0,A1,T,W*/
A0=587.935447693E-09;
A1=8.881784197001E-15;
sprintf(correction,"%19.12E",A0);
sprintf(correction2,"%19.12E",A1);
double alpha0,alpha1,alpha2,alpha3,beta0,beta1,beta2,beta3;
alpha0=8.3819E-09;
alpha1=-7.4506E-09;
alpha2=-5.9605E-08;
alpha3=5.9605E-08;
beta0=064E+04;
beta1=-3.2768E+04;
beta2=-1.9661E+05;
beta3= 1.9661E+05;
//Format(&correction[0],1);
//Format(&correction2[0],1);
//fp_rin_correction_time = ftell(fp_rin2);
// fprintf(fp_rin2," %12.4E%12.4E%12.4E%12.4E ION ALPHA\n",alpha0,alpha1,alpha2,alpha3);
// fprintf(fp_rin2," %12.4E%12.4E%12.4E%12.4E ION BETA\n",beta0,beta1,beta2,beta3);
fprintf(fp_rin2," %s%s%9.0f%9d DELTA-UTC: A0,A1,T,W\n",correction,correction2,gpstime,(int)nav_msg.d_GPS_week+1024);
fprintf(fp_rin2," 15 LEAP SECONDS\n");
fprintf(fp_rin2," END OF HEADER\n");
fp_rin_end2 = ftell(fp_rin2);
correccio_primera_obs=0;
}
//preparacio lines de efemerides per imprimir!!!
char linia0[256],linia1[256],linia2[256],linia3[256],linia4[256],linia5[256],linia6[256],linia7[256];
char idef[256];
sprintf(idef,"%2.0d",nav_msg.d_satellite_PRN);
sprintf(linia0,"%19.12E%19.12E%19.12E",nav_msg.d_A_f0,nav_msg.d_A_f1,nav_msg.d_A_f2);
sprintf(linia1,"%19.12E%19.12E%19.12E%19.12E",nav_msg.d_IODE_SF2,nav_msg.d_Crs,nav_msg.d_Delta_n,nav_msg.d_M_0);
sprintf(linia2,"%19.12E%19.12E%19.12E%19.12E",nav_msg.d_Cuc,nav_msg.d_e_eccentricity,nav_msg.d_Cus,nav_msg.d_sqrt_A);
sprintf(linia3,"%19.12E%19.12E%19.12E%19.12E",nav_msg.d_Toe,nav_msg.d_Cic,nav_msg.d_OMEGA0,nav_msg.d_Cis);
sprintf(linia4,"%19.12E%19.12E%19.12E%19.12E",nav_msg.d_i_0,nav_msg.d_Crc,nav_msg.d_OMEGA,nav_msg.d_OMEGA_DOT);
sprintf(linia5,"%19.12E%19.12E%19.12E%19.12E",nav_msg.d_IDOT,0.0,nav_msg.d_GPS_week+1024.0,0.0);//CodeL2, L2pData
sprintf(linia6,"%19.12E%19.12E%19.12E%19.12E",nav_msg.d_SV_accuracy,nav_msg.d_SV_health,nav_msg.d_TGD,nav_msg.d_IODC);
sprintf(linia7,"%19.12E%19.12E",nav_msg.d_TOW,0.0); //fit interval is set to 0
fseek(fp_rin2, fp_rin_end2, SEEK_SET);
fprintf(fp_rin2,"%s %s %s%s\n",idef,cad1,cad2,linia0);
fprintf(fp_rin2," %s\n",linia1);
fprintf(fp_rin2," %s\n",linia2);
fprintf(fp_rin2," %s\n",linia3);
fprintf(fp_rin2," %s\n",linia4);
fprintf(fp_rin2," %s\n",linia5);
fprintf(fp_rin2," %s\n",linia6);
fprintf(fp_rin2," %s\n",linia7);
fp_rin_end2 = ftell(fp_rin2);
}
}
void rinex_printer::Rinex2ObsHeader()
{
//Clock_S *pClock = &tNav.master_clock; /* Clock solution */
if(fp_rin != NULL)
{
//calculate UTC_TIME
time_t tiempo;
char cad[80];
struct tm *tmPtr;
tiempo = time(NULL);
tmPtr = gmtime(&tiempo);
strftime( cad, 24, "%d/%m/%Y %H:%M:%S", tmPtr );
fseek(fp_rin, fp_rin_end, SEEK_SET);
//fprintf(fp_rin,"----|---1|0---|---2|0---|---3|0---|---4|0---|---5|0---|---6|0---|---7|0---|---8|\n\n");
fprintf(fp_rin," 2.10 Observation G (GPS) RINEX VERSION / TYPE\n");
fprintf(fp_rin,"GNSS-SDR-mercurio CTTC %s PGM / RUN BY / DATE\n",cad);
fprintf(fp_rin,"CTTC MARKER NAME\n");
//fprintf(fp_rin,"0000 MARKERNUMBER\n");
fprintf(fp_rin,"JAVIER ARRIBAS CTTC OBSERVER / AGENCY\n");
fprintf(fp_rin,"GNSS-SDR-mercurio SDR 1.0 REC # / TYPE / VERS\n");
fprintf(fp_rin,"0000000000 CTTC00000.00 ANT # / TYPE\n");
fprintf(fp_rin," 4797642.2790 166436.1500 4185504.6370 APPROX POSITION XYZ\n");
fprintf(fp_rin," 0.0000 0.0000 0.0000 ANTENNA: DELTA H/E/N\n");
fprintf(fp_rin," 1 0 WAVELENGTH FACT L1/2\n");
fprintf(fp_rin," 2 C1 L1 # / TYPES OF OBSERV\n");
fprintf(fp_rin," 0.1000 INTERVAL\n");
//INTRODUIR HORA PRIMERA OBSERVACIO
//ho escriure un cop hagi adquirit dades
//fprintf(fp_rin," 2001 3 24 13 10 36.0000000 TIME OF FIRST OBS\n");
//fprintf(fp_rin," END OF HEADER\n");
fp_rin_end = ftell(fp_rin);
temps_primera_obs=1;
}
}
void rinex_printer::LogRinex2Obs(gps_navigation_message nav_msg,double pseudoranges_clock, std::map<int,float> pseudoranges)
{
int ss;
char sat_vis[36];
for(int i=0;i<36;i++) sat_vis[i]=' ';
char packet[80];
int index=0;
char cad[2];
double setmanes;
std::map<int,float>::iterator pseudoranges_iter;
//necessito
//1-Data i hora<--- de struct Clock_S
//2-#sat visibles, identificador dat visibles-----> de Com fa a LogNav() // (Chan_Packet_S *) &tChan[lcv]->sv
//3-pseudodistancia de cada satèl·lit ----> Com fa a LogPseudo(), tb es pot treure la carrier_phase. Serveix per algo??
//4- El punt 1 i 2 s'han d'escriure a la mateixa línia. El punt 3 una línia per a cada satèl·lit.
if(fp_rin != NULL)
{
setmanes=nav_msg.d_GPS_week + 1024;
//1-Calcul data i hora gps
//Calculo el any,mes i dia a partir de l'hora UTC
//calculate UTC_TIME
time_t temps;
char cad1[80];
char cad2[80];
char cad3[80];
char cad4[80];
struct tm *tmPtr;
//Calculo hora, minut, segons a partir de pClocK.time =hora GPS
double decimalday,daydecimalhour,decimalhour,decimalmin,decimalsec;
double day,hour,minutes,seconds,enterseconds,a;
double gpstime;
gpstime=pseudoranges_clock; //[s]
//calculate date of gps time:
//Days & weeks between 00h 1 Jan 1970 and 00h 6 Jan 1980
//520 weeks and 12 days.
temps=(520+setmanes)*7*24*3600+gpstime+17*24*3600;
tmPtr = gmtime(&temps);
strftime( cad1, 20, " %y %m %d", tmPtr );
strftime( cad2, 20, " %Y %m %d", tmPtr );
decimalday=(gpstime/(24*3600));//Dies dins de la semana
daydecimalhour=modf(decimalday,&day);//day=#dies sencers, daydecimalhour=porcio de dia
daydecimalhour=daydecimalhour*24;//porcio de dia en hores
decimalhour=modf(daydecimalhour,&hour);//hour=hora del dia; decimalhour=porcio d'hora
decimalmin=decimalhour*60;//decimalmin=minuts del dia amb decimal
decimalsec=modf(decimalmin,&minutes);//minutes=minuts del dia enters,decimalsec=porcio de minuts
seconds=decimalsec*60;//seconds=segons del dia en decimal
a=modf(seconds,&enterseconds);
sprintf(cad4,"%6.0f%6.0f%13.7f",hour,minutes,seconds);
sprintf(cad3," %2.0f %2.0f%11.7f",hour,minutes,seconds);
//TODO: Include receiver clock offset
if(temps_primera_obs==1){
//Escriure Hora Primera Observació
fseek(fp_rin, fp_rin_end, SEEK_SET);
fprintf(fp_rin,"%s%s GPS TIME OF FIRST OBS\n",cad2,cad4);
fprintf(fp_rin,"00000CTTC MARKER NUMBER\n");
//fprintf(fp_rin,"Edited by .... COMMENT\n");
fprintf(fp_rin," END OF HEADER\n");
fp_rin_end = ftell(fp_rin);
temps_primera_obs=0;
}
//2-Num sat visibles i identificador
signed long int nsvs = 0;
//3-Escriure pseudodistancia
for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++)
{
//PER FORMAT RINEX2
nsvs++;
sprintf(cad,"%2.0f",(double)pseudoranges_iter->first); //satellite PRN ID
int k=3*index;
sat_vis[k]='G';
sat_vis[k+1]=cad[0];
sat_vis[k+2]=cad[1];
index++;
}
//sat_vis tinc vector de identif de sat visibles
//Per format RINEX2
//sprintf(packet,"%s%s 0%3d%s%12.9f",cad1,cad3,nsvs,sat_vis,offset);
sprintf(packet,"%s%s 0%3d%s",cad1,cad3,nsvs,sat_vis);
packet[69]=packet[68];
packet[68]=' ';
fseek(fp_rin, fp_rin_end, SEEK_SET);
fprintf(fp_rin,"%s\n",packet);
fp_rin_end = ftell(fp_rin);
//3-Escriure pseudodistancia
for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++)
{
ss=signalstrength(54.00); // TODO: include estimated signal strength
fseek(fp_rin, fp_rin_end, SEEK_SET);
fprintf(fp_rin,"%14.3f %14.3f %d\n",pseudoranges_iter->second,0.0,ss); //TODO: include the carrier phase
fp_rin_end = ftell(fp_rin);
}
}
}
int rinex_printer::signalstrength( double snr)
{
int ss;
if(snr<=12.00) ss=1;
else if(snr>12.00 && snr<=18.00) ss=2;
else if(snr>18.00 && snr<=24.00) ss=3;
else if(snr>24.00 && snr<=30.00) ss=4;
else if(snr>30.00 && snr<=36.00) ss=5;
else if(snr>36.00 && snr<=42.00) ss=6;
else if(snr>42.00 && snr<=48.00) ss=7;
else if(snr>48.00 && snr<=54) ss=8;
else if(snr>=54.00) ss=9;
return (ss);
}

52
src/algorithms/PVT/libs/rinex_2_1_printer.h Normal file
View File

@@ -0,0 +1,52 @@
/**
* Copyright notice
*/
/**
* Author: Javier Arribas, 2011. jarribas(at)cttc.es
* Luis Esteve, 2011. luis(at)epsilon-formacion.com
*/
#ifndef RINEX_PRINTER_H_
#define RINEX_PRINTER_H_
#include <string>
#include <iostream>
#include <sstream>
#include <stdlib.h>
#include <stdio.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>
#include <map>
#include <algorithm>
#include "gps_navigation_message.h"
class rinex_printer
{
private:
int temps_primera_obs; //per saber si he escrit el temps de la primera observació.
int correccio_primera_obs; //per saber si he escrit l correccio de temps de la primera observació.
int primeravegada;//per evitar problemes primera pseudodistancia
int PERMIS_RINEX; //PER DEIXAR ESCRIURE PER PRIMERA VEGADA AL RINEX
FILE *fp_rin; //!< RINEX OBS FILE Output
FILE *fp_rin2; //!< RINEX NAV FILE Output
unsigned long int fp_rin_end; //!< Hold place of last RINEX OBS FILE pointer, minus the header
unsigned long int fp_rin_end2; //!< Hold place of last RINEX NAV FILE pointer, minus the header
int signalstrength( double snr);
void Rinex2ObsHeader();
void Rinex2NavHeader();
public:
void set_headers(std::string file_name);
void LogRinex2Nav(gps_navigation_message nav_msg);
void LogRinex2Obs(gps_navigation_message nav_msg,double interframe_seconds, std::map<int,float> pseudoranges);
rinex_printer();
~rinex_printer();
};
#endif