mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-03-28 06:17:02 +00:00
Code Issues Releases Wiki Activity

Mara Branzanti GSoC commit: Galileo PVT block skeleton. Not usable yet!

Browse Source 
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@419 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas 2013-09-10 08:44:02 +00:00
parent 5fddaae79d
commit f4f22dffcd
10 changed files with 1433 additions and 3 deletions

5
src/algorithms/PVT/adapters/CMakeLists.txt

@ -16,7 +16,10 @@
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
set(PVT_ADAPTER_SOURCES gps_l1_ca_pvt.cc)
set(PVT_ADAPTER_SOURCES
gps_l1_ca_pvt.cc
galileo_e1_pvt.cc
)
include_directories(
$(CMAKE_CURRENT_SOURCE_DIR)

109
src/algorithms/PVT/adapters/galileo_e1_pvt.cc Normal file

@ -0,0 +1,109 @@
/*!
* \file galileo_e1_pvt.cc
* \brief Implementation of an adapter of a GALILEO E1 PVT solver block to a
* PvtInterface
* \author Javier Arribas, 2011. 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 "galileo_e1_pvt.h"
#include "configuration_interface.h"
#include "galileo_e1_pvt_cc.h"
#include <glog/log_severity.h>
#include <glog/logging.h>
using google::LogMessage;
GalileoE1Pvt::GalileoE1Pvt(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
boost::shared_ptr<gr::msg_queue> queue) :
role_(role),
in_streams_(in_streams),
out_streams_(out_streams),
queue_(queue)
{
// dump parameters
std::string default_dump_filename = "./pvt.dat";
std::string default_nmea_dump_filename = "./nmea_pvt.nmea";
std::string default_nmea_dump_devname = "/dev/tty1";
DLOG(INFO) << "role " << role;
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
// moving average depth parameters
int averaging_depth;
averaging_depth = configuration->property(role + ".averaging_depth", 10);
bool flag_averaging;
flag_averaging = configuration->property(role + ".flag_averaging", false);
// output rate
int output_rate_ms;
output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
// display rate
int display_rate_ms;
display_rate_ms = configuration->property(role + ".display_rate_ms", 500);
// NMEA Printer settings
bool flag_nmea_tty_port;
flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false);
std::string nmea_dump_filename;
nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename);
std::string nmea_dump_devname;
nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname);
// make PVT object
pvt_ = galileo_e1_make_pvt_cc(in_streams_, queue_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
}
GalileoE1Pvt::~GalileoE1Pvt()
{}
void GalileoE1Pvt::connect(gr::top_block_sptr top_block)
{
// Nothing to connect internally
DLOG(INFO) << "nothing to connect internally";
}
void GalileoE1Pvt::disconnect(gr::top_block_sptr top_block)
{
// Nothing to disconnect
}
gr::basic_block_sptr GalileoE1Pvt::get_left_block()
{
return pvt_;
}
gr::basic_block_sptr GalileoE1Pvt::get_right_block()
{
return pvt_;
}

97
src/algorithms/PVT/adapters/galileo_e1_pvt.h Normal file

@ -0,0 +1,97 @@
/*!
* \file galileo_e1_pvt.h
* \brief Interface of an adapter of a GALILEO E1 PVT solver block to a
* PvtInterface
* Position Velocity and Time
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2013 (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_GALILEO_E1_PVT_H_
#define GNSS_SDR_GALILEO_E1_PVT_H_
#include "pvt_interface.h"
#include "galileo_e1_pvt_cc.h"
#include <gnuradio/msg_queue.h>
class ConfigurationInterface;
/*!
* \brief This class implements a PvtInterface for GPS L1 C/A
*/
class GalileoE1Pvt : public PvtInterface
{
public:
GalileoE1Pvt(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
boost::shared_ptr<gr::msg_queue> queue);
virtual ~GalileoE1Pvt();
std::string role()
{
return role_;
}
//! Returns "GPS_L1_CA_PVT"
std::string implementation()
{
return "GALILEO_E1_PVT";
}
void connect(gr::top_block_sptr top_block);
void disconnect(gr::top_block_sptr top_block);
gr::basic_block_sptr get_left_block();
gr::basic_block_sptr get_right_block();
void reset()
{
return;
}
//! All blocks must have an item_size() function implementation. Returns sizeof(gr_complex)
size_t item_size()
{
return sizeof(gr_complex);
}
private:
galileo_e1_pvt_cc_sptr pvt_;
bool dump_;
unsigned int fs_in_;
std::string dump_filename_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
boost::shared_ptr<gr::msg_queue> queue_;
};
#endif

5
src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt

@ -16,7 +16,10 @@
# along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
#
set(PVT_GR_BLOCKS_SOURCES gps_l1_ca_pvt_cc.cc)
set(PVT_GR_BLOCKS_SOURCES
gps_l1_ca_pvt_cc.cc
galileo_e1_pvt_cc.cc
)
include_directories(
$(CMAKE_CURRENT_SOURCE_DIR)

250
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc Normal file

@ -0,0 +1,250 @@
/*!
* \file galileo_e1_pvt_cc.cc
* \brief Implementation of a Position Velocity and Time computation block for GPS L1 C/A
* \author Javier Arribas, 2011. 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 "galileo_e1_pvt_cc.h"
#include <iostream>
#include <sstream>
#include <vector>
#include <map>
#include <algorithm>
#include <bitset>
#include <gnuradio/gr_complex.h>
#include <gnuradio/io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
#include "control_message_factory.h"
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
#include "concurrent_map.h"
using google::LogMessage;
extern concurrent_map<Galileo_Ephemeris> global_galileo_ephemeris_map;
extern concurrent_map<Galileo_Iono> global_galileo_iono_map;
extern concurrent_map<Galileo_Utc_Model> global_galileo_utc_model_map;
galileo_e1_pvt_cc_sptr
galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname)
{
return galileo_e1_pvt_cc_sptr(new galileo_e1_pvt_cc(nchannels, queue, dump, dump_filename, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname));
}
galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname) :
gr::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(gr_complex)))
{
d_output_rate_ms = output_rate_ms;
d_display_rate_ms = display_rate_ms;
d_queue = queue;
d_dump = dump;
d_nchannels = nchannels;
d_dump_filename = dump_filename;
std::string 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
d_nmea_printer = new Nmea_Printer(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname);
d_dump_filename.append("_raw.dat");
dump_ls_pvt_filename.append("_ls_pvt.dat");
d_averaging_depth = averaging_depth;
d_flag_averaging = flag_averaging;
d_ls_pvt = new galileo_e1_ls_pvt(nchannels,dump_ls_pvt_filename,d_dump);
d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_sample_counter = 0;
d_last_sample_nav_output = 0;
d_rx_time = 0.0;
b_rinex_header_writen = false;
rp = new Rinex_Printer();
// ############# ENABLE DATA FILE LOG #################
if (d_dump == 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 dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
}
catch (std::ifstream::failure e)
{
std::cout << "Exception opening PVT dump file " << e.what() << std::endl;
}
}
}
}
galileo_e1_pvt_cc::~galileo_e1_pvt_cc()
{
d_kml_dump.close_file();
delete d_ls_pvt;
delete rp;
delete d_nmea_printer;
}
bool galileo_e1_pvt_cc::pseudoranges_pairCompare_min( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.Pseudorange_m) < (b.second.Pseudorange_m);
}
int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
d_sample_counter++;
std::map<int,Gnss_Synchro> gnss_pseudoranges_map;
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
for (unsigned int i=0; i<d_nchannels; i++)
{
if (in[i][0].Flag_valid_pseudorange == true)
{
gnss_pseudoranges_map.insert(std::pair<int,Gnss_Synchro>(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map
d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges)
}
}
// ############ 1. READ EPHEMERIS/UTC_MODE/IONO FROM GLOBAL MAPS ####
d_ls_pvt->galileo_ephemeris_map = global_galileo_ephemeris_map.get_map_copy();
if (global_galileo_utc_model_map.size()>0)
{
// UTC MODEL data is shared for all the Galileo satellites. Read always at ID=0
global_galileo_utc_model_map.read(0,d_ls_pvt->galileo_utc_model);
}
if (global_galileo_iono_map.size()>0)
{
// IONO data is shared for all the Galileo satellites. Read always at ID=0
global_galileo_iono_map.read(0,d_ls_pvt->galileo_iono);
}
// ############ 2 COMPUTE THE PVT ################################
// if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() >0)
// {
// // compute on the fly PVT solution
// //mod 8/4/2012 Set the PVT computation rate in this block
// if ((d_sample_counter % d_output_rate_ms) == 0)
// {
// bool pvt_result;
// pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging);
// if (pvt_result==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!
// {
// std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
// if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
// {
// rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second,d_rx_time);
// rp->rinex_nav_header(rp->navFile,d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model);
// b_rinex_header_writen = true; // do not write header anymore
// }
// }
// if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s)
// {
// // Limit the RINEX navigation output rate to 1/6 seg
// // Notice that d_sample_counter period is 1ms (for GPS correlators)
// if ((d_sample_counter-d_last_sample_nav_output)>=6000)
// {
// rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map);
// d_last_sample_nav_output=d_sample_counter;
// }
// std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
// if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
// {
// rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map);
// }
// }
// }
// }
//
// // DEBUG MESSAGE: Display position in console output
// if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true)
// {
// std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
// << " is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
// << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
//
// std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
// << " is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " <<
// d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP <<
// " GDOP = " << d_ls_pvt->d_GDOP <<std::endl;
// }
// // MULTIPLEXED FILE RECORDING - Record results to file
// if(d_dump == true)
// {
// try
// {
// double tmp_double;
// for (unsigned int i=0; i<d_nchannels ; i++)
// {
// tmp_double = in[i][0].Pseudorange_m;
// d_dump_file.write((char*)&tmp_double, sizeof(double));
// tmp_double = 0;
// d_dump_file.write((char*)&tmp_double, sizeof(double));
// d_dump_file.write((char*)&d_rx_time, sizeof(double));
// }
// }
// catch (std::ifstream::failure e)
// {
// std::cout << "Exception writing observables dump file " << e.what() << std::endl;
// }
// }
// }
//
// consume_each(1); //one by one
return 0;
}

96
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h Normal file

@ -0,0 +1,96 @@
/*!
* \file gps_l1_ca_pvt_cc.h
* \brief Interface of a Position Velocity and Time computation block for Galileo E1
* \author Javier Arribas, 2011. 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_GALILEO_E1_PVT_CC_H
#define GNSS_SDR_GALILEO_E1_PVT_CC_H
#include <fstream>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
#include <queue>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include "galileo_navigation_message.h"
#include "galileo_ephemeris.h"
#include "galileo_utc_model.h"
#include "galileo_iono.h"
#include "nmea_printer.h"
#include "kml_printer.h"
#include "rinex_printer.h"
#include "galileo_e1_ls_pvt.h" //this file is just a copy of gps
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"
class galileo_e1_pvt_cc;
typedef boost::shared_ptr<galileo_e1_pvt_cc> galileo_e1_pvt_cc_sptr;
galileo_e1_pvt_cc_sptr
galileo_e1_make_pvt_cc(unsigned int n_channels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname);
/*!
* \brief This class implements a block that computes the PVT solution
*/
class galileo_e1_pvt_cc : public gr::block
{
private:
friend galileo_e1_pvt_cc_sptr
galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname);
galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname);
boost::shared_ptr<gr::msg_queue> d_queue;
bool d_dump;
bool b_rinex_header_writen;
Rinex_Printer *rp;
unsigned int d_nchannels;
std::string d_dump_filename;
std::ofstream d_dump_file;
int d_averaging_depth;
bool d_flag_averaging;
int d_output_rate_ms;
int d_display_rate_ms;
long unsigned int d_sample_counter;
long unsigned int d_last_sample_nav_output;
Kml_Printer d_kml_dump;
Nmea_Printer *d_nmea_printer;
double d_rx_time;
galileo_e1_ls_pvt *d_ls_pvt; /*modify PVT/libs/galileo_e1_ls_pvt*/
bool pseudoranges_pairCompare_min( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b);
public:
~galileo_e1_pvt_cc (); //!< Default destructor
/*!
* \brief Set the queue for getting navigation messages from the GpsL1CaTelemetryDecoder
*/
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing
};
#endif

1
src/algorithms/PVT/libs/CMakeLists.txt

@ -18,6 +18,7 @@
set(PVT_LIB_SOURCES
gps_l1_ca_ls_pvt.cc
galileo_e1_ls_pvt.cc
kml_printer.cc
rinex_printer.cc
nmea_printer.cc

719
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc Normal file

@ -0,0 +1,719 @@
/*!
* \file galileo_e1_ls_pvt.cc
* \brief Implementation of a Least Squares Position, Velocity, and Time
* (PVT) solver, based on K.Borre's Matlab receiver.
* \author Javier Arribas, 2011. 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 <armadillo>
#include "galileo_e1_ls_pvt.h"
#include "GPS_L1_CA.h"
#include <glog/log_severity.h>
#include <glog/logging.h>
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
using google::LogMessage;
galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file)
{
// init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels;
d_ephemeris = new Galileo_Navigation_Message[nchannels];
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth = 0;
d_galileo_current_time = 0;
b_valid_position = false;
// ############# 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() << std::endl;
}
}
}
}
void galileo_e1_ls_pvt::set_averaging_depth(int depth)
{
d_averaging_depth = depth;
}
galileo_e1_ls_pvt::~galileo_e1_ls_pvt()
{
d_dump_file.close();
delete[] d_ephemeris;
}
arma::vec galileo_e1_ls_pvt::rotateSatellite(double traveltime, arma::vec X_sat)
{
/*
* Returns rotated satellite ECEF coordinates due to Earth
* rotation during signal travel time
*
* Inputs:
* travelTime - signal travel time
* X_sat - satellite's ECEF coordinates
*
* Returns:
* X_sat_rot - rotated satellite's coordinates (ECEF)
*/
//--- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = OMEGA_EARTH_DOT * traveltime;
//--- Build 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 galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w)
{
/* Computes the Least Squares Solution.
* Inputs:
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
* obs - Observations - the pseudorange measurements to each satellite
* w - weigths vector
*
* Returns:
* pos - receiver position and receiver clock error
* (in ECEF system: [X, Y, Z, dt])
*/
//=== Initialization =======================================================
int nmbOfIterations = 10; // TODO: include in config
int nmbOfSatellites;
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(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.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 = rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites
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, :));
}
//--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0,2),2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix ---------------------------------------
//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;
}
//--- Find position update ---------------------------------------------
x = arma::solve(w*A, w*omc); // Armadillo
//--- Apply position update --------------------------------------------
pos = pos + x;
if (arma::norm(x,2)<1e-4)
{
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
}
}
try
{
//-- compute the Dilution Of Precision values
//arma::mat Q;
d_Q = arma::inv(arma::htrans(A)*A);
}
catch(std::exception& e)
{
d_Q=arma::zeros(4,4);
}
return pos;
}
bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging)
{
// std::map<int,Gnss_Synchro>::iterator gnss_pseudoranges_iter;
// std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
// int valid_pseudoranges = gnss_pseudoranges_map.size();
//
// arma::mat W = arma::eye(valid_pseudoranges,valid_pseudoranges); //channels weights matrix
// arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
// arma::mat satpos = arma::zeros(3,valid_pseudoranges); //satellite positions matrix
//
// int GPS_week = 0;
// double utc = 0;
// double SV_clock_drift_s = 0;
// double SV_relativistic_clock_corr_s = 0;
// double TX_time_corrected_s;
// double SV_clock_bias_s = 0;
//
// d_flag_averaging = flag_averaging;
//
// // ********************************************************************************
// // ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) ****
// // ********************************************************************************
// int valid_obs = 0; //valid observations counter
// int obs_counter = 0;
// for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
// gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
// gnss_pseudoranges_iter++)
// {
// // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
// gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first);
// if (gps_ephemeris_iter != gps_ephemeris_map.end())
// {
// /*!
// * \todo Place here the satellite CN0 (power level, or weight factor)
// */
// W(obs_counter, obs_counter) = 1;
//
// // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// // first estimate of transmit time
// double Rx_time = GPS_current_time;
// double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m/GPS_C_m_s;
//
// // 2- compute the clock drift using the clock model (broadcast) for this SV
// SV_clock_drift_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time);
//
// // 3- compute the relativistic clock drift using the clock model (broadcast) for this SV
// SV_relativistic_clock_corr_s = gps_ephemeris_iter->second.sv_clock_relativistic_term(Tx_time);
//
// // 4- compute the current ECEF position for this SV using corrected TX time
// SV_clock_bias_s = SV_clock_drift_s + SV_relativistic_clock_corr_s - gps_ephemeris_iter->second.d_TGD;
// TX_time_corrected_s = Tx_time - SV_clock_bias_s;
// gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//
// satpos(0,obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
// satpos(1,obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
// satpos(2,obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
//
// // 5- fill the observations vector with the corrected pseudorranges
// obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s*GPS_C_m_s;
// d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
// d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
// valid_obs++;
//
// // SV ECEF DEBUG OUTPUT
// DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
// << " X=" << gps_ephemeris_iter->second.d_satpos_X
// << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
// << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
// << " [m] PR_obs=" << obs(obs_counter) << " [m]" << std::endl;
//
// // compute the UTC time for this SV (just to print the asociated UTC timestamp)
// GPS_week = gps_ephemeris_iter->second.i_GPS_week;
// utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
//
// }
// else // the ephemeris are not available for this SV
// {
// // no valid pseudorange for the current SV
// W(obs_counter, obs_counter) = 0; // SV de-activated
// obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
// DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first << std::endl;
// }
// obs_counter++;
// }
//
// // ********************************************************************************
// // ****** SOLVE LEAST SQUARES******************************************************
// // ********************************************************************************
// d_valid_observations = valid_obs;
// DLOG(INFO) << "(new)PVT: valid observations=" << valid_obs << std::endl;
//
// if (valid_obs >= 4)
// {
// arma::vec mypos;
// DLOG(INFO) << "satpos=" << satpos << std::endl;
// DLOG(INFO) << "obs="<< obs << std::endl;
// DLOG(INFO) << "W=" << W <<std::endl;
// mypos = leastSquarePos(satpos, obs, W);
// DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
// gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4);
// //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km)
// if (d_height_m>50000)
// {
// b_valid_position=false;
// return false; //erratic PVT
// }
// // Compute UTC time and print PVT solution
// double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
// boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek*(double)GPS_week);
// // 22 August 1999 last GPS time roll over
// boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
// d_position_UTC_time = p_time;
//
// DLOG(INFO) << "(new)Position at " << boost::posix_time::to_simple_string(p_time)
// << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
// << " [deg], Height= " << d_height_m << " [m]" << std::endl;
//
// // ###### Compute DOPs ########
//
// // 1- Rotation matrix from ECEF coordinates to ENU coordinates
// // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
//
// arma::mat F=arma::zeros(3,3);
// F(0,0)=-sin(GPS_TWO_PI*(d_longitude_d/360.0));
// F(0,1)=-sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
// F(0,2)=cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
//
// F(1,0)=cos((GPS_TWO_PI*d_longitude_d)/360.0);
// F(1,1)=-sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0);
// F(1,2)=cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
//
// F(2,0)=0;
// F(2,1)=cos((GPS_TWO_PI*d_latitude_d)/360.0);
// F(2,2)=sin((GPS_TWO_PI*d_latitude_d/360.0));
//
// // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
//
// arma::mat Q_ECEF=d_Q.submat( 0, 0, 2, 2);
// arma::mat DOP_ENU=arma::zeros(3,3);
//
// try
// {
// DOP_ENU=arma::htrans(F)*Q_ECEF*F;
// d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
// d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP
// d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
// d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
// d_TDOP = sqrt(d_Q(3,3)); // TDOP
// }catch(std::exception& ex)
// {
// d_GDOP = -1; // Geometric DOP
// d_PDOP = -1; // PDOP
// d_HDOP = -1; // HDOP
// d_VDOP = -1; // VDOP
// d_TDOP = -1; // TDOP
// }
//
// // ######## 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 LS dump file "<< e.what() << std::endl;
// }
// }
//
// // 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;
// b_valid_position = true;
// 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;
// b_valid_position = false;
// return false; //indicates that the returned position is not valid yet
// }
// }
// else
// {
// b_valid_position = true;
// return true; //indicates that the returned position is valid
// }
// }
// else
// {
// b_valid_position = false;
// return false;
// }
return false;
}
void galileo_e1_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
Choices 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
*/
const double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137};
const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda = atan2(Y,X);
double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1+ex2);
double 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 galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
{
/* 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).
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
Based in a Matlab function by Kai Borre
*/
*h = 0;
double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations
double rtd = 180/GPS_PI;
// compute square of eccentricity
double esq;
if (finv < 1.0E-20)
{
esq = 0;
}
else
{
esq = (2 - 1/finv) / finv;
}
// first guess
double P = sqrt(X*X + Y*Y); // P is distance from spin axis
//direct calculation of longitude
if (P > 1.0E-20)
{
*dlambda = atan2(Y,X) * rtd;
}
else
{
*dlambda = 0;
}
// correct longitude bound
if (*dlambda < 0)
{
*dlambda = *dlambda + 360.0;
}
double r = sqrt(P*P + Z*Z); // r is distance from origin (0,0,0)
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);
// iterate
double cosphi;
double N_phi;
double dP;
double dZ;
double oneesq = 1 - esq;
for (int i=0; i<maxit; i++)
{
sinphi = sin(*dphi);
cosphi = cos(*dphi);
// 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;
}
if (i == (maxit-1))
{
DLOG(INFO) << "The computation of geodetic coordinates did not converge";
}
}
*dphi = (*dphi) * rtd;
}
void galileo_e1_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx)
{
/* Transformation of vector dx into topocentric coordinate
system with origin at x
Inputs:
x - vector origin coordinates (in ECEF system [X; Y; Z;])
dx - vector ([dX; dY; dZ;]).
Outputs:
D - vector length. Units like the input
Az - azimuth from north positive clockwise, degrees
El - elevation angle, degrees
Based on a Matlab function by Kai Borre
*/
double lambda;
double phi;
double h;
double dtr = GPS_PI/180.0;
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
// Transform x into geodetic coordinates
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
arma::mat F = arma::zeros(3,3);
F(0,0) = -sl;
F(0,1) = -sb*cl;
F(0,2) = cb*cl;
F(1,0) = cl;
F(1,1) = -sb*sl;
F(1,2) = cb*sl;
F(2,0) = 0;
F(2,1) = cb;
F(2,2) = sb;
arma::vec local_vector;
local_vector = arma::htrans(F) * dx;
double E = local_vector(0);
double N = local_vector(1);
double 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
{
*Az = atan2(E, N)/dtr;
*El = atan2(U, hor_dis)/dtr;
}
if (*Az < 0)
{
*Az = *Az + 360.0;
}
*D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2));
}

147
src/algorithms/PVT/libs/galileo_e1_ls_pvt.h Normal file

@ -0,0 +1,147 @@
/*!
* \file galileo_e1_ls_pvt.h
* \brief Interface of a Least Squares Position, Velocity, and Time (PVT)
* solver, based on K.Borre's Matlab receiver.
* \author Javier Arribas, 2011. 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_GALILEO_E1_LS_PVT_H_
#define GNSS_SDR_GALILEO_E1_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 <cmath>
#include <map>
#include <algorithm>
#include "GPS_L1_CA.h"
#include "galileo_navigation_message.h"
#include "armadillo"
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
#include "galileo_ephemeris.h"
#include "galileo_utc_model.h"
#define PVT_MAX_CHANNELS 24
/*!
* \brief This class implements a simple PVT Least Squares solution
*/
class galileo_e1_ls_pvt
{
private:
arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w);
arma::vec rotateSatellite(double traveltime, arma::vec X_sat);
void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx);
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
public:
int d_nchannels; //! Number of available channels for positioning
int d_valid_observations; //! Number of valid pseudorange 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
Galileo_Navigation_Message* d_ephemeris;
// new ephemeris storage
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map;
// new utc_model storage
Galileo_Utc_Model galileo_utc_model;
// new iono storage
Galileo_Iono galileo_iono;
//double d_GPS_current_time;
double d_galileo_current_time;
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
double d_latitude_d; //! Latitude in degrees
double d_longitude_d; //! Longitude in degrees
double d_height_m; //! 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; //! Length of averaging window
double d_avg_latitude_d; //! Averaged latitude in degrees
double d_avg_longitude_d; //! Averaged longitude in degrees
double d_avg_height_m; //! Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
double d_HDOP;
double d_VDOP;
double d_TDOP;
bool d_flag_dump_enabled;
bool d_flag_averaging;
std::string d_dump_filename;
std::ofstream d_dump_file;
void set_averaging_depth(int depth);
galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~galileo_e1_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging);
/*!
* \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical
* coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid.
*
* \param[in] X [m] Cartesian coordinate
* \param[in] Y [m] Cartesian coordinate
* \param[in] Z [m] Cartesian coordinate
* \param[in] elipsoid_selection. Choices 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.
*
*/
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
};
#endif

7
src/core/receiver/gnss_block_factory.cc

@ -72,6 +72,7 @@
#include "gps_l1_ca_observables.h"
#include "galileo_e1_observables.h"
#include "gps_l1_ca_pvt.h"
#include "galileo_e1_pvt.h"
#if GN3S_DRIVER
#include "gn3s_signal_source.h"
@ -436,7 +437,11 @@ GNSSBlockInterface* GNSSBlockFactory::GetBlock(
block = new GpsL1CaPvt(configuration, role, in_streams,
out_streams, queue);
}
else if (implementation.compare("GALILEO_E1_PVT") == 0)
{
block = new GalileoE1Pvt(configuration, role, in_streams,
out_streams, queue);
}
// OUTPUT FILTERS --------------------------------------------------------------
else if (implementation.compare("Null_Sink_Output_Filter") == 0)
{