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

FIRST GALILEO STANDALONE PVT FIX!!

Browse Source 
- Bug fixes at Galileo telemetry decoder state machine
- Bug fixes at Galileo Ephemeris decoder (two complement's data interpreted as unsigned, mismatch at the LSB weights)

Galileo KML output is avalable!!

Galileo RINEX is still under construction

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@433 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas
2013-11-06 20:14:40 +00:00
parent 83a9d41b05
commit 87c5c212a4
20 changed files with 535 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
conf/gnss-sdr_Galileo_E1_IFEN.conf
View File

@@ -151,7 +151,7 @@ Resampler.implementation=Pass_Through
;######### CHANNELS GLOBAL CONFIG ############
;#count: Number of available satellite channels.
Channels.count=1
Channels.count=6
;#in_acquisition: Number of channels simultaneously acquiring
Channels.in_acquisition=1
;#system: GPS, GLONASS, GALILEO, SBAS or COMPASS
@@ -234,7 +234,7 @@ Acquisition.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition
;#threshold: Acquisition threshold
;Acquisition.threshold=0
;#pfa: Acquisition false alarm probability. This option overrides the threshold option. Only use with implementations: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition]
Acquisition.pfa=0.00005
Acquisition.pfa=0.00001
;#doppler_max: Maximum expected Doppler shift [Hz]
Acquisition.doppler_max=10000
;#doppler_max: Doppler step in the grid search [Hz]
@@ -271,10 +271,10 @@ Tracking.dump=false
Tracking.dump_filename=../data/veml_tracking_ch_
;#pll_bw_hz: PLL loop filter bandwidth [Hz]
Tracking.pll_bw_hz=15.0;
Tracking.pll_bw_hz=20.0;
;#dll_bw_hz: DLL loop filter bandwidth [Hz]
Tracking.dll_bw_hz=1.0;
Tracking.dll_bw_hz=2.0;
;#order: PLL/DLL loop filter order [2] or [3]
Tracking.order=3;
@@ -295,7 +295,7 @@ TelemetryDecoder.dump=false
Observables.implementation=Galileo_E1B_Observables
;#dump: Enable or disable the Observables internal binary data file logging [true] or [false]
Observables.dump=true
Observables.dump=false
;#dump_filename: Log path and filename.
Observables.dump_filename=./observables.dat

9
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
View File

@@ -177,10 +177,11 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
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_galileo(d_ls_pvt, d_flag_averaging);
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs
// 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!
@@ -210,7 +211,7 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
// rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map);
// }
// }
// }
}
}
// DEBUG MESSAGE: Display position in console output

2
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h
View File

@@ -43,7 +43,7 @@
#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 "galileo_e1_ls_pvt.h"
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"

26
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
View File

@@ -245,7 +245,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
double TX_time_corrected_s;
double SV_clock_bias_s = 0;
double GST=0;
//double GST=0;
d_flag_averaging = flag_averaging;
@@ -269,7 +269,18 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
//Galileo_week_number = galileo_ephemeris_iter->second.WN_5;//for GST
//double sec_in_day = 86400;
//double day_in_week = 7;
// t = WN*sec_in_day*day_in_week + TOW; // t is Galileo System Time to use to compute satellite positions
//JAVIER VERSION:
double Rx_time = galileo_current_time;
//to compute satellite position we need GST = WN+TOW (everything expressed in seconds)
//double Rx_time = galileo_current_time + Galileo_week_number*sec_in_day*day_in_week;
double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m/GALILEO_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV
@@ -299,11 +310,12 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
double GST=galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number,galileo_current_time);
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
// get time string gregorian calendar
//std::cout<<"UTC_raw="<<utc<<std::endl;
boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
std::cout << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time)<<std::endl;
//std::cout << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time)<<std::endl;
//end debug
// SV ECEF DEBUG OUTPUT
@@ -345,12 +357,16 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time;
std::cout << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time)<<std::endl;
//std::cout << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time)<<std::endl;
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
cart2geo((double)mypos(0), (double)mypos(1), (double)mypos(2), 4);
// TODO: Compute UTC time and print PVT solution
//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
}
DLOG(INFO) << "Galileo 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;

30
src/algorithms/PVT/libs/kml_printer.cc
View File

@@ -110,6 +110,36 @@ bool Kml_Printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_v
}
}
//ToDo: make the class ls_pvt generic and heritate the particular gps/gal/glo ls_pvt in order to
// reuse kml_printer functions
bool Kml_Printer::print_position_galileo(galileo_e1_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 << std::endl;
return true;
}
else
{
return false;
}
}
bool Kml_Printer::close_file()

6
src/algorithms/PVT/libs/kml_printer.h
View File

@@ -30,12 +30,13 @@
*/
#ifndef GNSS_SDR_KML_PRINTER_H_
#define GNSS_SDR_KML_PRINTER_H_
#ifndef GNSS_SDR_KML_PRINTER_H_
#define GNSS_SDR_KML_PRINTER_H_
#include <iostream>
#include <fstream>
#include "gps_l1_ca_ls_pvt.h"
#include "galileo_e1_ls_pvt.h"
/*!
@@ -50,6 +51,7 @@ private:
public:
bool set_headers(std::string filename);
bool print_position(gps_l1_ca_ls_pvt* position, bool print_average_values);
bool print_position_galileo(galileo_e1_ls_pvt* position, bool print_average_values);
bool close_file();
Kml_Printer();
~Kml_Printer();

1
src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
View File

@@ -157,6 +157,7 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
// compute the required symbol history shift in order to match the reference symbol
delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms;
//std::cout<<"delta_rx_time_ms["<<gnss_synchro_iter->second.Channel_ID<<"]="<<delta_rx_time_ms<<std::endl;
//std::cout<<"d_TOW_at_current_symbol["<<gnss_synchro_iter->second.Channel_ID<<"]="<<gnss_synchro_iter->second.d_TOW_at_current_symbol<<std::endl;
//compute the pseudorange
traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms;
//std::cout<<"traveltime_ms="<<traveltime_ms<<std::endl;

53
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
View File

@@ -64,10 +64,7 @@ galileo_e1b_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, lo
void galileo_e1b_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
for (unsigned i = 0; i < 3; i++)
{
ninput_items_required[i] = GALILEO_INAV_PAGE_SYMBOLS; //set the required sample history
}
ninput_items_required[0] = GALILEO_INAV_PAGE_SYMBOLS; //set the required sample history
}
@@ -386,50 +383,45 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items, gr_vector
Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block
//1. Copy the current tracking output
current_synchro_data = in[0][0];
//std::cout<<"flag TOW before add the telemetry decoder information: d_nav.flag_TOW_set"<<std::endl;
//2. Add the telemetry decoder information
if (this->d_flag_preamble==true and d_nav.flag_TOW_set==true) //update TOW at the preamble instant (todo: check for valid d_TOW) //flag preamble is true after the all page (even or odd) is recevived
if (this->d_flag_preamble==true and d_nav.flag_TOW_set==true)
//update TOW at the preamble instant
//flag preamble is true after the all page (even and odd) is recevived. I/NAV page period is 2 SECONDS
{
//std::cout<<"time stamp, identified preamble and TOW set" << std::endl;
Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
if((d_nav.flag_TOW_5 == 1) and (d_nav.Page_type_time_stamp == 5)) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
if(d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
//std::cout<< "Using TOW_5 for timestamping" << std::endl;
d_TOW_at_Preamble = d_nav.TOW_5+GALILEO_PAGE_SECONDS; //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later
/* 1 sec (GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD) is added because if we have a TOW value it means that we are at the and of the odd page*/
d_TOW_at_current_symbol = d_TOW_at_Preamble + GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD;
//std::cout << "d_TOW_at_current_symbol="<< d_TOW_at_current_symbol << std::endl;
d_nav.flag_TOW_5 = 0;
d_TOW_at_Preamble = d_nav.TOW_5+GALILEO_INAV_PAGE_PART_SECONDS; //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later
/* 1 sec (GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD) is added because
* if we have a TOW value it means that we are at the begining of the last page part
* (GNU Radio history keeps in a buffer the rest of the incomming frame part)*/
d_TOW_at_current_symbol = d_TOW_at_Preamble;// + GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD;
d_nav.flag_TOW_5 = false;
}
else if((d_nav.flag_TOW_6 == 1) and (d_nav.Page_type_time_stamp == 6)) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
else if(d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
//std::cout<< "Using TOW_6 for timestamping" << std::endl;
d_TOW_at_Preamble = d_nav.TOW_6+GALILEO_PAGE_SECONDS; //TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later
//std::cout << "d_TOW_at_Preamble="<< d_TOW_at_Preamble<< std::endl;
d_TOW_at_current_symbol = d_TOW_at_Preamble + GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD;
//std::cout << "d_TOW_at_current_symbol="<< d_TOW_at_current_symbol << std::endl;
d_nav.flag_TOW_6 = 0;
d_TOW_at_Preamble = d_nav.TOW_6+GALILEO_INAV_PAGE_PART_SECONDS;
//TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later
/* 1 sec (GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD) is added because
* if we have a TOW value it means that we are at the begining of the last page part
* (GNU Radio history keeps in a buffer the rest of the incomming frame part)*/
d_TOW_at_current_symbol = d_TOW_at_Preamble;// + GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD;
d_nav.flag_TOW_6 = false;
}
else
{
d_TOW_at_Preamble = d_TOW_at_Preamble + GALILEO_PAGE_SECONDS; //this is the even preamble after the last odd preamble
//std::cout << "d_TOW_at_Preamble="<< d_TOW_at_Preamble << std::endl;
d_TOW_at_current_symbol = d_TOW_at_Preamble + GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD;
//std::cout << "d_TOW_at_current_symbol="<< d_TOW_at_current_symbol << std::endl;
//this page has no timming information
d_TOW_at_Preamble = d_TOW_at_Preamble + GALILEO_INAV_PAGE_SECONDS;
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALIELO_E1_CODE_PERIOD;// + GALILEO_INAV_PAGE_PART_SYMBOLS*GALIELO_E1_CODE_PERIOD;
}
//std::cout << "Prn_timestamp_at_preamble_ms ="<< Prn_timestamp_at_preamble_ms << std::endl;
}
else //if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALIELO_E1_CODE_PERIOD;
//std::cout << "d_TOW_at_current_symbol="<< d_TOW_at_current_symbol << std::endl;
}
//if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
@@ -442,7 +434,6 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items, gr_vector
current_synchro_data.d_TOW = d_TOW_at_Preamble;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
//current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true);
current_synchro_data.Flag_preamble = d_flag_preamble;
current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms;

42
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
View File

@@ -444,26 +444,28 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
/*!
* \todo The stop timer has to be moved to the signal source!
*/
// debug: Second counter in channel 0
if (d_channel == 0)
{
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{
d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
}
}
else
{
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{
d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
}
}
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{
d_last_seg = floor(d_sample_counter / d_fs_in);
if (d_channel == 0)
{
// debug: Second counter in channel 0
tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
std::cout << tmp_str_stream.rdbuf() << std::flush;
}
tmp_str_stream << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", Doppler="<<d_carrier_doppler_hz<<" [Hz] CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
std::cout << tmp_str_stream.rdbuf() << std::flush;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
//if (d_channel == 0 || d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
}
else
{

4
src/core/receiver/control_thread.cc
View File

@@ -571,12 +571,12 @@ void ControlThread::galileo_iono_data_collector()
if (global_galileo_iono_map.read(0,galileo_iono_old))
{
// Check the Iono timestamp from UTC page (page 6). If it is newer, then update the Iono parameters
if (galileo_iono.WNot_6 > galileo_iono_old.WNot_6)
if (galileo_iono.WN_5 > galileo_iono_old.WN_5)
{
std::cout << "IONO record updated in global map--new GALILEO UTC-IONO Week Number"<< std::endl;
global_galileo_iono_map.write(0,galileo_iono);
}else{
if (galileo_iono.t0t_6 > galileo_iono_old.t0t_6)
if (galileo_iono.TOW_5 > galileo_iono_old.TOW_5)
{
std::cout << "IONO record updated in global map--new GALILEO UTC-IONO time of Week" << std::endl;
global_galileo_iono_map.write(0,galileo_iono);

19
src/core/system_parameters/Galileo_E1.h
View File

@@ -70,7 +70,8 @@ const int GALILEO_INAV_PREAMBLE_LENGTH_BITS = 10;
const int GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS = 250;
const int GALILEO_INAV_PAGE_PART_SYMBOLS = 250; //!< Each Galileo INAV pages are composed of two parts (even and odd) each of 250 symbols, including preamble. See Galileo ICD 4.3.2
const int GALILEO_INAV_PAGE_SYMBOLS = 500; //!< The complete Galileo INAV page length
const int GALILEO_PAGE_SECONDS = 1; //a page last 1 sec
const int GALILEO_INAV_PAGE_PART_SECONDS = 1; //a page part last 2 sec
const int GALILEO_INAV_PAGE_SECONDS = 2; //a full page last 2 sec
const int GALILEO_INAV_INTERLEAVER_ROWS = 8;
const int GALILEO_INAV_INTERLEAVER_COLS = 30;
const int GALILEO_TELEMETRY_RATE_BITS_SECOND = 250; //bps
@@ -88,7 +89,7 @@ const std::vector<std::pair<int,int>> IOD_nav_1_bit({{7,10}});
const std::vector<std::pair<int,int>> T0E_1_bit({{17,14}});
const double t0e_1_LSB = 60;
const std::vector<std::pair<int,int>> M0_1_bit({{31,32}});
const double M0_1_LSB = TWO_N31;
const double M0_1_LSB = PI_TWO_N31;
const std::vector<std::pair<int,int>> e_1_bit({{63,32}});
const double e_1_LSB = TWO_N33;
const std::vector<std::pair<int,int>> A_1_bit({{95,32}});
@@ -99,22 +100,22 @@ const double A_1_LSB_gal = TWO_N19;
/*Page 2 - Word type 2: Ephemeris (2/4)*/
const std::vector<std::pair<int,int>> IOD_nav_2_bit({{7,10}});
const std::vector<std::pair<int,int>> OMEGA_0_2_bit({{17,32}});
const double OMEGA_0_2_LSB = TWO_N31;
const double OMEGA_0_2_LSB = PI_TWO_N31;
const std::vector<std::pair<int,int>> i_0_2_bit({{49,32}});
const double i_0_2_LSB = TWO_N31;
const double i_0_2_LSB = PI_TWO_N31;
const std::vector<std::pair<int,int>> omega_2_bit({{81,32}});
const double omega_2_LSB = TWO_N31;
const double omega_2_LSB = PI_TWO_N31;
const std::vector<std::pair<int,int>> iDot_2_bit({{113,14}});
const double iDot_2_LSB = TWO_N43;
const double iDot_2_LSB = PI_TWO_N43;
//last two bits are reserved
/*Word type 3: Ephemeris (3/4) and SISA*/
const std::vector<std::pair<int,int>> IOD_nav_3_bit({{7,10}});
const std::vector<std::pair<int,int>> OMEGA_dot_3_bit({{17,24}});
const double OMEGA_dot_3_LSB = TWO_N43;
const double OMEGA_dot_3_LSB = PI_TWO_N43;
const std::vector<std::pair<int,int>> delta_n_3_bit({{41,16}});
const double delta_n_3_LSB = TWO_N43;
const double delta_n_3_LSB = PI_TWO_N43;
const std::vector<std::pair<int,int>> C_uc_3_bit({{57,16}});
const double C_uc_3_LSB = TWO_N29;
const std::vector<std::pair<int,int>> C_us_3_bit({{73,16}});
@@ -272,7 +273,7 @@ const double A_0G_10_LSB = TWO_N35;
const std::vector<std::pair<int,int>>A_1G_10_bit({{103,12}});
const double A_1G_10_LSB = TWO_N51;
const std::vector<std::pair<int,int>>t_0G_10_bit({{115,9}});
const std::vector<std::pair<int,int>>t_0G_10_bit({{115,8}});
const double t_0G_10_LSB = 3600;
const std::vector<std::pair<int,int>>WN_0G_10_bit({{123,6}});

1
src/core/system_parameters/galileo_ephemeris.cc
View File

@@ -205,6 +205,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) //when this funct
//t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
tk = transmitTime - t0e_1;
//std::cout<<"Diff t_tx-t_oe="<<tk<<std::endl;
// Corrected mean motion
n = n0 + delta_n_3;

63
src/core/system_parameters/galileo_ephemeris.h
View File

@@ -80,6 +80,7 @@ public:
double af2_4; //SV clock drift rate correction coefficient [s/s^2]
/*GST*/
//Not belong to ephemeris set (page 1 to 4)
double WN_5; //Week number
double TOW_5; //Time of Week
@@ -97,67 +98,7 @@ public:
unsigned int i_satellite_PRN; // SV PRN NUMBER
/*The following parameters refers to GPS
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
double d_Crs; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m]
double d_Delta_n; //!< Mean Motion Difference From Computed Value [semi-circles/s]
double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles]
double d_Cuc; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad]
double d_e_eccentricity; //!< Eccentricity [dimensionless]
double d_Cus; //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad]
double d_sqrt_A; //!< Square Root of the Semi-Major Axis [sqrt(m)]
double d_Toe; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s]
double d_Toc; //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s]
double d_Cic; //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad]
double d_OMEGA0; //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles]
double d_Cis; //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad]
double d_i_0; //!< Inclination Angle at Reference Time [semi-circles]
double d_Crc; //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m]
double d_OMEGA; //!< Argument of Perigee [semi-cicles]
double d_OMEGA_DOT; //!< Rate of Right Ascension [semi-circles/s]
double d_IDOT; //!< Rate of Inclination Angle [semi-circles/s]
int i_code_on_L2; //!< If 1, P code ON in L2; if 2, C/A code ON in L2;
int i_GPS_week; //!< GPS week number, aka WN [week]
bool b_L2_P_data_flag; //!< When true, indicates that the NAV data stream was commanded OFF on the P-code of the L2 channel
int i_SV_accuracy; //!< User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200E)
int i_SV_health;
double d_TGD; //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s]
double d_IODC; //!< Issue of Data, Clock
int i_AODO; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s]
bool b_fit_interval_flag;//!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours.
double d_spare1;
double d_spare2;
double d_A_f0; //!< Coefficient 0 of code phase offset model [s]
double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s]
double d_A_f2; //!< Coefficient 2 of code phase offset model [s/s^2]
// Flags
bool b_integrity_status_flag;
bool b_alert_flag; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
bool b_antispoofing_flag; //!< If true, the AntiSpoofing mode is ON in that SV
// clock terms derived from ephemeris data
double d_satClkDrift; // GPS clock error
double d_dtr; // relativistic clock correction term
// satellite positions
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis.
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system.
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP).
// Satellite velocity
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m]
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m]
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m]
std::map<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus
/*
template<class Archive>
\\brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.

4
src/core/system_parameters/galileo_iono.cc
View File

@@ -47,8 +47,8 @@ Galileo_Iono::Galileo_Iono()
Region4_flag_5 = false; // Ionospheric Disturbance Flag for region 4
Region5_flag_5 = false; // Ionospheric Disturbance Flag for region 5
t0t_6 = 0;
WNot_6 = 0;
TOW_5 = 0;
WN_5 = 0;
}

6
src/core/system_parameters/galileo_iono.h
View File

@@ -60,9 +60,9 @@ public:
bool Region4_flag_5; // Ionospheric Disturbance Flag for region 4
bool Region5_flag_5; // Ionospheric Disturbance Flag for region 5
/*from page 6 (UTC) to have a timestamp*/
double t0t_6;//UTC data reference Time of Week [s]
double WNot_6; //UTC data reference Week number [week]
/*from page 5 (UTC) to have a timestamp*/
double TOW_5;//UTC data reference Time of Week [s]
double WN_5; //UTC data reference Week number [week]
/*!

680
src/core/system_parameters/galileo_navigation_message.cc
View File

@@ -370,7 +370,7 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
// ToDo: Clean all the tests and create an independent google test code for the telemetry decoder.
//char correct_tail[7]="011110"; //the viterbi decoder output change the tail to this value (why?)
char correct_tail[7]="000000";
//char correct_tail[7]="000000";
int Page_type=0;
//std::cout << "Start decoding Galileo I/NAV " << std::endl;
@@ -486,7 +486,7 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int flag_ev
bool Galileo_Navigation_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class
{
if ((flag_ephemeris_1 == true) and (flag_ephemeris_2 == true) and (flag_ephemeris_3 == true) and (flag_ephemeris_4 == true))
if ((flag_ephemeris_1 == true) and (flag_ephemeris_2 == true) and (flag_ephemeris_3 == true) and (flag_ephemeris_4 == true) and (flag_iono_and_GST == true))
{
//if all ephemeris pages have the same IOD, then they belong to the same block
if ((IOD_nav_1 == IOD_nav_2) and (IOD_nav_3 == IOD_nav_4) and (IOD_nav_1 == IOD_nav_3))
@@ -602,8 +602,10 @@ Galileo_Iono Galileo_Navigation_Message::get_iono()
iono.Region4_flag_5 = Region4_flag_5; // Ionospheric Disturbance Flag for region 4
iono.Region5_flag_5 = Region5_flag_5; // Ionospheric Disturbance Flag for region 5
iono.t0t_6 = t0t_6;
iono.WNot_6 = WNot_6;
/*GST*/
// This is the ONLY page containing the Week Number (WN)
iono.TOW_5 = TOW_5;
iono.WN_5 = WN_5;
return iono;
}
@@ -713,7 +715,7 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
t0e_1 = t0e_1 * t0e_1_LSB;
DLOG(INFO) << "t0e_1= " << t0e_1 <<std::endl;
M0_1 = (double)read_navigation_unsigned(data_jk_bits, M0_1_bit);
M0_1 = (double)read_navigation_signed(data_jk_bits, M0_1_bit);
M0_1 = M0_1 * M0_1_LSB;
DLOG(INFO) << "M0_1= " << M0_1<<std::endl;
@@ -725,48 +727,46 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
A_1 = A_1 * A_1_LSB_gal;
DLOG(INFO) << "A_1= " << A_1 <<std::endl;
flag_ephemeris_1 = true;
break;
DLOG(INFO)<<"flag_tow_set"<< flag_TOW_set << std::endl;
break;
case 2: /*Word type 2: Ephemeris (2/4)*/
IOD_nav_2 = (int)read_navigation_unsigned(data_jk_bits, IOD_nav_2_bit);
DLOG(INFO)<<"IOD_nav_2= "<< IOD_nav_2 <<std::endl;
OMEGA_0_2 = (double)read_navigation_unsigned(data_jk_bits, OMEGA_0_2_bit);
OMEGA_0_2 = (double)read_navigation_signed(data_jk_bits, OMEGA_0_2_bit);
OMEGA_0_2 = OMEGA_0_2 * OMEGA_0_2_LSB;
DLOG(INFO)<<"OMEGA_0_2= "<< OMEGA_0_2 <<std::endl;
i_0_2 = (double)read_navigation_unsigned(data_jk_bits, i_0_2_bit);
i_0_2 = (double)read_navigation_signed(data_jk_bits, i_0_2_bit);
i_0_2 = i_0_2 * i_0_2_LSB;
DLOG(INFO)<<"i_0_2= "<< i_0_2 <<std::endl;
omega_2 = (double)read_navigation_unsigned(data_jk_bits, omega_2_bit);
omega_2 = (double)read_navigation_signed(data_jk_bits, omega_2_bit);
omega_2 = omega_2 * omega_2_LSB;
DLOG(INFO)<<"omega_2= "<< omega_2 <<std::endl;
iDot_2 = (double)read_navigation_unsigned(data_jk_bits, iDot_2_bit);
iDot_2 = (double)read_navigation_signed(data_jk_bits, iDot_2_bit);
iDot_2 = iDot_2 * iDot_2_LSB;
DLOG(INFO)<<"iDot_2= "<< iDot_2 <<std::endl;
flag_ephemeris_2 = true;
break;
DLOG(INFO)<<"flag_tow_set"<< flag_TOW_set << std::endl;
break;
case 3: /*Word type 3: Ephemeris (3/4) and SISA*/
IOD_nav_3 = (int)read_navigation_unsigned(data_jk_bits, IOD_nav_3_bit);
DLOG(INFO)<<"IOD_nav_3= "<< IOD_nav_3 <<std::endl;
OMEGA_dot_3 = (double)read_navigation_unsigned(data_jk_bits, OMEGA_dot_3_bit);
OMEGA_dot_3 = (double)read_navigation_signed(data_jk_bits, OMEGA_dot_3_bit);
OMEGA_dot_3 = OMEGA_dot_3 * OMEGA_dot_3_LSB;
DLOG(INFO)<<"OMEGA_dot_3= "<< OMEGA_dot_3 <<std::endl;
delta_n_3 = (double)read_navigation_unsigned(data_jk_bits, delta_n_3_bit);
delta_n_3 = (double)read_navigation_signed(data_jk_bits, delta_n_3_bit);
delta_n_3 = delta_n_3 * delta_n_3_LSB;
DLOG(INFO)<<"delta_n_3= "<< delta_n_3 <<std::endl;
C_uc_3 = (double)read_navigation_unsigned(data_jk_bits, C_uc_3_bit);
C_uc_3 = (double)read_navigation_signed(data_jk_bits, C_uc_3_bit);
C_uc_3 = C_uc_3 * C_uc_3_LSB;
DLOG(INFO)<<"C_uc_3= "<< C_uc_3 <<std::endl;
C_us_3 = (double)read_navigation_unsigned(data_jk_bits, C_us_3_bit);
C_us_3 = (double)read_navigation_signed(data_jk_bits, C_us_3_bit);
C_us_3 = C_us_3 * C_us_3_LSB;
DLOG(INFO)<<"C_us_3= "<< C_us_3 <<std::endl;
C_rc_3 = (double)read_navigation_unsigned(data_jk_bits, C_rc_3_bit);
C_rc_3 = (double)read_navigation_signed(data_jk_bits, C_rc_3_bit);
C_rc_3 = C_rc_3 * C_rc_3_LSB;
DLOG(INFO)<<"C_rc_3= "<< C_rc_3 <<std::endl;
C_rs_3 = (double)read_navigation_unsigned(data_jk_bits, C_rs_3_bit);
C_rs_3 = (double)read_navigation_signed(data_jk_bits, C_rs_3_bit);
C_rs_3 = C_rs_3 * C_rs_3_LSB;
DLOG(INFO)<<"C_rs_3= "<< C_rs_3 <<std::endl;
SISA_3 = (double)read_navigation_unsigned(data_jk_bits, SISA_3_bit);
@@ -780,23 +780,23 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
DLOG(INFO)<<"IOD_nav_4= "<< IOD_nav_4 <<std::endl;
SV_ID_PRN_4 = (int)read_navigation_unsigned(data_jk_bits, SV_ID_PRN_4_bit);
DLOG(INFO)<<"SV_ID_PRN_4= "<< SV_ID_PRN_4 <<std::endl;
C_ic_4 = (double)read_navigation_unsigned(data_jk_bits, C_ic_4_bit);
C_ic_4 = (double)read_navigation_signed(data_jk_bits, C_ic_4_bit);
C_ic_4 = C_ic_4 * C_ic_4_LSB;
DLOG(INFO)<<"C_ic_4= "<< C_ic_4 <<std::endl;
C_is_4 = (double)read_navigation_unsigned(data_jk_bits, C_is_4_bit);
C_is_4 = (double)read_navigation_signed(data_jk_bits, C_is_4_bit);
C_is_4 = C_is_4 * C_is_4_LSB;
DLOG(INFO)<<"C_is_4= "<< C_is_4 <<std::endl;
/*Clock correction parameters*/
t0c_4 = (double)read_navigation_unsigned(data_jk_bits, t0c_4_bit);
t0c_4 = t0c_4 * t0c_4_LSB;
DLOG(INFO)<<"t0c_4= "<< t0c_4 <<std::endl;
af0_4 = (double)read_navigation_unsigned(data_jk_bits, af0_4_bit);
af0_4 = (double)read_navigation_signed(data_jk_bits, af0_4_bit);
af0_4 = af0_4 * af0_4_LSB;
DLOG(INFO)<<"af0_4 = "<< af0_4 <<std::endl;
af1_4 = (double)read_navigation_unsigned(data_jk_bits, af1_4_bit);
af1_4 = (double)read_navigation_signed(data_jk_bits, af1_4_bit);
af1_4 = af1_4 * af1_4_LSB;
DLOG(INFO)<<"af1_4 = "<< af1_4 <<std::endl;
af2_4 = (double)read_navigation_unsigned(data_jk_bits, af2_4_bit);
af2_4 = (double)read_navigation_signed(data_jk_bits, af2_4_bit);
af2_4 = af2_4 * af2_4_LSB;
DLOG(INFO)<<"af2_4 = "<< af2_4 <<std::endl;
spare_4 = (double)read_navigation_unsigned(data_jk_bits, spare_4_bit);
@@ -811,10 +811,10 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
ai0_5 = (double)read_navigation_unsigned(data_jk_bits, ai0_5_bit);
ai0_5 = ai0_5 * ai0_5_LSB;
DLOG(INFO)<<"ai0_5= "<< ai0_5 <<std::endl;
ai1_5 = (double)read_navigation_unsigned(data_jk_bits, ai1_5_bit);
ai1_5 = (double)read_navigation_signed(data_jk_bits, ai1_5_bit);
ai1_5 = ai1_5 * ai1_5_LSB;
DLOG(INFO)<<"ai1_5= "<< ai1_5 <<std::endl;
ai2_5 = (double)read_navigation_unsigned(data_jk_bits, ai2_5_bit);
ai2_5 = (double)read_navigation_signed(data_jk_bits, ai2_5_bit);
ai2_5 = ai2_5 * ai2_5_LSB;
DLOG(INFO)<<"ai2_5= "<< ai2_5 <<std::endl;
/*Ionospheric disturbance flag*/
@@ -828,10 +828,10 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
DLOG(INFO)<<"Region4_flag_5= "<< Region4_flag_5 <<std::endl;
Region5_flag_5 = (bool)read_navigation_bool(data_jk_bits, Region5_5_bit);
DLOG(INFO)<<"Region5_flag_5= "<< Region5_flag_5 <<std::endl;
BGD_E1E5a_5 = (double)read_navigation_unsigned(data_jk_bits, BGD_E1E5a_5_bit);
BGD_E1E5a_5 = (double)read_navigation_signed(data_jk_bits, BGD_E1E5a_5_bit);
BGD_E1E5a_5 = BGD_E1E5a_5 * BGD_E1E5a_5_LSB;
DLOG(INFO)<<"BGD_E1E5a_5= "<< BGD_E1E5a_5 <<std::endl;
BGD_E1E5b_5 = (double)read_navigation_unsigned(data_jk_bits, BGD_E1E5b_5_bit);
BGD_E1E5b_5 = (double)read_navigation_signed(data_jk_bits, BGD_E1E5b_5_bit);
BGD_E1E5b_5 = BGD_E1E5b_5 * BGD_E1E5b_5_LSB;
DLOG(INFO)<<"BGD_E1E5b_5= "<< BGD_E1E5b_5 <<std::endl;
E5b_HS_5 = (double)read_navigation_unsigned(data_jk_bits, E5b_HS_5_bit);
@@ -847,24 +847,24 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
DLOG(INFO)<<"WN_5= "<< WN_5 <<std::endl;
TOW_5 = (double)read_navigation_unsigned(data_jk_bits, TOW_5_bit);
DLOG(INFO)<<"TOW_5= "<< TOW_5 <<std::endl;
flag_TOW_5 = 1;
flag_TOW_5 = true; //set to false externally
spare_5 = (double)read_navigation_unsigned(data_jk_bits, spare_5_bit);
DLOG(INFO)<<"spare_5= "<< spare_5 <<std::endl;
flag_iono_and_GST = true;
flag_TOW_set = true;
flag_iono_and_GST = true; //set to false externally
flag_TOW_set = true; //set to false externally
DLOG(INFO)<<"flag_tow_set"<< flag_TOW_set << std::endl;
break;
case 6: /*Word type 6: GST-UTC conversion parameters*/
A0_6= (double)read_navigation_unsigned(data_jk_bits, A0_6_bit);
A0_6= (double)read_navigation_signed(data_jk_bits, A0_6_bit);
A0_6= A0_6 * A0_6_LSB;
DLOG(INFO) << "A0_6= " << A0_6 << std::endl;
A1_6= (double)read_navigation_unsigned(data_jk_bits, A1_6_bit);
A1_6= (double)read_navigation_signed(data_jk_bits, A1_6_bit);
A1_6= A1_6 * A1_6_LSB;
DLOG(INFO) << "A1_6= " << A1_6 << std::endl;
Delta_tLS_6= (double)read_navigation_unsigned(data_jk_bits, Delta_tLS_6_bit);
Delta_tLS_6= (double)read_navigation_signed(data_jk_bits, Delta_tLS_6_bit);
DLOG(INFO) << "Delta_tLS_6= " << Delta_tLS_6 << std::endl;
t0t_6= (double)read_navigation_unsigned(data_jk_bits, t0t_6_bit);
@@ -880,14 +880,14 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
DN_6= (double)read_navigation_unsigned(data_jk_bits, DN_6_bit);
DLOG(INFO) << "DN_6= " << DN_6 << std::endl;
Delta_tLSF_6= (double)read_navigation_unsigned(data_jk_bits, Delta_tLSF_6_bit);
Delta_tLSF_6= (double)read_navigation_signed(data_jk_bits, Delta_tLSF_6_bit);
DLOG(INFO) << "Delta_tLSF_6= " << Delta_tLSF_6 << std::endl;
TOW_6= (double)read_navigation_unsigned(data_jk_bits, TOW_6_bit);
DLOG(INFO) << "TOW_6= " << TOW_6 << std::endl;
flag_TOW_6 = 1;
flag_utc_model = true;
flag_TOW_set = true;
flag_TOW_6 = true; //set to false externally
flag_utc_model = true; //set to false externally
flag_TOW_set = true; //set to false externally
DLOG(INFO)<<"flag_tow_set"<< flag_TOW_set << std::endl;
break;
@@ -906,7 +906,7 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
SVID1_7= (double)read_navigation_unsigned(data_jk_bits, SVID1_7_bit);
DLOG(INFO) << "SVID1_7= " << SVID1_7 << std::endl;
DELTA_A_7= (double)read_navigation_unsigned(data_jk_bits, DELTA_A_7_bit);
DELTA_A_7= (double)read_navigation_signed(data_jk_bits, DELTA_A_7_bit);
DELTA_A_7= DELTA_A_7 * DELTA_A_7_LSB;
DLOG(INFO) << "DELTA_A_7= " << DELTA_A_7 << std::endl;
@@ -914,23 +914,23 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
e_7= e_7 * e_7_LSB;
DLOG(INFO) << "e_7= " << e_7 << std::endl;
omega_7= (double)read_navigation_unsigned(data_jk_bits, omega_7_bit);
omega_7= (double)read_navigation_signed(data_jk_bits, omega_7_bit);
omega_7= omega_7 * omega_7_LSB;
DLOG(INFO) << "omega_7= " << omega_7 << std::endl;
delta_i_7= (double)read_navigation_unsigned(data_jk_bits, delta_i_7_bit);
delta_i_7= (double)read_navigation_signed(data_jk_bits, delta_i_7_bit);
delta_i_7= delta_i_7 * delta_i_7_LSB;
DLOG(INFO) << "delta_i_7= " << delta_i_7 << std::endl;
Omega0_7= (double)read_navigation_unsigned(data_jk_bits, Omega0_7_bit);
Omega0_7= (double)read_navigation_signed(data_jk_bits, Omega0_7_bit);
Omega0_7= Omega0_7 * Omega0_7_LSB;
DLOG(INFO) << "Omega0_7= " << Omega0_7 << std::endl;
Omega_dot_7= (double)read_navigation_unsigned(data_jk_bits, Omega_dot_7_bit);
Omega_dot_7= (double)read_navigation_signed(data_jk_bits, Omega_dot_7_bit);
Omega_dot_7= Omega_dot_7 * Omega_dot_7_LSB;
DLOG(INFO) << "Omega_dot_7= " << Omega_dot_7 << std::endl;
M0_7= (double)read_navigation_unsigned(data_jk_bits, M0_7_bit);
M0_7= (double)read_navigation_signed(data_jk_bits, M0_7_bit);
M0_7= M0_7 * M0_7_LSB;
DLOG(INFO) << "M0_7= " << M0_7 << std::endl;
flag_almanac_1 = true;
@@ -939,14 +939,14 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
case 8: /*Word type 8: Almanac for SVID1 (2/2) and SVID2 (1/2)*/
IOD_a_8= (double)read_navigation_unsigned(data_jk_bits, IOD_a_8_bit);
IOD_a_8= (double)read_navigation_signed(data_jk_bits, IOD_a_8_bit);
DLOG(INFO) << "IOD_a_8= " << IOD_a_8 << std::endl;
af0_8= (double)read_navigation_unsigned(data_jk_bits, af0_8_bit);
af0_8= (double)read_navigation_signed(data_jk_bits, af0_8_bit);
af0_8= af0_8 * af0_8_LSB;
DLOG(INFO) << "af0_8= " << af0_8 << std::endl;
af1_8= (double)read_navigation_unsigned(data_jk_bits, af1_8_bit);
af1_8= (double)read_navigation_signed(data_jk_bits, af1_8_bit);
af1_8= af1_8 * af1_8_LSB;
DLOG(INFO) << "af1_8= " << af1_8 << std::endl;
@@ -959,7 +959,7 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
SVID2_8= (double)read_navigation_unsigned(data_jk_bits, SVID2_8_bit);
DLOG(INFO) << "SVID2_8= " << SVID2_8 << std::endl;
DELTA_A_8= (double)read_navigation_unsigned(data_jk_bits, DELTA_A_8_bit);
DELTA_A_8= (double)read_navigation_signed(data_jk_bits, DELTA_A_8_bit);
DELTA_A_8= DELTA_A_8 * DELTA_A_8_LSB;
DLOG(INFO) << "DELTA_A_8= " << DELTA_A_8 << std::endl;
@@ -967,19 +967,19 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
e_8= e_8 * e_8_LSB;
DLOG(INFO) << "e_8= " << e_8 << std::endl;
omega_8= (double)read_navigation_unsigned(data_jk_bits, omega_8_bit);
omega_8= (double)read_navigation_signed(data_jk_bits, omega_8_bit);
omega_8= omega_8 * omega_8_LSB;
DLOG(INFO) << "omega_8= " << omega_8 << std::endl;
delta_i_8= (double)read_navigation_unsigned(data_jk_bits, delta_i_8_bit);
delta_i_8= (double)read_navigation_signed(data_jk_bits, delta_i_8_bit);
delta_i_8= delta_i_8 * delta_i_8_LSB;
DLOG(INFO) << "delta_i_8= " << delta_i_8 << std::endl;
Omega0_8= (double)read_navigation_unsigned(data_jk_bits, Omega0_8_bit);
Omega0_8= (double)read_navigation_signed(data_jk_bits, Omega0_8_bit);
Omega0_8= Omega0_8 * Omega0_8_LSB;
DLOG(INFO) << "Omega0_8= " << Omega0_8 << std::endl;
Omega_dot_8= (double)read_navigation_unsigned(data_jk_bits, Omega_dot_8_bit);
Omega_dot_8= (double)read_navigation_signed(data_jk_bits, Omega_dot_8_bit);
Omega_dot_8= Omega_dot_8 * Omega_dot_8_LSB;
DLOG(INFO) << "Omega_dot_8= " << Omega_dot_8 << std::endl;
flag_almanac_2 = true;
@@ -998,15 +998,15 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
t0a_9= t0a_9 * t0a_9_LSB;
DLOG(INFO) << "t0a_9= " << t0a_9 << std::endl;
M0_9= (double)read_navigation_unsigned(data_jk_bits, M0_9_bit);
M0_9= (double)read_navigation_signed(data_jk_bits, M0_9_bit);
M0_9= M0_9 * M0_9_LSB;
DLOG(INFO) << "M0_9= " << M0_9 << std::endl;
af0_9= (double)read_navigation_unsigned(data_jk_bits, af0_9_bit);
af0_9= (double)read_navigation_signed(data_jk_bits, af0_9_bit);
af0_9= af0_9 * af0_9_LSB;
DLOG(INFO) << "af0_9= " << af0_9 << std::endl;
af1_9= (double)read_navigation_unsigned(data_jk_bits, af1_9_bit);
af1_9= (double)read_navigation_signed(data_jk_bits, af1_9_bit);
af1_9= af1_9 * af1_9_LSB;
DLOG(INFO) << "af1_9= " << af1_9 << std::endl;
@@ -1020,7 +1020,7 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
DLOG(INFO) << "SVID3_9= " << SVID3_9 << std::endl;
DELTA_A_9= (double)read_navigation_unsigned(data_jk_bits, DELTA_A_9_bit);
DELTA_A_9= (double)read_navigation_signed(data_jk_bits, DELTA_A_9_bit);
DELTA_A_9= DELTA_A_9 * DELTA_A_9_LSB;
DLOG(INFO) << "DELTA_A_9= " << DELTA_A_9 << std::endl;
@@ -1028,11 +1028,11 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
e_9= e_9 * e_9_LSB;
DLOG(INFO) << "e_9= " << e_9 << std::endl;
omega_9= (double)read_navigation_unsigned(data_jk_bits, omega_9_bit);
omega_9= (double)read_navigation_signed(data_jk_bits, omega_9_bit);
omega_9= omega_9 * omega_9_LSB;
DLOG(INFO) << "omega_9= " << omega_9 << std::endl;
delta_i_9= (double)read_navigation_unsigned(data_jk_bits, delta_i_9_bit);
delta_i_9= (double)read_navigation_signed(data_jk_bits, delta_i_9_bit);
delta_i_9= delta_i_9 * delta_i_9_LSB;
DLOG(INFO) << "delta_i_9= " << delta_i_9 << std::endl;
flag_almanac_3 = true;
@@ -1044,23 +1044,23 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
IOD_a_10= (double)read_navigation_unsigned(data_jk_bits, IOD_a_10_bit);
DLOG(INFO) << "IOD_a_10= " << IOD_a_10 << std::endl;
Omega0_10= (double)read_navigation_unsigned(data_jk_bits, Omega0_10_bit);
Omega0_10= (double)read_navigation_signed(data_jk_bits, Omega0_10_bit);
Omega0_10= Omega0_10 * Omega0_10_LSB;
DLOG(INFO) << "Omega0_10= " << Omega0_10 << std::endl;
Omega_dot_10= (double)read_navigation_unsigned(data_jk_bits, Omega_dot_10_bit);
Omega_dot_10= (double)read_navigation_signed(data_jk_bits, Omega_dot_10_bit);
Omega_dot_10= Omega_dot_10 * Omega_dot_10_LSB;
DLOG(INFO) << "Omega_dot_10= " << Omega_dot_10 << std::endl;
M0_10= (double)read_navigation_unsigned(data_jk_bits, M0_10_bit);
M0_10= (double)read_navigation_signed(data_jk_bits, M0_10_bit);
M0_10= M0_10 * M0_10_LSB;
DLOG(INFO) << "M0_10= " << M0_10 << std::endl;
af0_10= (double)read_navigation_unsigned(data_jk_bits, af0_10_bit);
af0_10= (double)read_navigation_signed(data_jk_bits, af0_10_bit);
af0_10= af0_10 * af0_10_LSB;
DLOG(INFO) << "af0_10= " << af0_10 << std::endl;
af1_10= (double)read_navigation_unsigned(data_jk_bits, af1_10_bit);
af1_10= (double)read_navigation_signed(data_jk_bits, af1_10_bit);
af1_10= af1_10 * af1_10_LSB;
DLOG(INFO) << "af1_10= " << af1_10 << std::endl;
@@ -1070,15 +1070,15 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
E1B_HS_10= (double)read_navigation_unsigned(data_jk_bits, E1B_HS_10_bit);
DLOG(INFO) << "E1B_HS_10= " << E1B_HS_10 << std::endl;
A_0G_10= (double)read_navigation_unsigned(data_jk_bits, A_0G_10_bit);
A_0G_10= (double)read_navigation_signed(data_jk_bits, A_0G_10_bit);
A_0G_10= A_0G_10 * A_0G_10_LSB;
DLOG(INFO) << "A_0G_10= " << A_0G_10 << std::endl;
A_1G_10= (double)read_navigation_unsigned(data_jk_bits, A_1G_10_bit);
A_1G_10= (double)read_navigation_signed(data_jk_bits, A_1G_10_bit);
A_1G_10= A_1G_10 * A_1G_10_LSB;
DLOG(INFO) << "A_1G_10= " << A_1G_10 << std::endl;
t_0G_10= (double)read_navigation_unsigned(data_jk_bits, A_1G_10_bit);
t_0G_10= (double)read_navigation_unsigned(data_jk_bits, t_0G_10_bit);
t_0G_10= t_0G_10 * t_0G_10_LSB;
DLOG(INFO) << "t_0G_10= " << t_0G_10 << std::endl;
@@ -1107,278 +1107,278 @@ int Galileo_Navigation_Message::page_jk_decoder(const char *data_jk)
void Galileo_Navigation_Message::satellitePosition(double transmitTime) //when this function in used, the input must be the transmitted time (t) in second computed by Galileo_System_Time (above function)
{
double tk; // Time from ephemeris reference epoch
//double t; // Galileo System Time (ICD, paragraph 5.1.2)
double a; // Semi-major axis
double n; // Corrected mean motion
double n0; // Computed mean motion
double M; // Mean anomaly
double E; //Eccentric Anomaly (to be solved by iteration)
double E_old;
double dE;
double nu; //True anomaly
double phi; //argument of Latitude
double u; // Correct argument of latitude
double r; // Correct radius
double i;
double Omega;
// Find Galileo satellite's position ----------------------------------------------
// Restore semi-major axis
a = A_1*A_1;
// Computed mean motion
n0 = sqrt(GALILEO_GM / (a*a*a));
// Time from ephemeris reference epoch
//tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
//t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
tk = transmitTime - t0e_1;
// Corrected mean motion
n = n0 + delta_n_3;
// Mean anomaly
M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
// Initial guess of eccentric anomaly
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1; ii<20; ii++)
{
E_old = E;
E = M + e_1 * sin(E);
dE = fmod(E - E_old, 2*GALILEO_PI);
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
break;
}
}
// Compute the true anomaly
double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E);
double tmp_X = cos(E) - e_1;
nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude)
phi = nu + omega_2;
// Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2*GALILEO_PI));
// Correct argument of latitude
u = phi + C_uc_3 * cos(2*phi) + C_us_3 * sin(2*phi);
// Correct radius
r = a * (1 - e_1*cos(E)) + C_rc_3 * cos(2*phi) + C_rs_3 * sin(2*phi);
// Correct inclination
i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2*phi) + C_is_4 * sin(2*phi);
// Compute the angle between the ascending node and the Greenwich meridian
Omega = OMEGA_0_2 + (OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT)*tk - GALILEO_OMEGA_EARTH_DOT * t0e_1;
// Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2*GALILEO_PI), (2*GALILEO_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
galileo_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
galileo_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); //***********************NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus
galileo_satpos_Z = sin(u) * r * sin(i);
std::cout << "Galileo satellite position X [m]: " << galileo_satpos_X << std::endl;
std::cout << "Galileo satellite position Y [m]: " << galileo_satpos_Y << std::endl;
std::cout << "Galileo satellite position Z [m]: " << galileo_satpos_Z << std::endl;
double vector_position = sqrt(galileo_satpos_X*galileo_satpos_X + galileo_satpos_Y*galileo_satpos_Y + galileo_satpos_Z*galileo_satpos_Z);
std::cout << "Vector Earth Center-Satellite [Km]: " << vector_position/1000 << std::endl;
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT;
galileo_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + galileo_satpos_X * cos(Omega) - galileo_satpos_Y * cos(i) * sin(Omega);
galileo_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + galileo_satpos_X * sin(Omega) + galileo_satpos_Y * cos(i) * cos(Omega);
galileo_satvel_Z = galileo_satpos_Y * sin(i);
}
double Galileo_Navigation_Message::Galileo_System_Time(double WN, double TOW){
/* GALIELO SYSTEM TIME, ICD 5.1.2
* input parameter:
* WN: The Week Number is an integer counter that gives the sequential week number
from the origin of the Galileo time. It covers 4096 weeks (about 78 years).
Then the counter is reset to zero to cover additional period modulo 4096
TOW: The Time of Week is defined as the number of seconds that have occurred since
the transition from the previous week. The TOW covers an entire week from 0 to
604799 seconds and is reset to zero at the end of each week
WN and TOW are received in page 5
output:
t: it is the transmitted time in Galileo System Time (expressed in seconds)
The GST start epoch shall be 00:00 UT on Sunday 22nd August 1999 (midnight between 21st and 22nd August).
At the start epoch, GST shall be ahead of UTC by thirteen (13)
leap seconds. Since the next leap second was inserted at 01.01.2006, this implies that
as of 01.01.2006 GST is ahead of UTC by fourteen (14) leap seconds.
The epoch denoted in the navigation messages by TOW and WN
will be measured relative to the leading edge of the first chip of the
first code sequence of the first page symbol. The transmission timing of the navigation
message provided through the TOW is synchronised to each satellite’s version of Galileo System Time (GST).
*
*/
double t=0;
double sec_in_day = 86400;
double day_in_week = 7;
t = WN * sec_in_day * day_in_week + TOW; // second from the origin of the Galileo time
return t;
}
double Galileo_Navigation_Message::sv_clock_drift(double transmitTime){
/* Satellite Time Correction Algorithm, ICD 5.1.4
*
*/
double dt;
dt = transmitTime - t0c_4;
Galileo_satClkDrift = af0_4 + af1_4*dt + (af2_4 * dt)*(af2_4 * dt) + Galileo_dtr;
return Galileo_satClkDrift;
}
// compute the relativistic correction term
double Galileo_Navigation_Message::sv_clock_relativistic_term(double transmitTime) //Satellite Time Correction Algorithm, ICD 5.1.4
{
double tk;
double a;
double n;
double n0;
double E;
double E_old;
double dE;
double M;
// Restore semi-major axis
a = A_1*A_1;
n0 = sqrt(GALILEO_GM / (a*a*a));
// Time from ephemeris reference epoch
//tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
//t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
tk = transmitTime - t0e_1;
// Corrected mean motion
n = n0 + delta_n_3;
// Mean anomaly
M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
// Initial guess of eccentric anomaly
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1; ii<20; ii++)
{
E_old = E;
E = M + e_1 * sin(E);
dE = fmod(E - E_old, 2*GALILEO_PI);
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
break;
}
}
// Compute relativistic correction term
Galileo_dtr = GALILEO_F * e_1* A_1 * sin(E);
return Galileo_dtr;
}
double Galileo_Navigation_Message::GST_to_UTC_time(double t_e, int WN) //t_e is GST (WN+TOW) in second
{
double t_Utc;
double t_Utc_daytime;
double Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
// Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = WN_LSF_6 - WN;
if ((weeksToLeapSecondEvent) >= 0) // is not in the past
{
//Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60;
if (weeksToLeapSecondEvent > 0)
{
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
else //we are in the same week than the leap second event
{
if (abs(t_e - secondOfLeapSecondEvent) > 21600)
{
/* 5.1.7a
* Whenever the leap second adjusted time indicated by the WN_LSF and the DN values
* is not in the past (relative to the user's present time), and the user's
* present time does not fall in the time span which starts at six hours prior
* to the effective time and ends at six hours after the effective time,
* the GST/Utc relationship is given by
*/
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
else
{
/* 5.1.7b
* Whenever the user's current time falls within the time span of six hours
* prior to the leap second adjustment to six hours after the adjustment time, ,
* the effective time is computed according to the following equations:
*/
int W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200;
t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
//implement something to handle a leap second event!
}
if ( (t_e - secondOfLeapSecondEvent) > 21600)
{
Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800*(double)(WN - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
}
}
else // the effectivity time is in the past
{
/* 5.1.7c
* Whenever the leap second adjustment time, as indicated by the WN_LSF and DN values,
* is in the past (relative to the user’s current time) and the user’s present time does not
* fall in the time span which starts six hours prior to the leap second adjustment time and
* ends six hours after the adjustment time, the effective time is computed according to
* the following equation:
*/
Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
double secondsOfWeekBeforeToday = 43200 * floor(t_e / 43200);
t_Utc = secondsOfWeekBeforeToday + t_Utc_daytime;
return t_Utc;
}
//void Galileo_Navigation_Message::satellitePosition(double transmitTime) //when this function in used, the input must be the transmitted time (t) in second computed by Galileo_System_Time (above function)
//{
//
// double tk; // Time from ephemeris reference epoch
// //double t; // Galileo System Time (ICD, paragraph 5.1.2)
// double a; // Semi-major axis
// double n; // Corrected mean motion
// double n0; // Computed mean motion
// double M; // Mean anomaly
// double E; //Eccentric Anomaly (to be solved by iteration)
// double E_old;
// double dE;
// double nu; //True anomaly
// double phi; //argument of Latitude
// double u; // Correct argument of latitude
// double r; // Correct radius
// double i;
// double Omega;
//
// // Find Galileo satellite's position ----------------------------------------------
//
// // Restore semi-major axis
// a = A_1*A_1;
//
// // Computed mean motion
// n0 = sqrt(GALILEO_GM / (a*a*a));
//
// // Time from ephemeris reference epoch
// //tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
// //t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
// tk = transmitTime - t0e_1;
//
// // Corrected mean motion
// n = n0 + delta_n_3;
//
// // Mean anomaly
// M = M0_1 + n * tk;
//
// // Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
//
// // Initial guess of eccentric anomaly
// E = M;
//
// // --- Iteratively compute eccentric anomaly ----------------------------
// for (int ii = 1; ii<20; ii++)
// {
// E_old = E;
// E = M + e_1 * sin(E);
// dE = fmod(E - E_old, 2*GALILEO_PI);
// if (fabs(dE) < 1e-12)
// {
// //Necessary precision is reached, exit from the loop
// break;
// }
// }
//
// // Compute the true anomaly
//
// double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E);
// double tmp_X = cos(E) - e_1;
// nu = atan2(tmp_Y, tmp_X);
//
// // Compute angle phi (argument of Latitude)
// phi = nu + omega_2;
//
// // Reduce phi to between 0 and 2*pi rad
// phi = fmod((phi), (2*GALILEO_PI));
//
// // Correct argument of latitude
// u = phi + C_uc_3 * cos(2*phi) + C_us_3 * sin(2*phi);
//
// // Correct radius
// r = a * (1 - e_1*cos(E)) + C_rc_3 * cos(2*phi) + C_rs_3 * sin(2*phi);
//
// // Correct inclination
// i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2*phi) + C_is_4 * sin(2*phi);
//
// // Compute the angle between the ascending node and the Greenwich meridian
// Omega = OMEGA_0_2 + (OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT)*tk - GALILEO_OMEGA_EARTH_DOT * t0e_1;
//
// // Reduce to between 0 and 2*pi rad
// Omega = fmod((Omega + 2*GALILEO_PI), (2*GALILEO_PI));
//
// // --- Compute satellite coordinates in Earth-fixed coordinates
// galileo_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
// galileo_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); //***********************NOTE: in GALILEO ICD this expression is not correct because it has minus (- sin(u) * r * cos(i) * cos(Omega)) instead of plus
// galileo_satpos_Z = sin(u) * r * sin(i);
//
// std::cout << "Galileo satellite position X [m]: " << galileo_satpos_X << std::endl;
// std::cout << "Galileo satellite position Y [m]: " << galileo_satpos_Y << std::endl;
// std::cout << "Galileo satellite position Z [m]: " << galileo_satpos_Z << std::endl;
// double vector_position = sqrt(galileo_satpos_X*galileo_satpos_X + galileo_satpos_Y*galileo_satpos_Y + galileo_satpos_Z*galileo_satpos_Z);
// std::cout << "Vector Earth Center-Satellite [Km]: " << vector_position/1000 << std::endl;
//
// // Satellite's velocity. Can be useful for Vector Tracking loops
// double Omega_dot = OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT;
// galileo_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + galileo_satpos_X * cos(Omega) - galileo_satpos_Y * cos(i) * sin(Omega);
// galileo_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + galileo_satpos_X * sin(Omega) + galileo_satpos_Y * cos(i) * cos(Omega);
// galileo_satvel_Z = galileo_satpos_Y * sin(i);
//
//}
//
//
//double Galileo_Navigation_Message::Galileo_System_Time(double WN, double TOW){
// /* GALIELO SYSTEM TIME, ICD 5.1.2
// * input parameter:
// * WN: The Week Number is an integer counter that gives the sequential week number
// from the origin of the Galileo time. It covers 4096 weeks (about 78 years).
// Then the counter is reset to zero to cover additional period modulo 4096
//
// TOW: The Time of Week is defined as the number of seconds that have occurred since
// the transition from the previous week. The TOW covers an entire week from 0 to
// 604799 seconds and is reset to zero at the end of each week
//
// WN and TOW are received in page 5
//
// output:
// t: it is the transmitted time in Galileo System Time (expressed in seconds)
//
// The GST start epoch shall be 00:00 UT on Sunday 22nd August 1999 (midnight between 21st and 22nd August).
// At the start epoch, GST shall be ahead of UTC by thirteen (13)
// leap seconds. Since the next leap second was inserted at 01.01.2006, this implies that
// as of 01.01.2006 GST is ahead of UTC by fourteen (14) leap seconds.
//
// The epoch denoted in the navigation messages by TOW and WN
// will be measured relative to the leading edge of the first chip of the
// first code sequence of the first page symbol. The transmission timing of the navigation
// message provided through the TOW is synchronised to each satellite’s version of Galileo System Time (GST).
// *
// */
// double t=0;
// double sec_in_day = 86400;
// double day_in_week = 7;
// t = WN * sec_in_day * day_in_week + TOW; // second from the origin of the Galileo time
//
// return t;
//
//}
//
//
//
//double Galileo_Navigation_Message::sv_clock_drift(double transmitTime){
// /* Satellite Time Correction Algorithm, ICD 5.1.4
// *
// */
// double dt;
// dt = transmitTime - t0c_4;
// Galileo_satClkDrift = af0_4 + af1_4*dt + (af2_4 * dt)*(af2_4 * dt) + Galileo_dtr;
// return Galileo_satClkDrift;
//}
//
//// compute the relativistic correction term
//double Galileo_Navigation_Message::sv_clock_relativistic_term(double transmitTime) //Satellite Time Correction Algorithm, ICD 5.1.4
//{
// double tk;
// double a;
// double n;
// double n0;
// double E;
// double E_old;
// double dE;
// double M;
//
// // Restore semi-major axis
// a = A_1*A_1;
//
// n0 = sqrt(GALILEO_GM / (a*a*a));
//
// // Time from ephemeris reference epoch
// //tk = check_t(transmitTime - d_Toe); this is tk for GPS; for Galileo it is different
// //t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time
// tk = transmitTime - t0e_1;
//
// // Corrected mean motion
// n = n0 + delta_n_3;
//
// // Mean anomaly
// M = M0_1 + n * tk;
//
// // Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2* GALILEO_PI), (2* GALILEO_PI));
//
// // Initial guess of eccentric anomaly
// E = M;
//
// // --- Iteratively compute eccentric anomaly ----------------------------
// for (int ii = 1; ii<20; ii++)
// {
// E_old = E;
// E = M + e_1 * sin(E);
// dE = fmod(E - E_old, 2*GALILEO_PI);
// if (fabs(dE) < 1e-12)
// {
// //Necessary precision is reached, exit from the loop
// break;
// }
// }
//
//
// // Compute relativistic correction term
// Galileo_dtr = GALILEO_F * e_1* A_1 * sin(E);
// return Galileo_dtr;
//}
//double Galileo_Navigation_Message::GST_to_UTC_time(double t_e, int WN) //t_e is GST (WN+TOW) in second
//{
// double t_Utc;
// double t_Utc_daytime;
// double Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
//
// // Determine if the effectivity time of the leap second event is in the past
// int weeksToLeapSecondEvent = WN_LSF_6 - WN;
//
// if ((weeksToLeapSecondEvent) >= 0) // is not in the past
// {
// //Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
// int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60;
// if (weeksToLeapSecondEvent > 0)
// {
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
// }
// else //we are in the same week than the leap second event
// {
// if (abs(t_e - secondOfLeapSecondEvent) > 21600)
// {
// /* 5.1.7a
// * Whenever the leap second adjusted time indicated by the WN_LSF and the DN values
// * is not in the past (relative to the user's present time), and the user's
// * present time does not fall in the time span which starts at six hours prior
// * to the effective time and ends at six hours after the effective time,
// * the GST/Utc relationship is given by
// */
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
// }
// else
// {
// /* 5.1.7b
// * Whenever the user's current time falls within the time span of six hours
// * prior to the leap second adjustment to six hours after the adjustment time, ,
// * the effective time is computed according to the following equations:
// */
//
// int W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200;
// t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
// //implement something to handle a leap second event!
// }
// if ( (t_e - secondOfLeapSecondEvent) > 21600)
// {
// Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800*(double)(WN - WNot_6));
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
// }
// }
// }
// else // the effectivity time is in the past
// {
// /* 5.1.7c
// * Whenever the leap second adjustment time, as indicated by the WN_LSF and DN values,
// * is in the past (relative to the user’s current time) and the user’s present time does not
// * fall in the time span which starts six hours prior to the leap second adjustment time and
// * ends six hours after the adjustment time, the effective time is computed according to
// * the following equation:
// */
// Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
// t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
// }
//
// double secondsOfWeekBeforeToday = 43200 * floor(t_e / 43200);
// t_Utc = secondsOfWeekBeforeToday + t_Utc_daytime;
// return t_Utc;
//
//}
//
//
//

18
src/core/system_parameters/galileo_navigation_message.h
View File

@@ -296,15 +296,15 @@ public:
*/
Galileo_Almanac get_almanac();
void satellitePosition(double transmitTime);
double Galileo_System_Time(double WN, double TOW); // Galileo System Time (GST), ICD paragraph 5.1.2
double sv_clock_drift(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4
double sv_clock_relativistic_term(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4
double GST_to_UTC_time(double t_e, int WN); //GST-UTC Conversion Algorithm and Parameters
// void satellitePosition(double transmitTime);
//
// double Galileo_System_Time(double WN, double TOW); // Galileo System Time (GST), ICD paragraph 5.1.2
//
// double sv_clock_drift(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4
//
// double sv_clock_relativistic_term(double transmitTime); //Satellite Time Correction Algorithm, ICD 5.1.4
//
// double GST_to_UTC_time(double t_e, int WN); //GST-UTC Conversion Algorithm and Parameters
Galileo_Navigation_Message();
};

97
src/core/system_parameters/galileo_utc_model.cc
View File

@@ -49,63 +49,54 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int WN)
{
double t_Utc;
double t_Utc_daytime;
double Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
double Delta_t_Utc=0;
// Determine if the effectivity time of the leap second event is in the past
int weeksToLeapSecondEvent = WN_LSF_6 - WN;
int weeksToLeapSecondEvent = WN_LSF_6 - (WN % 256);
if ((weeksToLeapSecondEvent) >= 0) // is not in the past
{
//Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60;
if (weeksToLeapSecondEvent > 0)
{
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
else //we are in the same week than the leap second event
{
if (abs(t_e - secondOfLeapSecondEvent) > 21600)
{
/* 5.1.7a
* Whenever the leap second adjusted time indicated by the WN_LSF and the DN values
* is not in the past (relative to the user's present time), and the user's
* present time does not fall in the time span which starts at six hours prior
* to the effective time and ends at six hours after the effective time,
* the GST/Utc relationship is given by
*/
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
else
{
/* 5.1.7b
* Whenever the user's current time falls within the time span of six hours
* prior to the leap second adjustment to six hours after the adjustment time, ,
* the effective time is computed according to the following equations:
*/
int W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200;
t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
//implement something to handle a leap second event!
}
if ( (t_e - secondOfLeapSecondEvent) > 21600)
{
Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800*(double)(WN - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
}
}
{
//Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s
int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60;
if (abs(t_e - secondOfLeapSecondEvent) > 21600)
{
/* 5.1.7a
* Whenever the leap second adjusted time indicated by the WN_LSF and the DN values
* is not in the past (relative to the user's present time), and the user's
* present time does not fall in the time span which starts at six hours prior
* to the effective time and ends at six hours after the effective time,
* the GST/Utc relationship is given by
*/
//std::cout<<"GST->UTC case a"<<std::endl;
Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)((WN % 256) - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
else
{
/* 5.1.7b
* Whenever the user's current time falls within the time span of six hours
* prior to the leap second adjustment to six hours after the adjustment time, ,
* the effective time is computed according to the following equations:
*/
//std::cout<<"GST->UTC case b"<<std::endl;
Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)((WN % 256) - WNot_6));
double W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200;
t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
//implement something to handle a leap second event!
}
}
else // the effectivity time is in the past
{
/* 5.1.7c
* Whenever the leap second adjustment time, as indicated by the WN_LSF and DN values,
* is in the past (relative to the user’s current time) and the user’s present time does not
* fall in the time span which starts six hours prior to the leap second adjustment time and
* ends six hours after the adjustment time, the effective time is computed according to
* the following equation:
*/
Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)(WN - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
{
/* 5.1.7c
* Whenever the leap second adjustment time, as indicated by the WN_LSF and DN values,
* is in the past (relative to the user’s current time) and the user’s present time does not
* fall in the time span which starts six hours prior to the leap second adjustment time and
* ends six hours after the adjustment time, the effective time is computed according to
* the following equation:
*/
//std::cout<<"GST->UTC case c"<<std::endl;
Delta_t_Utc = Delta_tLSF_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * (double)((WN % 256) - WNot_6));
t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400);
}
double secondsOfWeekBeforeToday = 43200 * floor(t_e / 43200);
t_Utc = secondsOfWeekBeforeToday + t_Utc_daytime;

6
src/utils/matlab/galileo_e1_dll_pll_veml_plot_sample_32bits.m
View File

@@ -29,10 +29,10 @@
% */
close all;
clear all;
samplingFreq = 64e6/8; %[Hz]
channels=2;
samplingFreq = 20480000/4; %[Hz]
channels=8;
%path='/home/javier/workspace/gnss-sdr/trunk/install/';
path='/home/javier/workspace/gnss-sdr/trunk/data/';
path='/home/gnss/workspace/gnss-sdr/trunk/data/';
clear PRN_absolute_sample_start;
for N=1:1:channels
tracking_log_path=[path 'veml_tracking_ch_' num2str(N-1) '.dat'];

11
src/utils/matlab/galileo_e1b_observables_plot_sample.m
View File

@@ -6,12 +6,17 @@ close all;
%IFEN NSR Sampler Fs=20480000
% GNSS-SDR decimation factor 8
samplingFreq = 20480000/8; %[Hz]
channels=8;
channels=4;
path='/home/gnss/workspace/gnss-sdr/trunk/install/';
observables_log_path=[path 'observables.dat'];
GNSS_observables= gps_l1_ca_read_observables_dump(channels,observables_log_path);
skip=10000;
skip=9000;
ref_channel=1;
plot(GNSS_observables.d_TOW_at_current_symbol(ref_channel,skip:end),GNSS_observables.Pseudorange_m(1:6,skip:end).')
plot(GNSS_observables.d_TOW_at_current_symbol(ref_channel,skip:end),GNSS_observables.Pseudorange_m(:,skip:end).')
title('psudoranges');
figure
plot(GNSS_observables.d_TOW_at_current_symbol(ref_channel,skip:end),GNSS_observables.Prn_timestamp_ms(:,skip:end).')
title('Prn_timestamps');