mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-05 15:00:33 +00:00
Code Issues Releases Wiki Activity

- Major changes:

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

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

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

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

2
README
View File

@ -66,7 +66,7 @@ In order to do profiling, you'll have to install google-perftools library.
- PERFTOOLS$ sudo make install
Once google-perftools is installed, you can use the script "profiler" which is placed
in the root folder of MERCURIO sources. The script must be run as root since it makes use
in the root folder of GNSS-SDR sources. The script must be run as root since it makes use
of "nice". The result of the profiling are two files, mercurio.cpu.prof and mercurio.head.prof.0001.heap,
that contain the results for CPU and HEAP profiling. You can use google-perftools' script pprof
to analyze the recorded data.

222
conf/gnss-sdr.conf Normal file
View File

@ -0,0 +1,222 @@
; MASTER configuration file
; This file should be updated with the latest changes in the configuration parameters, as a sample configuration file.
; use RELATIVE file path in this configuration file
; Sample for a configuration file for GNSS-SDR
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz].
GNSS-SDR.internal_fs_hz=4000000
;######### CONTROL_THREAD CONFIG ############
ControlThread.wait_for_flowgraph=false
;######### SIGNAL_SOURCE CONFIG ############
;#implementation: Use only File_Signal_Source in this version
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=/media/DATALOGGER/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=gr_complex
;#sampling_frequency: Original Signal sampling frequency in [Hz]
SignalSource.sampling_frequency=4000000
;#samples: Number of samples to be processed. Notice that 0 indicates the entire file.
SignalSource.samples=0
;#repeat: Repeat the processing file. Disable this option in this version
SignalSource.repeat=false
;#dump: Dump the Signal source data to a file. Disable this option in this version
SignalSource.dump=false
;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing.
; it helps to not overload the CPU, but the processing time will be longer.
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
;## Enables and configures a real-time resampler. Please disable it in this version.
;implementation: Pass_Through disables this block
SignalConditioner.implementation=Pass_Through
SignalConditioner.item_type=gr_complex
SignalConditioner.sample_freq_in=4000000
SignalConditioner.sample_freq_out=4000000
SignalConditioner.dump=false
;######### CHANNELS CONFIGURATION CONFIG ############
;#count: Number of avalable satellite channels.
Channels.count=6
;######### ACQUISITION GLOBAL CONFIG ############
;#dump: Enable or disable the acquisition internal data file logging [true] or [false]
Acquisition.dump=false
;#filename: Log path and filename
Acquisition.dump_filename=./acq_dump.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
Acquisition.item_type=gr_complex
;#if: Signal intermediate frequency in [Hz]
Acquisition.if=0
;#sampled_ms: Signal block duration for the acquisition signal detection [ms]
Acquisition.sampled_ms=1
;######### ACQUISITION CHANNELS CONFIG ######
;######### ACQUISITION CH 0 CONFIG ############
;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition]
Acquisition0.implementation=GPS_L1_CA_PCPS_Acquisition
;#threshold: Acquisition threshold
Acquisition0.threshold=70
;#doppler_max: Maximum expected Doppler shift [Hz]
Acquisition0.doppler_max=10000
;#doppler_max: Doppler step in the grid search [Hz]
Acquisition0.doppler_step=250
;#satellite: Satellite PRN ID for this channel. Disable this option to random search
;Acquisition0.satellite=14
;#repeat_satellite: Use only jointly with the satellte PRN ID option.
;#Enable repeat_satellite to keep searching the same satellite during the runtime.
;Acquisition0.repeat_satellite=true
;######### ACQUISITION CH 1 CONFIG ############
Acquisition1.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition1.threshold=70
Acquisition1.doppler_max=10000
Acquisition1.doppler_step=250
;Acquisition1.satellite=32
;Acquisition1.repeat_satellite=true
;######### ACQUISITION CH 2 CONFIG ############
Acquisition2.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition2.threshold=70
Acquisition2.doppler_max=10000
Acquisition2.doppler_step=250
;Acquisition2.satellite=11
;Acquisition2.repeat_satellite=true
;######### ACQUISITION CH 3 CONFIG ############
Acquisition3.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition3.threshold=70
Acquisition3.doppler_max=10000
Acquisition3.doppler_step=250
;Acquisition3.satellite=1
;Acquisition3.repeat_satellite=true
;######### ACQUISITION CH 4 CONFIG ############
Acquisition4.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition4.threshold=70
Acquisition4.doppler_max=10000
Acquisition4.doppler_step=250
;Acquisition4.satellite=31
;Acquisition4.repeat_satellite=true
;######### ACQUISITION CH 5 CONFIG ############
Acquisition5.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition5.threshold=70
Acquisition5.doppler_max=10000
Acquisition5.doppler_step=250
;Acquisition5.satellite=20
;Acquisition5.repeat_satellite=true
;######### ACQUISITION CH 6 CONFIG ############
Acquisition6.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition6.threshold=70
Acquisition6.doppler_max=10000
Acquisition6.doppler_step=250
;Acquisition6.satellite=22
;Acquisition6.repeat_satellite=true
;######### ACQUISITION CH 7 CONFIG ############
Acquisition7.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition7.threshold=70
Acquisition7.doppler_max=10000
Acquisition7.doppler_step=250
;Acquisition7.satellite=23
;Acquisition7.repeat_satellite=true
;######### TRACKING GLOBAL CONFIG ############
;#implementatiion: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking]
Tracking.implementation=GPS_L1_CA_DLL_FLL_PLL_Tracking
;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version.
Tracking.item_type=gr_complex
;#sampling_frequency: Signal Intermediate Frequency in [Hz]
Tracking.if=0
;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false]
Tracking.dump=true
;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number.
Tracking.dump_filename=./tracking_ch_
;#pll_bw_hz: PLL loop filter bandwidth [Hz]
Tracking.pll_bw_hz=50.0;
;#dll_bw_hz: DLL loop filter bandwidth [Hz]
Tracking.dll_bw_hz=2.0;
;#fll_bw_hz: FLL loop filter bandwidth [Hz]
Tracking.fll_bw_hz=20.0;
;#order: PLL/DLL loop filter order [2] or [3]
Tracking.order=2;
;#early_late_space_chips: correlator early-late space [chips]. Use [0.5]
Tracking.early_late_space_chips=0.5;
;######### TELEMETRY DECODER CONFIG ############
;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A.
TelemetryDecoder.implementation=GPS_L1_CA_Telemetry_Decoder
;#fs_in: Signal sampling frequency in [Hz]
TelemetryDecoder.fs_in=4000000
;######### OBSERVABLES CONFIG ############
;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A.
Observables.implementation=GPS_L1_CA_Observables
;#output_rate_ms: Period between two psudoranges outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms]
Observables.output_rate_ms=100
;#dump: Enable or disable the Observables internal binary data file logging [true] or [false]
Observables.dump=false
;#dump_filename: Log path and filename.
Observables.dump_filename=./observables.dat
;######### PVT CONFIG ############
;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version.
PVT.implementation=GPS_L1_CA_PVT
;#averaging_depth: Number of PVT observations in the moving average algorithm
PVT.averaging_depth=2
;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false]
PVT.flag_averaging=true
;#dump: Enable or disable the PVT internal binary data file logging [true] or [false]
PVT.dump=true
;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump.
PVT.dump_filename=./PVT
;######### OUTPUT_FILTER CONFIG ############
;# Receiver output filter: Leave this block disabled in this version
OutputFilter.implementation=Null_Sink_Output_Filter
OutputFilter.filename=data/gnss-sdr.dat
OutputFilter.item_type=gr_complex

223
conf/master.conf Normal file
View File

@ -0,0 +1,223 @@
; MASTER configuration file
; This file should be updated with the latest changes in the configuration parameters, as a sample configuration file.
; use RELATIVE file path in this configuration file
; Sample for a configuration file for GNSS-SDR
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz].
GNSS-SDR.internal_fs_hz=4000000
;######### CONTROL_THREAD CONFIG ############
ControlThread.wait_for_flowgraph=false
;######### SIGNAL_SOURCE CONFIG ############
;#implementation: Use only File_Signal_Source in this version
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=../data/agilent_cap2.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=gr_complex
;#sampling_frequency: Original Signal sampling frequency in [Hz]
SignalSource.sampling_frequency=4000000
;#samples: Number of samples to be processed. Notice that 0 indicates the entire file.
SignalSource.samples=0
;#repeat: Repeat the processing file. Disable this option in this version
SignalSource.repeat=false
;#dump: Dump the Signal source data to a file. Disable this option in this version
SignalSource.dump=false
;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing.
; it helps to not overload the CPU, but the processing time will be longer.
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
;## Enables and configures a real-time resampler. Please disable it in this version.
;implementation: Pass_Through disables this block
SignalConditioner.implementation=Pass_Through
SignalConditioner.item_type=gr_complex
SignalConditioner.sample_freq_in=4000000
SignalConditioner.sample_freq_out=4000000
SignalConditioner.dump=false
;######### CHANNELS CONFIGURATION CONFIG ############
;#count: Number of avalable satellite channels.
Channels.count=4
;######### ACQUISITION GLOBAL CONFIG ############
;#dump: Enable or disable the acquisition internal data file logging [true] or [false]
Acquisition.dump=false
;#filename: Log path and filename
Acquisition.dump_filename=./acq_dump.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
Acquisition.item_type=gr_complex
;#if: Signal intermediate frequency in [Hz]
Acquisition.if=0
;#sampled_ms: Signal block duration for the acquisition signal detection [ms]
Acquisition.sampled_ms=1
;######### ACQUISITION CHANNELS CONFIG ######
;######### ACQUISITION CH 0 CONFIG ############
;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition]
Acquisition0.implementation=GPS_L1_CA_PCPS_Acquisition
;#threshold: Acquisition threshold
Acquisition0.threshold=70
;#doppler_max: Maximum expected Doppler shift [Hz]
Acquisition0.doppler_max=10000
;#doppler_max: Doppler step in the grid search [Hz]
Acquisition0.doppler_step=250
;#satellite: Satellite PRN ID for this channel. Disable this option to random search
;Acquisition0.satellite=14
;#repeat_satellite: Use only jointly with the satellte PRN ID option.
;#Enable repeat_satellite to keep searching the same satellite during the runtime.
;Acquisition0.repeat_satellite=true
;######### ACQUISITION CH 1 CONFIG ############
Acquisition1.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition1.threshold=70
Acquisition1.doppler_max=10000
Acquisition1.doppler_step=250
;Acquisition1.satellite=32
;Acquisition1.repeat_satellite=true
;######### ACQUISITION CH 2 CONFIG ############
Acquisition2.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition2.threshold=70
Acquisition2.doppler_max=10000
Acquisition2.doppler_step=250
;Acquisition2.satellite=11
;Acquisition2.repeat_satellite=true
;######### ACQUISITION CH 3 CONFIG ############
Acquisition3.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition3.threshold=70
Acquisition3.doppler_max=10000
Acquisition3.doppler_step=250
;Acquisition3.satellite=1
;Acquisition3.repeat_satellite=true
;######### ACQUISITION CH 4 CONFIG ############
Acquisition4.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition4.threshold=70
Acquisition4.doppler_max=10000
Acquisition4.doppler_step=250
;Acquisition4.satellite=31
;Acquisition4.repeat_satellite=true
;######### ACQUISITION CH 5 CONFIG ############
Acquisition5.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition5.threshold=70
Acquisition5.doppler_max=10000
Acquisition5.doppler_step=250
;Acquisition5.satellite=20
;Acquisition5.repeat_satellite=true
;######### ACQUISITION CH 6 CONFIG ############
Acquisition6.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition6.threshold=70
Acquisition6.doppler_max=10000
Acquisition6.doppler_step=250
;Acquisition6.satellite=22
;Acquisition6.repeat_satellite=true
;######### ACQUISITION CH 7 CONFIG ############
Acquisition7.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition7.threshold=70
Acquisition7.doppler_max=10000
Acquisition7.doppler_step=250
;Acquisition7.satellite=23
;Acquisition7.repeat_satellite=true
;######### TRACKING GLOBAL CONFIG ############
;#implementatiion: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_FLL_PLL_Tracking]
Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version.
Tracking.item_type=gr_complex
;#sampling_frequency: Signal Intermediate Frequency in [Hz]
Tracking.if=0
;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false]
Tracking.dump=false
;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number.
Tracking.dump_filename=./tracking_ch_
;#pll_bw_hz: PLL loop filter bandwidth [Hz]
Tracking.pll_bw_hz=50.0;
;#dll_bw_hz: DLL loop filter bandwidth [Hz]
Tracking.dll_bw_hz=2.0;
;#fll_bw_hz: FLL loop filter bandwidth [Hz]
Tracking.fll_bw_hz=20.0;
;#order: PLL/DLL loop filter order [2] or [3]
Tracking.order=2;
;#early_late_space_chips: correlator early-late space [chips]. Use [0.5]
Tracking.early_late_space_chips=0.5;
;######### TELEMETRY DECODER CONFIG ############
;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A.
TelemetryDecoder.implementation=GPS_L1_CA_Telemetry_Decoder
;#fs_in: Signal sampling frequency in [Hz]
TelemetryDecoder.fs_in=4000000
;######### OBSERVABLES CONFIG ############
;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A.
Observables.implementation=GPS_L1_CA_Observables
;#output_rate_ms: Period between two psudoranges outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms]
Observables.output_rate_ms=100
;#dump: Enable or disable the Observables internal binary data file logging [true] or [false]
Observables.dump=false
;#dump_filename: Log path and filename.
Observables.dump_filename=./observables.dat
;######### PVT CONFIG ############
;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version.
PVT.implementation=GPS_L1_CA_PVT
;#averaging_depth: Number of PVT observations in the moving average algorithm
PVT.averaging_depth=2
;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false]
PVT.flag_averaging=true
;#dump: Enable or disable the PVT internal binary data file logging [true] or [false]
PVT.dump=true
;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump.
PVT.dump_filename=./PVT
;######### OUTPUT_FILTER CONFIG ############
;# Receiver output filter: Leave this block disabled in this version
OutputFilter.implementation=Null_Sink_Output_Filter
OutputFilter.filename=data/gnss-sdr.dat
OutputFilter.item_type=gr_complex

101
conf/mercurio.conf
View File

@ -1,101 +0,0 @@
; Sample for a configuration file for MERCURIO
[mercurio]
;######### CONTROL_THREAD CONFIG ############
ControlThread.wait_for_flowgraph=false
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=File_Signal_Source
;SignalSource.filename=/media/DATALOGGER/signals/spirent scenario 2/data/sc2_d8.dat
;SignalSource.filename=/media/My Passport/KINGSTON (G)/Project Luis/GPSL1_Fs_8MHz_ID_1_CN0_60.dat
SignalSource.filename=/media/DATALOGGER/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
;SignalSource.filename=/home/luis/Project/signals/GPS_L1_8sats_4_Msps_CN0_52.dat
SignalSource.item_type=gr_complex
SignalSource.sampling_frequency=4000000
SignalSource.samples=292000000
SignalSource.repeat=false
SignalSource.dump=false
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Pass_Through
SignalConditioner.item_type=gr_complex
SignalConditioner.sample_freq_in=4000000
SignalConditioner.sample_freq_out=4000000
SignalConditioner.dump=false
;######### CHANNELS CONFIGURATION CONFIG ############
Channels.count=1
;######### ACQUISITION CONFIG ############
Acquisition.dump=false
Acquisition.dump_filename=./acq_dump.dat
Acquisition.item_type=gr_complex
Acquisition.fs_in=4000000
Acquisition.if=0
Acquisition.sampled_ms=1
;######### ACQUISITION 0 CONFIG ############
Acquisition0.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition0.threshold=440
Acquisition0.doppler_max=10000
Acquisition0.doppler_step=500
Acquisition0.satellite=14
Acquisition0.repeat_satellite=true
;######### ACQUISITION 1 CONFIG ############
Acquisition1.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition1.threshold=440
Acquisition1.doppler_max=10000
Acquisition1.doppler_step=500
Acquisition1.satellite=1
Acquisition1.repeat_satellite=true
;######### ACQUISITION 2 CONFIG ############
Acquisition2.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition2.threshold=440
Acquisition2.doppler_max=10000
Acquisition2.doppler_step=500
Acquisition2.satellite=1
Acquisition2.repeat_satellite=true
;######### ACQUISITION 3 CONFIG ############
Acquisition3.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition3.threshold=440
Acquisition3.doppler_max=10000
Acquisition3.doppler_step=500
Acquisition3.satellite=1
Acquisition3.repeat_satellite=true
;######### TRACKING CONFIG ############
;Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
Tracking.implementation=GPS_L1_CA_DLL_FLL_PLL_Tracking
Tracking.item_type=gr_complex
Tracking.vector_length=4000
Tracking.fs_in=4000000
Tracking.if=0
Tracking.dump=true
Tracking.pll_bw_hz=50.0;
Tracking.dll_bw_hz=2.0;
Tracking.fll_bw_hz=50;
Tracking.order=2;
Tracking.early_late_space_chips=0.5;
;######### TELEMETRY DECODER CONFIG ############
TelemetryDecoder.implementation=GPS_L1_CA_Telemetry_Decoder
TelemetryDecoder.item_type=gr_complex
;######### OBSERVABLES CONFIG ############
Observables.implementation=GPS_L1_CA_Observables
Observables.fs_in=4000000;
;######### PVT CONFIG ############
PVT.implementation=Pass_Through
PVT.item_type=gr_complex
;######### OUTPUT_FILTER CONFIG ############
OutputFilter.implementation=Null_Sink_Output_Filter
OutputFilter.filename=data/mercurio.dat
OutputFilter.item_type=gr_complex

2
jamroot.jam
View File

@ -29,11 +29,11 @@ project : requirements
<include>src/algorithms/libs
<include>src/algorithms/observables/adapters
<include>src/algorithms/observables/gnuradio_blocks
<include>src/algorithms/observables/libs
<include>src/algorithms/output_filter/adapters
<include>src/algorithms/output_filter/gnuradio_blocks
<include>src/algorithms/PVT/adapters
<include>src/algorithms/PVT/gnuradio_blocks
<include>src/algorithms/PVT/libs
<include>src/algorithms/signal_source/adapters
<include>src/algorithms/signal_source/gnuradio_blocks
<include>src/algorithms/telemetry_decoder/adapters

103
src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc Normal file
View File

@ -0,0 +1,103 @@
/*!
* \file gps_l1_ca_pvt.cc
* \brief Simple Least Squares implementation for GPS L1 C/A Position Velocity and Time
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_pvt.h"
#include "configuration_interface.h"
#include "gps_l1_ca_pvt_cc.h"
#include <gnuradio/gr_io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
extern concurrent_queue<gps_navigation_message> global_gps_nav_msg_queue;
using google::LogMessage;
GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
gr_msg_queue_sptr queue) :
role_(role),
in_streams_(in_streams),
out_streams_(out_streams),
queue_(queue)
{
std::string default_dump_filename = "./pvt.dat";
DLOG(INFO) << "role " << role;
int averaging_depth;
averaging_depth=configuration->property(role + ".averaging_depth", 10);
bool flag_averaging;
flag_averaging=configuration->property(role + ".flag_averaging", false);
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
pvt_ = gps_l1_ca_make_pvt_cc(in_streams_, queue_, dump_, dump_filename_, averaging_depth, flag_averaging);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
// set the navigation msg queue;
pvt_->set_navigation_queue(&global_gps_nav_msg_queue);
DLOG(INFO) << "global navigation message queue assigned to pvt ("<< pvt_->unique_id() << ")";
}
GpsL1CaPvt::~GpsL1CaPvt()
{}
void GpsL1CaPvt::connect(gr_top_block_sptr top_block)
{
// Nothing to connect internally
DLOG(INFO) << "nothing to connect internally";
}
void GpsL1CaPvt::disconnect(gr_top_block_sptr top_block)
{
// Nothing to disconnect
}
gr_basic_block_sptr GpsL1CaPvt::get_left_block()
{
return pvt_;
}
gr_basic_block_sptr GpsL1CaPvt::get_right_block()
{
return pvt_;
}

94
src/algorithms/PVT/adapters/gps_l1_ca_pvt.h Normal file
View File

@ -0,0 +1,94 @@
/*!
* \file gps_l1_ca_pvt.h
* \brief Simple Least Squares implementation for GPS L1 C/A Position Velocity and Time
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GPS_L1_CA_PVT_H_
#define GPS_L1_CA_PVT_H_
#include "pvt_interface.h"
#include "gps_l1_ca_pvt_cc.h"
#include <gnuradio/gr_msg_queue.h>
class ConfigurationInterface;
class GpsL1CaPvt : public PvtInterface
{
public:
GpsL1CaPvt(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
gr_msg_queue_sptr queue);
virtual ~GpsL1CaPvt();
std::string role()
{
return role_;
}
std::string implementation()
{
return "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 intem_size() function implementation
size_t item_size()
{
return sizeof(gr_complex);
}
private:
gps_l1_ca_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_;
gr_msg_queue_sptr queue_;
};
#endif

3
src/algorithms/PVT/adapters/jamfile.jam Normal file
View File

@ -0,0 +1,3 @@
project : build-dir ../../../../build ;
obj gps_l1_ca_pvt : gps_l1_ca_pvt.cc ;

167
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc Normal file
View File

@ -0,0 +1,167 @@
/*!
* \file gps_l1_ca_pvt_cc.cc
* \brief Position Velocity and Time computation for GPS L1 C/A using Least Squares algorithm
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <iostream>
#include <sstream>
#include <vector>
#include <map>
#include <algorithm>
#include <bitset>
#include <cmath>
#include "math.h"
#include "gps_l1_ca_pvt_cc.h"
#include "control_message_factory.h"
#include <gnuradio/gr_io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
using google::LogMessage;
using namespace std;
gps_l1_ca_pvt_cc_sptr
gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging) {
return gps_l1_ca_pvt_cc_sptr(new gps_l1_ca_pvt_cc(nchannels, queue, dump, dump_filename, averaging_depth, flag_averaging));
}
gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging) :
gr_block ("gps_l1_ca_pvt_cc", gr_make_io_signature (nchannels, nchannels, sizeof(gnss_pseudorange)),
gr_make_io_signature(1, 1, sizeof(gr_complex))) {
// initialize internal vars
d_queue = queue;
d_dump = dump;
d_nchannels = nchannels;
d_dump_filename=dump_filename;
std::string kml_dump_filename;
kml_dump_filename=d_dump_filename;
kml_dump_filename.append(".kml");
d_kml_dump.set_headers(kml_dump_filename);
d_dump_filename.append(".dat");
d_averaging_depth=averaging_depth;
d_flag_averaging=flag_averaging;
/*!
* \todo Enable RINEX printer: The actual RINEX printer need a complete refactoring and some bug fixing work
*/
//d_rinex_printer.set_headers("GNSS-SDR");
d_ls_pvt=new gps_l1_ca_ls_pvt(nchannels,d_dump_filename,d_dump);
d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_ephemeris_clock_s=0.0;
d_sample_counter=0;
}
gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc() {
d_kml_dump.close_file();
delete d_ls_pvt;
}
bool pseudoranges_pairCompare_min( pair<int,gnss_pseudorange> a, pair<int,gnss_pseudorange> b)
{
return (a.second.pseudorange_m) < (b.second.pseudorange_m);
}
int gps_l1_ca_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++;
map<int,gnss_pseudorange> gnss_pseudoranges_map;
map<int,gnss_pseudorange>::iterator gnss_pseudoranges_iter;
gnss_pseudorange **in = (gnss_pseudorange **) &input_items[0]; //Get the input pointer
for (unsigned int i=0;i<d_nchannels;i++)
{
if (in[i][0].valid==true)
{
gnss_pseudoranges_map.insert(pair<int,gnss_pseudorange>(in[i][0].SV_ID,in[i][0])); //record the valid psudorrange in a map
}
}
//debug print
cout << setprecision(16);
for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin();
gnss_pseudoranges_iter != gnss_pseudoranges_map.end();
gnss_pseudoranges_iter++)
{
cout<<"Pseudoranges(SV ID,pseudorange [m]) =("<<gnss_pseudoranges_iter->first<<","<<gnss_pseudoranges_iter->second.pseudorange_m<<")"<<endl;
}
// ############ 1. READ EPHEMERIS FROM QUEUE ######################
// find the minimum index (nearest satellite, will be the reference)
gnss_pseudoranges_iter=min_element(gnss_pseudoranges_map.begin(),gnss_pseudoranges_map.end(),pseudoranges_pairCompare_min);
gps_navigation_message nav_msg;
while (d_nav_queue->try_pop(nav_msg)==true)
{
cout<<"New ephemeris record has arrived from SAT ID "<<nav_msg.d_satellite_PRN<<endl;
d_last_nav_msg=nav_msg;
d_ls_pvt->d_ephemeris[nav_msg.d_channel_ID]=nav_msg;
// **** update pseudoranges clock ****
if (nav_msg.d_satellite_PRN==gnss_pseudoranges_iter->second.SV_ID)
{
d_ephemeris_clock_s=d_last_nav_msg.d_TOW;
d_ephemeris_timestamp_ms=d_last_nav_msg.d_subframe1_timestamp_ms;
}
// **** write ephemeris to RINES NAV file
//d_rinex_printer.LogRinex2Nav(nav_msg);
}
// ############ 2. COMPUTE THE PVT ################################
// write the pseudoranges to RINEX OBS file
// 1- need a valid clock
if (d_ephemeris_clock_s>0 and d_last_nav_msg.d_satellite_PRN>0)
{
//d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,pseudoranges);
// compute on the fly PVT solution
//std::cout<<"diff_clock_ephemerids="<<(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0<<"\r\n";
if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,
d_ephemeris_clock_s+(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,
d_flag_averaging)==true)
{
d_kml_dump.print_position(d_ls_pvt,d_flag_averaging);
}
}
consume_each(1); //one by one
return 0;
}

101
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h Normal file
View File

@ -0,0 +1,101 @@
/*!
* \file gps_l1_ca_pvt_cc.h
* \brief Position Velocity and Time computation for GPS L1 C/A using Least Squares algorithm
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GPS_L1_CA_PVT_CC_H
#define GPS_L1_CA_PVT_CC_H
#include <fstream>
#include <gnuradio/gr_block.h>
#include <gnuradio/gr_msg_queue.h>
#include <queue>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include "concurrent_queue.h"
#include "gps_navigation_message.h"
#include "kml_printer.h"
#include "rinex_2_1_printer.h"
#include "gps_l1_ca_ls_pvt.h"
#include "GPS_L1_CA.h"
class gps_l1_ca_pvt_cc;
typedef boost::shared_ptr<gps_l1_ca_pvt_cc> gps_l1_ca_pvt_cc_sptr;
gps_l1_ca_pvt_cc_sptr
gps_l1_ca_make_pvt_cc(unsigned int n_channels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging);
class gps_l1_ca_pvt_cc : public gr_block {
private:
friend gps_l1_ca_pvt_cc_sptr
gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging);
gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging);
// class private vars
gr_msg_queue_sptr d_queue;
bool d_dump;
unsigned int d_nchannels;
std::string d_dump_filename;
std::ofstream d_dump_file;
int d_averaging_depth;
bool d_flag_averaging;
long unsigned int d_sample_counter;
kml_printer d_kml_dump;
concurrent_queue<gps_navigation_message> *d_nav_queue; // Navigation ephemeris queue
gps_navigation_message d_last_nav_msg; //last navigation message
double d_ephemeris_clock_s;
double d_ephemeris_timestamp_ms;
gps_l1_ca_ls_pvt *d_ls_pvt;
//rinex_printer d_rinex_printer; // RINEX printer class
public:
~gps_l1_ca_pvt_cc ();
void set_navigation_queue(concurrent_queue<gps_navigation_message> *nav_queue){d_nav_queue=nav_queue;}
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
};
#endif

3
src/algorithms/PVT/gnuradio_blocks/jamfile.jam Normal file
View File

@ -0,0 +1,3 @@
project : build-dir ../../../../build ;
obj gps_l1_ca_pvt_cc : gps_l1_ca_pvt_cc.cc ;

3
src/algorithms/PVT/jamfile.jam Normal file
View File

@ -0,0 +1,3 @@
build-project adapters ;
build-project gnuradio_blocks ;
build-project libs ;

144
src/algorithms/observables/libs/gps_l1_ca_ls_pvt.cc → src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@ -46,16 +46,43 @@
#include "gps_l1_ca_ls_pvt.h"
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels)
gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file)
{
// init empty ephemerids for all the available GNSS channels
d_nchannels=nchannels;
d_ephemeris=new gps_navigation_message[nchannels];
d_dump_filename=dump_filename;
d_flag_dump_enabled=flag_dump_to_file;
d_averaging_depth=0;
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"PVT lib dump enabled Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "Exception opening PVT lib dump file "<<e.what()<<"\r\n";
}
}
}
}
void gps_l1_ca_ls_pvt::set_averaging_depth(int depth)
{
d_averaging_depth=depth;
}
gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt()
{
delete d_ephemeris;
d_dump_file.close();
delete[] d_ephemeris;
}
arma::vec gps_l1_ca_ls_pvt::e_r_corr(double traveltime, arma::vec X_sat) {
@ -205,9 +232,9 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
return pos;
}
void gps_l1_ca_ls_pvt::get_PVT(std::map<int,float> pseudoranges,double GPS_current_time)
bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,gnss_pseudorange> gnss_pseudoranges_map,double GPS_current_time,bool flag_averaging)
{
std::map<int,float>::iterator pseudoranges_iter;
std::map<int,gnss_pseudorange>::iterator gnss_pseudoranges_iter;
//ITPP
//mat W=eye(d_nchannels); //channels weights matrix
//vec obs=zeros(d_nchannels); // pseudoranges observation vector
@ -223,10 +250,13 @@ void gps_l1_ca_ls_pvt::get_PVT(std::map<int,float> pseudoranges,double GPS_curre
{
if (d_ephemeris[i].satellite_validation()==true)
{
pseudoranges_iter=pseudoranges.find(d_ephemeris[i].d_satellite_PRN);
if (pseudoranges_iter!=pseudoranges.end())
gnss_pseudoranges_iter=gnss_pseudoranges_map.find(d_ephemeris[i].d_satellite_PRN);
if (gnss_pseudoranges_iter!=gnss_pseudoranges_map.end())
{
W(i,i)=1; // TODO: Place here the satellite CN0 (power level, or weight factor)
/*!
* \todo Place here the satellite CN0 (power level, or weight factor)
*/
W(i,i)=1;
// compute the GPS master clock
d_ephemeris[i].master_clock(GPS_current_time);
// compute the satellite current ECEF position
@ -236,7 +266,9 @@ void gps_l1_ca_ls_pvt::get_PVT(std::map<int,float> pseudoranges,double GPS_curre
satpos(0,i)=d_ephemeris[i].d_satpos_X;
satpos(1,i)=d_ephemeris[i].d_satpos_Y;
satpos(2,i)=d_ephemeris[i].d_satpos_Z;
obs(i)=pseudoranges_iter->second+d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
std::cout<<"ECEF satellite SV ID="<<d_ephemeris[i].d_satellite_PRN<<" X="<<d_ephemeris[i].d_satpos_X
<<" [m] Y="<<d_ephemeris[i].d_satpos_Y<<" [m] Z="<<d_ephemeris[i].d_satpos_Z<<" [m]\r\n";
obs(i)=gnss_pseudoranges_iter->second.pseudorange_m+d_ephemeris[i].d_satClkCorr*GPS_C_m_s;
valid_obs++;
}else{
// no valid pseudorange for the current channel
@ -254,9 +286,101 @@ void gps_l1_ca_ls_pvt::get_PVT(std::map<int,float> pseudoranges,double GPS_curre
{
arma::vec mypos;
mypos=leastSquarePos(satpos,obs,W);
std::cout << "Position ("<<GPS_current_time<<") ECEF = " << mypos << std::endl;
std::cout << "Position at TOW="<<GPS_current_time<<" is ECEF (X,Y,Z) = " << mypos << std::endl;
cart2geo(mypos(0), mypos(1), mypos(2), 4);
std::cout << "Position ("<<GPS_current_time<<") Lat = " << d_latitude_d << " Long ="<< d_longitude_d <<" Height="<<d_height_m<< std::endl;
std::cout << "Position at TOW="<<GPS_current_time<<" is Lat = " << d_latitude_d << " [º] Long ="<< d_longitude_d <<" [º] Height="<<d_height_m<<" [m]\r\n";
// ######## LOG FILE #########
if(d_flag_dump_enabled==true) {
// MULTIPLEXED FILE RECORDING - Record results to file
try {
double tmp_double;
// PVT GPS time
tmp_double=GPS_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double=mypos(0);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position North [m]
tmp_double=mypos(1);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position Up [m]
tmp_double=mypos(2);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// User clock offset [s]
tmp_double=mypos(3);
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Latitude [deg]
tmp_double=d_latitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Longitude [deg]
tmp_double=d_longitude_d;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// GEO user position Height [m]
tmp_double=d_height_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing PVT lib dump file "<<e.what()<<"\r\n";
}
}
// MOVING AVERAGE PVT
if (flag_averaging==true)
{
if (d_hist_longitude_d.size()==(unsigned int)d_averaging_depth)
{
// Pop oldest value
d_hist_longitude_d.pop_back();
d_hist_latitude_d.pop_back();
d_hist_height_m.pop_back();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d=0;
d_avg_longitude_d=0;
d_avg_height_m=0;
for (unsigned int i=0;i<d_hist_longitude_d.size();i++)
{
d_avg_latitude_d=d_avg_latitude_d+d_hist_latitude_d.at(i);
d_avg_longitude_d=d_avg_longitude_d+d_hist_longitude_d.at(i);
d_avg_height_m=d_avg_height_m+d_hist_height_m.at(i);
}
d_avg_latitude_d=d_avg_latitude_d/(double)d_averaging_depth;
d_avg_longitude_d=d_avg_longitude_d/(double)d_averaging_depth;
d_avg_height_m=d_avg_height_m/(double)d_averaging_depth;
return true; //indicates that the returned position is valid
}else{
//int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d);
d_hist_height_m.push_front(d_height_m);
d_avg_latitude_d=d_latitude_d;
d_avg_longitude_d=d_longitude_d;
d_avg_height_m=d_height_m;
return false;//indicates that the returned position is not valid yet
// output the average, although it will not have the full historic available
// d_avg_latitude_d=0;
// d_avg_longitude_d=0;
// d_avg_height_m=0;
// for (unsigned int i=0;i<d_hist_longitude_d.size();i++)
// {
// d_avg_latitude_d=d_avg_latitude_d+d_hist_latitude_d.at(i);
// d_avg_longitude_d=d_avg_longitude_d+d_hist_longitude_d.at(i);
// d_avg_height_m=d_avg_height_m+d_hist_height_m.at(i);
// }
// d_avg_latitude_d=d_avg_latitude_d/(double)current_depth;
// d_avg_longitude_d=d_avg_longitude_d/(double)current_depth;
// d_avg_height_m=d_avg_height_m/(double)current_depth;
}
}else{
return true;//indicates that the returned position is valid
}
}else{
return false;
}
}
void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection)

21
src/algorithms/observables/libs/gps_l1_ca_ls_pvt.h → src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
View File

@ -30,6 +30,7 @@
#ifndef GPS_L1_CA_LS_PVT_H_
#define GPS_L1_CA_LS_PVT_H_
#include <fstream>
#include <string>
#include <iostream>
#include <sstream>
@ -67,14 +68,30 @@ public:
double d_latitude_d;
double d_longitude_d;
double d_height_m;
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
int d_averaging_depth;
double d_avg_latitude_d;
double d_avg_longitude_d;
double d_avg_height_m;
double d_x_m;
double d_y_m;
double d_z_m;
gps_l1_ca_ls_pvt(int nchannels);
bool d_flag_dump_enabled;
std::string d_dump_filename;
std::ofstream d_dump_file;
void set_averaging_depth(int depth);
gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~gps_l1_ca_ls_pvt();
void get_PVT(std::map<int,float> pseudoranges,double GPS_current_time);
bool get_PVT(std::map<int,gnss_pseudorange> pseudoranges,double GPS_current_time,bool flag_averaging);
void cart2geo(double X, double Y, double Z, int elipsoid_selection);
};

3
src/algorithms/observables/libs/jamfile.jam → src/algorithms/PVT/libs/jamfile.jam
View File

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

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

@ -0,0 +1,126 @@
/*!
* \file kml_printer.cc
* \brief Prints PVT information to a GoogleEarth kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "kml_printer.h"
#include <glog/log_severity.h>
#include <glog/logging.h>
#include <time.h>
bool kml_printer::set_headers(std::string filename)
{
time_t rawtime;
struct tm * timeinfo;
time ( &rawtime );
timeinfo = localtime ( &rawtime );
kml_file.open(filename.c_str());
if (kml_file.is_open())
{
DLOG(INFO)<<"KML printer writting on "<<filename.c_str();
kml_file<<"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\r\n"
<<"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\r\n"
<<" <Document>\r\n"
<<" <name>GNSS Track</name>\r\n"
<<" <description>GNSS-SDR Receiver position log file created at "<<asctime (timeinfo)
<<" </description>\r\n"
<<"<Style id=\"yellowLineGreenPoly\">\r\n"
<<" <LineStyle>\r\n"
<<" <color>7f00ffff</color>\r\n"
<<" <width>1</width>\r\n"
<<" </LineStyle>\r\n"
<<"<PolyStyle>\r\n"
<<" <color>7f00ff00</color>\r\n"
<<"</PolyStyle>\r\n"
<<"</Style>\r\n"
<<"<Placemark>\r\n"
<<"<name>GNSS-SDR PVT</name>\r\n"
<<"<description>GNSS-SDR position log</description>\r\n"
<<"<styleUrl>#yellowLineGreenPoly</styleUrl>\r\n"
<<"<LineString>\r\n"
<<"<extrude>0</extrude>\r\n"
<<"<tessellate>1</tessellate>\r\n"
<<"<altitudeMode>absolute</altitudeMode>\r\n"
<<"<coordinates>\r\n";
return true;
}else{
return false;
}
}
bool kml_printer::print_position(gps_l1_ca_ls_pvt* position,bool print_average_values)
{
double latitude;
double longitude;
double height;
if (print_average_values==false)
{
latitude=position->d_latitude_d;
longitude=position->d_longitude_d;
height=position->d_height_m;
}else{
latitude=position->d_avg_latitude_d;
longitude=position->d_avg_longitude_d;
height=position->d_avg_height_m;
}
if (kml_file.is_open())
{
kml_file<<longitude<<","<<latitude<<","<<height<<"\r\n";
return true;
}else
{
return false;
}
}
bool kml_printer::close_file()
{
if (kml_file.is_open())
{
kml_file<<"</coordinates>\r\n"
<<"</LineString>\r\n"
<<"</Placemark>\r\n"
<<"</Document>\r\n"
<<"</kml>";
kml_file.close();
return true;
}else{
return false;
}
}
kml_printer::kml_printer ()
{
}
kml_printer::~kml_printer ()
{
}

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

@ -0,0 +1,59 @@
/*!
* \file kml_printer.h
* \brief Prints PVT information to a GoogleEarth kml file
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef KML_PRINTER_H_
#define KML_PRINTER_H_
#include <iostream>
#include <fstream>
#include "gps_l1_ca_ls_pvt.h"
class kml_printer
{
private:
std::ofstream kml_file;
public:
bool set_headers(std::string filename);
bool print_position(gps_l1_ca_ls_pvt* position,bool print_average_values);
bool close_file();
kml_printer();
~kml_printer();
};
#endif

0
src/algorithms/observables/libs/rinex_2_1_printer.cc → src/algorithms/PVT/libs/rinex_2_1_printer.cc
View File

0
src/algorithms/observables/libs/rinex_2_1_printer.h → src/algorithms/PVT/libs/rinex_2_1_printer.h
View File

8
src/algorithms/acquisition/adapters/gps_l1_ca_gps_sdr_acquisition.cc
View File

@ -30,7 +30,7 @@
*/
#include "gps_l1_ca_gps_sdr_acquisition.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include <gnuradio/gr_io_signature.h>
@ -63,7 +63,7 @@ GpsL1CaGpsSdrAcquisition::GpsL1CaGpsSdrAcquisition(
//vector_length_ = configuration->property(role + ".vector_length", 2048);
satellite_ = 0;
fs_in_ = configuration->property(role + ".fs_in", 2048000);
fs_in_ = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
if_ = configuration->property(role + ".ifreq", 0);
dump_ = configuration->property(role + ".dump", false);
doppler_max_ = 0;
@ -74,9 +74,7 @@ GpsL1CaGpsSdrAcquisition::GpsL1CaGpsSdrAcquisition(
//vector_length_=ceil((float)fs_in_*((float)acquisition_ms_/1000));
//--- Find number of samples per spreading code ----------------------------
const int32 _codeFreqBasis = 1023000; //Hz
const int32 _codeLength = 1023;
vector_length_ = round(fs_in_ / (_codeFreqBasis / _codeLength));
vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
printf("vector_length_ %i\n\r", vector_length_);

11
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc
View File

@ -32,7 +32,7 @@
*/
#include "gps_l1_ca_pcps_acquisition.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include <iostream>
#include <gnuradio/gr_io_signature.h>
@ -63,7 +63,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
default_item_type);
satellite_ = 0;
fs_in_ = configuration->property(role + ".fs_in", 2048000);
fs_in_ = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
if_ = configuration->property(role + ".ifreq", 0);
dump_ = configuration->property(role + ".dump", false);
shift_resolution_ = configuration->property(role + ".doppler_max", 15);
@ -72,12 +72,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
default_dump_filename);
//--- Find number of samples per spreading code ----------------------------
const signed int _codeFreqBasis = 1023000; //Hz
const signed int _codeLength = 1023;
std::cout << fs_in_ << " " << role << std::endl;
vector_length_ = round(fs_in_ / (_codeFreqBasis / _codeLength));
printf("vector_length_ %i\n\r", vector_length_);
vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
if (item_type_.compare("gr_complex") == 0)
{

10
src/algorithms/acquisition/adapters/gps_l1_ca_tong_pcps_acquisition.cc
View File

@ -31,7 +31,7 @@
*/
#include "gps_l1_ca_tong_pcps_acquisition.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include <gnuradio/gr_io_signature.h>
@ -64,7 +64,7 @@ GpsL1CaTongPcpsAcquisition::GpsL1CaTongPcpsAcquisition(
std::cout << "item type " << item_type_ << std::endl;
satellite_ = 0;
fs_in_ = configuration->property(role + ".fs_in", 2048000);
fs_in_ = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
if_ = configuration->property(role + ".ifreq", 0);
dump_ = configuration->property(role + ".dump", false);
doppler_max_ = configuration->property(role + ".doppler_max", 10);
@ -73,11 +73,7 @@ GpsL1CaTongPcpsAcquisition::GpsL1CaTongPcpsAcquisition(
default_dump_filename);
//--- Find number of samples per spreading code ----------------------------
const signed int _codeFreqBasis = 1023000; //Hz
const signed int _codeLength = 1023;
vector_length_ = round(fs_in_ / (_codeFreqBasis / _codeLength));
printf("vector_length_ %i\n\r", vector_length_);
vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
if (item_type_.compare("gr_complex") == 0)
{

21
src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_gps_sdr_acquisition_cc.cc
View File

@ -31,9 +31,6 @@
* -------------------------------------------------------------------------
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "gps_l1_ca_gps_sdr_acquisition_cc.h"
#include "control_message_factory.h"
@ -126,15 +123,15 @@ gps_l1_ca_gps_sdr_acquisition_cc::gps_l1_ca_gps_sdr_acquisition_cc(
gps_l1_ca_gps_sdr_acquisition_cc::~gps_l1_ca_gps_sdr_acquisition_cc()
{
delete d_sine_if;
delete d_sine_250;
delete d_sine_500;
delete d_sine_750;
delete d_fft_codes;
delete d_fft_if;
delete d_fft_250;
delete d_fft_500;
delete d_fft_750;
delete[] d_sine_if;
delete[] d_sine_250;
delete[] d_sine_500;
delete[] d_sine_750;
delete[] d_fft_codes;
delete[] d_fft_if;
delete[] d_fft_250;
delete[] d_fft_500;
delete[] d_fft_750;
if (d_dump)
{

16
src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_gps_sdr_acquisition_ss.cc
View File

@ -30,10 +30,6 @@
*
* -------------------------------------------------------------------------
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "gps_l1_ca_gps_sdr_acquisition_ss.h"
#include "gps_sdr_simd.h"
@ -129,12 +125,12 @@ gps_l1_ca_gps_sdr_acquisition_ss::gps_l1_ca_gps_sdr_acquisition_ss(
gps_l1_ca_gps_sdr_acquisition_ss::~gps_l1_ca_gps_sdr_acquisition_ss()
{
delete d_baseband_signal;
delete d_baseband_signal_shift;
delete d_sine_if;
delete d_sine_250;
delete d_sine_500;
delete d_sine_750;
delete[] d_baseband_signal;
delete[] d_baseband_signal_shift;
delete[] d_sine_if;
delete[] d_sine_250;
delete[] d_sine_500;
delete[] d_sine_750;
delete d_pFFT;
delete d_piFFT;

10
src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_pcps_acquisition_cc.cc
View File

@ -31,10 +31,6 @@
* -------------------------------------------------------------------------
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "gps_l1_ca_pcps_acquisition_cc.h"
#include "gps_sdr_signal_processing.h"
#include "control_message_factory.h"
@ -117,8 +113,10 @@ gps_l1_ca_pcps_acquisition_cc::gps_l1_ca_pcps_acquisition_cc(
gps_l1_ca_pcps_acquisition_cc::~gps_l1_ca_pcps_acquisition_cc()
{
delete d_sine_if;
delete d_fft_codes;
delete[] d_sine_if;
delete[] d_fft_codes;
delete d_ifft;
delete d_fft_if;
if (d_dump)
{

10
src/algorithms/acquisition/gnuradio_blocks/gps_l1_ca_tong_pcps_acquisition_cc.cc
View File

@ -29,9 +29,6 @@
*
* -------------------------------------------------------------------------
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "gps_l1_ca_tong_pcps_acquisition_cc.h"
#include "gps_sdr_signal_processing.h"
@ -129,8 +126,11 @@ gps_l1_ca_tong_pcps_acquisition_cc::gps_l1_ca_tong_pcps_acquisition_cc(
gps_l1_ca_tong_pcps_acquisition_cc::~gps_l1_ca_tong_pcps_acquisition_cc()
{
delete d_if_sin;
delete d_ca_codes;
delete[] d_if_sin;
delete[] d_ca_codes;
delete[] d_aux_ca_code;
delete d_fft_if;
delete d_ifft;
if (d_dump)
{

2
src/algorithms/channel/adapters/channel.cc
View File

@ -77,7 +77,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel,
repeat_ = configuration->property("Acquisition" + boost::lexical_cast<
std::string>(channel_) + ".repeat_satellite", false);
std::cout << "Channel " << channel_ << " satellite repeat = " << repeat_
DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_
<< std::endl;
acq_->set_channel_queue(&channel_internal_queue_);

1
src/algorithms/jamfile.jam
View File

@ -6,4 +6,5 @@ build-project signal_source ;
build-project tracking ;
build-project telemetry_decoder ;
build-project observables ;
build-project PVT ;
build-project output_filter ;

2
src/algorithms/libs/gps_sdr_signal_processing.cc
View File

@ -333,7 +333,7 @@ int32 run_agc(CPX *_buff, int32 _samps, int32 _bits, int32 _scale)
p = (int16 *)&_buff[0];
val = (1 << _scale - 1);
val = (1 << (_scale - 1));
max = 1 << _bits;
num = 0;

6
src/algorithms/libs/gps_sdr_simd.cc
View File

@ -497,7 +497,7 @@ void sse_cmul(CPX *A, CPX *B, int32 cnt)
int32 cnt1;
int32 cnt2;
volatile int32 M[4] = {0xffff0001, 0x00010001, 0xffff0001, 0x00010001}; //{1,-1,1,1,1,-1,1,1};
//volatile int32 M[4] = {0xffff0001, 0x00010001, 0xffff0001, 0x00010001}; //{1,-1,1,1,1,-1,1,1};
cnt1 = cnt/4;
cnt2 = cnt-4*cnt1;
@ -571,7 +571,7 @@ void sse_cmuls(CPX *A, CPX *B, int32 cnt, int32 shift)
int32 cnt2;
int32 round;
volatile int32 M[4] = {0xffff0001, 0x00010001, 0xffff0001, 0x00010001}; //{1,-1,1,1,1,-1,1,1};
//volatile int32 M[4] = {0xffff0001, 0x00010001, 0xffff0001, 0x00010001}; //{1,-1,1,1,1,-1,1,1};
cnt1 = cnt/4;
cnt2 = cnt-4*cnt1;
@ -652,7 +652,7 @@ void sse_cmulsc(CPX *A, CPX *B, CPX *C, int32 cnt, int32 shift)
int32 cnt2;
int32 round;
volatile int32 M[4] = {0xffff0001, 0x00010001, 0xffff0001, 0x00010001}; //{1,-1,1,1,1,-1,1,1};
//volatile int32 M[4] = {0xffff0001, 0x00010001, 0xffff0001, 0x00010001}; //{1,-1,1,1,1,-1,1,1};
cnt1 = cnt/4;
cnt2 = cnt-4*cnt1;

2
src/algorithms/libs/gps_sdr_x86.cc
View File

@ -290,7 +290,7 @@ void x86_float_max(float* _A, unsigned int* _index, float* _magt, unsigned int _
mag = index = 0;
for(int i=0; i<_cnt; i++) {
for(unsigned int i=0; i<_cnt; i++) {
if(_A[i] > mag) {
index = i;
mag = _A[i];

10
src/algorithms/observables/adapters/gps_l1_ca_observables.cc
View File

@ -60,15 +60,21 @@ GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration,
queue_(queue)
{
int output_rate_ms;
output_rate_ms=configuration->property(role + ".output_rate_ms", 500);
std::string default_dump_filename = "./observables.dat";
DLOG(INFO) << "role " << role;
bool flag_averaging;
flag_averaging=configuration->property(role + ".flag_averaging", false);
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
fs_in_ = configuration->property(role + ".fs_in", 0);
fs_in_ = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
observables_ = gps_l1_ca_make_observables_cc(in_streams_, queue_, dump_);
observables_ = gps_l1_ca_make_observables_cc(in_streams_, queue_, dump_,dump_filename_,output_rate_ms,flag_averaging);
observables_->set_fs_in(fs_in_);
DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")";

332
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@ -1,17 +1,32 @@
/**
* Copyright notice
/*!
* \file gps_l1_ca_observables_cc.cc
* \brief Pseudorange computation module for GPS L1 C/A
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
/**
* Author: Javier Arribas, 2011. jarribas(at)cttc.es
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <iostream>
#include <sstream>
#include <vector>
@ -37,165 +52,236 @@ using namespace std;
gps_l1_ca_observables_cc_sptr
gps_l1_ca_make_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump) {
gps_l1_ca_make_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging) {
return gps_l1_ca_observables_cc_sptr(new gps_l1_ca_observables_cc(nchannels, queue, dump));
return gps_l1_ca_observables_cc_sptr(new gps_l1_ca_observables_cc(nchannels, queue, dump, dump_filename, output_rate_ms, flag_averaging));
}
gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump) :
gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging) :
gr_block ("gps_l1_ca_observables_cc", gr_make_io_signature (nchannels, nchannels, sizeof(gnss_synchro)),
gr_make_io_signature(1, 1, sizeof(gr_complex))) {
//TODO: change output channels to have Pseudorange GNURadio signature: nchannels input (float), nchannels output (float)
gr_make_io_signature(nchannels, nchannels, sizeof(gnss_pseudorange))) {
// initialize internal vars
d_queue = queue;
d_dump = dump;
d_nchannels = nchannels;
d_rinex_printer.set_headers("GNSS-SDR"); //TODO: read filename from config
d_ls_pvt=new gps_l1_ca_ls_pvt(nchannels);
d_ephemeris_clock_s=0.0;
if (d_dump==true)
{
std::stringstream d_dump_filename_str;//create a stringstream to form the dump filename
d_dump_filename_str<<"./data/obs.dat"; //TODO: get filename and path from config in the adapter
d_dump_filename=d_dump_filename_str.str();
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
}
d_output_rate_ms=output_rate_ms;
d_history_prn_delay_ms= new std::deque<double>[d_nchannels];
d_dump_filename=dump_filename;
d_flag_averaging=flag_averaging;
// ############# 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<<"Observables dump enabled Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "Exception opening observables dump file "<<e.what()<<"\r\n";
}
}
}
}
gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc() {
delete d_ls_pvt;
d_dump_file.close();
delete[] d_history_prn_delay_ms;
}
bool pairCompare_min( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
bool pairCompare_gnss_synchro( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
{
return (a.second.preamble_delay_ms+a.second.prn_delay_ms) < (b.second.preamble_delay_ms+b.second.prn_delay_ms);
return (a.second.preamble_delay_ms) < (b.second.preamble_delay_ms);
}
bool pairCompare_max( pair<int,gnss_synchro> a, pair<int,gnss_synchro> b)
bool pairCompare_double( pair<int,double> a, pair<int,double> b)
{
return (a.second.preamble_delay_ms+a.second.prn_delay_ms) > (b.second.preamble_delay_ms+b.second.prn_delay_ms);
return (a.second) < (b.second);
}
void clearQueue( std::deque<double> &q )
{
std::deque<double> empty;
std::swap( q, empty );
}
int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
gnss_synchro **in = (gnss_synchro **) &input_items[0]; //Get the input samples pointer
gnss_synchro **in = (gnss_synchro **) &input_items[0]; //Get the input pointer
gnss_pseudorange **out = (gnss_pseudorange **) &output_items[0]; //Get the output pointer
// CONSTANTS TODO: place in a file
const float code_length_s=0.001; //1 ms
const float C_m_ms= GPS_C_m_s/1000.0; // The speed of light, [m/ms]
const float nav_sol_period_ms=1000;
//--- Find number of samples per spreading code ----------------------------
const signed int codeFreqBasis=1023000; //Hz
const signed int codeLength=1023;
const unsigned int samples_per_code = round(d_fs_in/ (codeFreqBasis / codeLength));
gnss_pseudorange current_gnss_pseudorange;
map<int,gnss_synchro> gps_words;
map<int,gnss_synchro>::iterator gps_words_iter;
map<int,float> pseudoranges;
map<int,float>::iterator pseudoranges_iter;
map<int,float> pseudoranges_dump;
map<int,double>::iterator current_prn_timestamps_ms_iter;
map<int,double> current_prn_timestamps_ms;
float min_preamble_delay_ms;
float max_preamble_delay_ms;
bool flag_valid_pseudoranges=false;
double min_preamble_delay_ms;
double max_preamble_delay_ms;
float pseudoranges_timestamp_ms;
float traveltime_ms;
float pseudorange_m;
double pseudoranges_timestamp_ms;
double traveltime_ms;
double pseudorange_m;
int history_shift=0;
double delta_timestamp_ms;
double min_delta_timestamp_ms;
double actual_min_prn_delay_ms;
double current_prn_delay_ms;
int pseudoranges_reference_sat_ID=0;
unsigned int pseudoranges_reference_sat_channel_ID=0;
d_sample_counter++; //count for the processed samples
bool flag_history_ok=true; //flag to indicate that all the queues have filled their timestamp history
/*!
* 1. Read the GNSS SYNCHRO objects from available channels to obtain the preamble timestamp, current PRN start time and accumulated carrier phase
*/
for (unsigned int i=0; i<d_nchannels ; i++)
{
if (in[i][0].valid_word) //if this channel have valid word
{
gps_words.insert(pair<int,gnss_synchro>(in[i][0].channel_ID,in[i][0])); //record the word structure in a map for pseudoranges
}
}
if(gps_words.size()>0)
{
// find the minimum index (nearest satellite, will be the reference)
gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare_min);
min_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms+gps_words_iter->second.prn_delay_ms; //[ms]
pseudoranges_timestamp_ms=min_preamble_delay_ms; //save the shortest pseudorange timestamp to compute the RINEX timestamp
pseudoranges_reference_sat_ID=gps_words_iter->second.satellite_PRN;
gps_words_iter=max_element(gps_words.begin(),gps_words.end(),pairCompare_max);
max_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; //[ms]
if ((max_preamble_delay_ms-min_preamble_delay_ms)<70) flag_valid_pseudoranges=true;
//compute the pseudoranges
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{
std::cout<<"prn_delay="<<gps_words_iter->second.prn_delay_ms<<std::endl;
traveltime_ms=gps_words_iter->second.preamble_delay_ms+gps_words_iter->second.prn_delay_ms-min_preamble_delay_ms+GPS_STARTOFFSET_ms; //[ms]
pseudorange_m=traveltime_ms*C_m_ms; // [m]
pseudoranges.insert(pair<int,float>(gps_words_iter->second.satellite_PRN,pseudorange_m)); //record the preamble index in a map
if (d_dump==true)
{
pseudoranges_dump.insert(pair<int,float>(gps_words_iter->second.channel_ID,pseudorange_m));
}
}
// write the pseudoranges to RINEX OBS file
// 1- need a valid clock
if (flag_valid_pseudoranges==true and d_last_nav_msg.d_satellite_PRN>0)
// RECORD PRN start timestamps history
if (d_history_prn_delay_ms[i].size()<MAX_TOA_DELAY_MS)
{
//d_rinex_printer.LogRinex2Obs(d_last_nav_msg,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0,pseudoranges);
// compute on the fly PVT solution
d_ls_pvt->get_PVT(pseudoranges,d_ephemeris_clock_s+((double)pseudoranges_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0);
d_history_prn_delay_ms[i].push_front(in[i][0].prn_delay_ms);
flag_history_ok=false; // at least one channel need more samples
}else{
//clearQueue(d_history_prn_delay_ms[i]); //clear the queue as the preamble arrives
d_history_prn_delay_ms[i].pop_back();
d_history_prn_delay_ms[i].push_front(in[i][0].prn_delay_ms);
}
}
//debug
cout << setprecision(16);
for(pseudoranges_iter = pseudoranges.begin();
pseudoranges_iter != pseudoranges.end();
pseudoranges_iter++)
{
cout<<"Pseudoranges =("<<pseudoranges_iter->first<<","<<pseudoranges_iter->second<<")"<<endl;
}
gps_navigation_message nav_msg;
if (d_nav_queue->try_pop(nav_msg)==true)
{
cout<<"New ephemeris record has arrived from SAT ID "<<nav_msg.d_satellite_PRN<<endl;
d_last_nav_msg=nav_msg;
d_ls_pvt->d_ephemeris[nav_msg.d_channel_ID]=nav_msg;
// **** update pseudoranges clock ****
if (nav_msg.d_satellite_PRN==pseudoranges_reference_sat_ID)
{
d_ephemeris_clock_s=d_last_nav_msg.d_TOW;
d_ephemeris_timestamp_ms=d_last_nav_msg.d_subframe1_timestamp_ms;
}
// **** write ephemeris to RINES NAV file
//d_rinex_printer.LogRinex2Nav(nav_msg);
}
if (d_dump==true)
/*!
* 1.2 Assume no satellites in tracking
*/
for (unsigned int i=0; i<d_nchannels ; i++)
{
float tmp_float=0.0;
for (int i=0;i<d_nchannels;i++)
{
pseudoranges_iter=pseudoranges_dump.find(i);
if (pseudoranges_iter!=pseudoranges_dump.end())
{
d_dump_file.write((char*)&pseudoranges_iter->second, sizeof(float));
}else{
d_dump_file.write((char*)&tmp_float, sizeof(float));
}
}
current_gnss_pseudorange.valid=false;
current_gnss_pseudorange.SV_ID=0;
current_gnss_pseudorange.pseudorange_m=0;
current_gnss_pseudorange.timestamp_ms=0;
*out[i]=current_gnss_pseudorange;
}
/*!
* 2. Compute RAW pseudorranges: Use only the valid channels (channels that are tracking a satellite)
*/
if(gps_words.size()>0 and flag_history_ok==true)
{
/*!
* 2.1 find the minimum preamble timestamp (nearest satellite, will be the reference)
*/
// The nearest satellite, first preamble to arrive
gps_words_iter=min_element(gps_words.begin(),gps_words.end(),pairCompare_gnss_synchro);
min_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms; //[ms]
pseudoranges_reference_sat_ID=gps_words_iter->second.satellite_PRN; // it is the reference!
pseudoranges_reference_sat_channel_ID=gps_words_iter->second.channel_ID;
// The farthest satellite, last preamble to arrive
gps_words_iter=max_element(gps_words.begin(),gps_words.end(),pairCompare_gnss_synchro);
max_preamble_delay_ms=gps_words_iter->second.preamble_delay_ms;
min_delta_timestamp_ms=gps_words_iter->second.prn_delay_ms-max_preamble_delay_ms; //[ms]
// check if this is a valid set of observations
if ((max_preamble_delay_ms-min_preamble_delay_ms)<MAX_TOA_DELAY_MS)
{
// Now we have to determine were we are in time, compared with the last preamble! -> we select the corresponding history
/*!
* \todo Explain this better!
*/
//bool flag_preamble_navigation_now=true;
// find again the minimum CURRENT minimum preamble time, taking into account the preamble timeshift
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{
delta_timestamp_ms=(gps_words_iter->second.prn_delay_ms-gps_words_iter->second.preamble_delay_ms)-min_delta_timestamp_ms;
history_shift=round(delta_timestamp_ms);
//std::cout<<"history_shift="<<history_shift<<"\r\n";
current_prn_timestamps_ms.insert(pair<int,double>(gps_words_iter->second.channel_ID,d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]));
// debug: preamble position test
//if ((d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]-gps_words_iter->second.preamble_delay_ms)<0.1)
//{std::cout<<"ch "<<gps_words_iter->second.channel_ID<<" current_prn_time-last_preamble_prn_time="<<
// d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]-gps_words_iter->second.preamble_delay_ms<<"\r\n";
//}else{
// flag_preamble_navigation_now=false;
//}
}
//if (flag_preamble_navigation_now==true)
//{
//std::cout<<"PREAMBLE NAVIGATION NOW!\r\n";
//d_sample_counter=0;
//}
current_prn_timestamps_ms_iter=min_element(current_prn_timestamps_ms.begin(),current_prn_timestamps_ms.end(),pairCompare_double);
actual_min_prn_delay_ms=current_prn_timestamps_ms_iter->second;
pseudoranges_timestamp_ms=actual_min_prn_delay_ms; //save the shortest pseudorange timestamp to compute the current GNSS timestamp
/*!
* 2.2 compute the pseudoranges
*/
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{
// #### compute the pseudorrange for this satellite ###
current_prn_delay_ms=current_prn_timestamps_ms.at(gps_words_iter->second.channel_ID);
traveltime_ms=current_prn_delay_ms-actual_min_prn_delay_ms+GPS_STARTOFFSET_ms; //[ms]
//std::cout<<"delta_time_ms="<<current_prn_delay_ms-actual_min_prn_delay_ms<<"\r\n";
pseudorange_m=traveltime_ms*GPS_C_m_ms; // [m]
// update the pseudorange object
current_gnss_pseudorange.pseudorange_m=pseudorange_m;
current_gnss_pseudorange.timestamp_ms=pseudoranges_timestamp_ms;
current_gnss_pseudorange.SV_ID=gps_words_iter->second.satellite_PRN;
current_gnss_pseudorange.valid=true;
// #### write the pseudorrange block output for this satellite ###
*out[gps_words_iter->second.channel_ID]=current_gnss_pseudorange;
}
}
}
if(d_dump==true) {
// MULTIPLEXED FILE RECORDING - Record results to file
try {
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
tmp_double=in[i][0].preamble_delay_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=in[i][0].prn_delay_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=out[i][0].pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=out[i][0].timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double=out[i][0].SV_ID;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing observables dump file "<<e.what()<<"\r\n";
}
}
consume_each(1); //one by one
return 0;
if ((d_sample_counter%d_output_rate_ms)==0)
{
return 1; //Output the observables
}else{
return 0;//hold on
}
}

58
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
View File

@ -1,11 +1,32 @@
/**
* Copyright notice
/*!
* \file gps_l1_ca_observables_cc.h
* \brief Pseudorange computation module for GPS L1 C/A
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
/**
* Author: Javier Arribas, 2011. jarribas(at)cttc.es
*/
#ifndef GPS_L1_CA_OBSERVABLES_CC_H
#define GPS_L1_CA_OBSERVABLES_CC_H
@ -24,14 +45,13 @@
#include "gps_navigation_message.h"
#include "rinex_2_1_printer.h"
#include "gps_l1_ca_ls_pvt.h"
#include "GPS_L1_CA.h"
class gps_l1_ca_observables_cc;
typedef boost::shared_ptr<gps_l1_ca_observables_cc> gps_l1_ca_observables_cc_sptr;
gps_l1_ca_observables_cc_sptr
gps_l1_ca_make_observables_cc(unsigned int n_channels, gr_msg_queue_sptr queue, bool dump);
gps_l1_ca_make_observables_cc(unsigned int n_channels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging);
class gps_l1_ca_observables_cc : public gr_block {
@ -39,32 +59,24 @@ class gps_l1_ca_observables_cc : public gr_block {
private:
friend gps_l1_ca_observables_cc_sptr
gps_l1_ca_make_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump);
gps_l1_ca_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump);
gps_l1_ca_make_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging);
gps_l1_ca_observables_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int output_rate_ms, bool flag_averaging);
// class private vars
gr_msg_queue_sptr d_queue;
bool d_dump;
bool d_flag_averaging;
long int d_sample_counter;
unsigned int d_nchannels;
unsigned long int d_fs_in;
int d_output_rate_ms;
std::string d_dump_filename;
std::ofstream d_dump_file;
std::deque<double> *d_history_prn_delay_ms;
concurrent_queue<gps_navigation_message> *d_nav_queue; // Navigation ephemeris queue
rinex_printer d_rinex_printer; // RINEX printer class
gps_navigation_message d_last_nav_msg; //last navigation message
double d_ephemeris_clock_s;
double d_ephemeris_timestamp_ms;
gps_l1_ca_ls_pvt *d_ls_pvt;
public:
~gps_l1_ca_observables_cc ();

3
src/algorithms/observables/jamfile.jam
View File

@ -1,3 +1,2 @@
build-project adapters ;
build-project gnuradio_blocks ;
build-project libs ;
build-project gnuradio_blocks ;

30
src/algorithms/signal_source/adapters/file_signal_source.cc
View File

@ -38,6 +38,8 @@
#include <string>
#include <iostream>
#include <fstream>
#include <glog/log_severity.h>
#include <glog/logging.h>
@ -93,6 +95,34 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration,
= gr_make_file_source(item_size_, filename_.c_str(), repeat_);
DLOG(INFO) << "file_source(" << file_source_->unique_id() << ")";
if (samples_==0)
{
/*!
* BUG workaround: The GNURadio file source does not stop the receiver after reaching the End of File.
* A possible solution is to compute the file lenght in samples using file size, excluding the last 100 milliseconds, and enable always the
* valve block
*/
std::ifstream file (filename_.c_str(), std::ios::in|std::ios::binary|std::ios::ate);
std::ifstream::pos_type size;
if (file.is_open())
{
size =file.tellg();
}else{
std::cout<<"file_signal_source: Unable to open the samples file "<<filename_.c_str()<<"\r\n";
LOG_AT_LEVEL(WARNING)<<"file_signal_source: Unable to open the samples file "<<filename_.c_str();
}
std::cout<<std::setprecision(16);
std::cout<<"Processing file "<<filename_<<" containing "<<(double)size<<" [bytes] \r\n";
if (size>0)
{
samples_=floor((double)size/(double)item_size())-ceil(0.1*(double)sampling_frequency_); //process all the samples available in the file excluding the last 100 ms
}
}
double signal_duration_s;
signal_duration_s=(double)samples_*(1/(double)sampling_frequency_);
DLOG(INFO)<<"Total samples to be processed="<<samples_<<" GNSS signal duration= "<<signal_duration_s<<" [s]";
std::cout<<"GNSS signal recorded time to be processed: "<<signal_duration_s<<" [s]\r\n";
// if samples != 0 then enable a flow valve to stop the process after n samples
if (samples_ != 0)
{

17
src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc
View File

@ -66,22 +66,13 @@ GpsL1CaTelemetryDecoder::GpsL1CaTelemetryDecoder(ConfigurationInterface* configu
DLOG(INFO) << "role " << role;
DLOG(INFO) << "vector length " << vector_length_;
item_type_ = configuration->property(role + ".item_type", default_item_type);
vector_length_ = configuration->property(role + ".vector_length", 2048);
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); //unused!
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
int fs_in;
fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
if(item_type_.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
telemetry_decoder_ = gps_l1_ca_make_telemetry_decoder_cc(satellite_, 0, 0, vector_length_, queue_, dump_); // TODO fix me
}
else
{
LOG_AT_LEVEL(WARNING) << item_type_ << " unknown navigation item type.";
}
telemetry_decoder_ = gps_l1_ca_make_telemetry_decoder_cc(satellite_, 0, (long)fs_in, vector_length_, queue_, dump_); // TODO fix me
DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";

3
src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.h
View File

@ -79,7 +79,7 @@ public:
size_t item_size()
{
return item_size_;
return 0;
}
private:
@ -87,7 +87,6 @@ private:
int satellite_;
int channel_;
size_t item_size_;
unsigned int vector_length_;
std::string item_type_;

178
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -1,19 +1,36 @@
/*!
* Navigation message demodulator based on the Kay Borre book MATLAB-based GPS receiver
* \file gps_l1_ca_telemetry_decoder_cc.cc
* \brief Navigation message demodulator based on the Kay Borre book MATLAB-based GPS receiver
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
/**
* Copyright notice
/*!
* \todo Clean this code and move the telemetri definitions to GPS_L1_CA system definitions file
*/
/**
* Author: Javier Arribas, 2011. jarribas(at)cttc.es
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "gps_l1_ca_telemetry_decoder_cc.h"
#include "control_message_factory.h"
@ -27,8 +44,11 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
using google::LogMessage;
using google::LogMessage;
/*!
* \todo name and move the magic numbers to GPS_L1_CA.h
*/
gps_l1_ca_telemetry_decoder_cc_sptr
gps_l1_ca_make_telemetry_decoder_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump) {
@ -47,7 +67,7 @@ void gps_l1_ca_telemetry_decoder_cc::forecast (int noutput_items,
gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump) :
gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(float)),
gr_block ("gps_navigation_cc", gr_make_io_signature (5, 5, sizeof(double)),
gr_make_io_signature(1, 1, sizeof(gnss_synchro))) {
// initialize internal vars
d_queue = queue;
@ -55,6 +75,9 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int sate
d_satellite = satellite;
d_vector_length = vector_length;
d_samples_per_bit=20; // it is exactly 1000*(1/50)=20
d_fs_in=fs_in;
d_preamble_duration_seconds=(1.0/(float)GPS_CA_TELEMETRY_RATE_BITS_SECOND)*(float)GPS_CA_PREAMBLE_LENGTH_BITS;
//std::cout<<"d_preamble_duration_seconds="<<d_preamble_duration_seconds<<"\r\n";
// set the preamble
unsigned short int preambles_bits[8]=GPS_PREAMBLE;
@ -77,6 +100,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(unsigned int sate
}
}
d_sample_counter=0;
d_preamble_code_phase_seconds=0;
d_stat=0;
d_preamble_index=0;
d_symbol_accumulator_counter=0;
@ -101,57 +125,88 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
int corr_value=0;
int preamble_diff;
gnss_synchro gps_synchro; //structure to save the synchronization information
gnss_synchro **out = (gnss_synchro **) &output_items[0];
d_sample_counter++; //count for the processed samples
const float **in = (const float **) &input_items[0]; //Get the input samples pointer
const double **in = (const double **) &input_items[0]; //Get the input samples pointer
// ########### Output the tracking data to navigation and PVT ##########
// Output channel 0: Prompt correlator output Q
// *out[0]=(double)d_Prompt.real();
// // Output channel 1: Prompt correlator output I
// *out[1]=(double)d_Prompt.imag();
// // Output channel 2: PRN absolute delay [s]
// *out[2]=d_sample_counter_seconds;
// // Output channel 3: d_acc_carrier_phase_rad [rad]
// *out[3]=(double)d_acc_carrier_phase_rad;
// // Output channel 4: PRN code phase [s]
// *out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in);
/*!
* \todo Check the HOW GPS time computation, taking into account that the preamble correlation last 160 symbols, which is 160 ms in GPS CA L1
*/
// FIFO history to get the exact timestamp of the first symbol of the preamble
// if (d_prn_start_sample_history.size()<160)
// {
// // fill the queue
// d_prn_start_sample_history.push_front(in[2][0]);
// consume_each(1); //one by one
// return 1;
// }else{
// d_prn_start_sample_history.pop_back();
// d_prn_start_sample_history.push_front(in[2][0]);
// }
// TODO Optimize me!
//******* preamble correlation ********
for (unsigned int i=0;i<d_samples_per_bit*8;i++){
if (in[1][i] <= 0) // symbols clipping
if (in[1][i] < 0) // symbols clipping
{
corr_value-=d_preambles_symbols[i];
}else{
corr_value+=d_preambles_symbols[i];
}
}
d_flag_preamble=false;
//******* frame sync ******************
if (abs(corr_value)>=160){
//TODO: Rewrite with state machine
if (d_stat==0)
{
d_GPS_FSM.Event_gps_word_preamble();
d_preamble_index=d_sample_counter;//record the preamble sample stamp
std::cout<<"Pre-detection SAT "<<this->d_satellite<<std::endl;
d_symbol_accumulator=0; //sync the symbol to bits integrator
d_symbol_accumulator_counter=0;
d_frame_bit_index=8;
d_stat=1; // enter into frame pre-detection status
}else if (d_stat==1) //check 6 seconds of preample separation
{
preamble_diff=abs(d_sample_counter-d_preamble_index);
if (abs(preamble_diff-6000)<1)
{
d_GPS_FSM.Event_gps_word_preamble();
d_preamble_index=d_sample_counter;//record the preamble sample stamp (t_P)
d_preamble_phase=in[2][0]; //record the PRN start sample index associated to the preamble
//TODO: Rewrite with state machine
if (d_stat==0)
{
d_GPS_FSM.Event_gps_word_preamble();
d_preamble_index=d_sample_counter;//record the preamble sample stamp
std::cout<<"Preamble detection for SAT "<<d_satellite<<std::endl;
d_symbol_accumulator=0; //sync the symbol to bits integrator
d_symbol_accumulator_counter=0;
d_frame_bit_index=8;
d_stat=1; // enter into frame pre-detection status
}else if (d_stat==1) //check 6 seconds of preample separation
{
preamble_diff=abs(d_sample_counter-d_preamble_index);
if (abs(preamble_diff-6000)<1)
{
d_GPS_FSM.Event_gps_word_preamble();
d_flag_preamble=true;
d_preamble_index=d_sample_counter;//record the preamble sample stamp (t_P)
d_preamble_time_seconds=in[2][0]-d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble
d_preamble_code_phase_seconds=in[4][0];
if (!d_flag_frame_sync){
d_flag_frame_sync=true;
std::cout<<" Frame sync SAT "<<this->d_satellite<<" with preamble start at "<<in[2][0]<<" [ms]"<<std::endl;
}
}else
{
if (preamble_diff>7000){
std::cout<<"lost of frame sync SAT "<<this->d_satellite<<std::endl;
d_stat=0; //lost of frame sync
d_flag_frame_sync=false;
}
}
}
if (!d_flag_frame_sync){
d_flag_frame_sync=true;
std::cout<<" Frame sync SAT "<<d_satellite<<" with preamble start at "<<d_preamble_time_seconds<<" [s]"<<std::endl;
}
}
}
}else{
if (d_stat==1)
{
preamble_diff=d_sample_counter-d_preamble_index;
if (preamble_diff>6001){
std::cout<<"Lost of frame sync SAT "<<this->d_satellite<<" preamble_diff= "<<preamble_diff<<std::endl;
d_stat=0; //lost of frame sync
d_flag_frame_sync=false;
}
}
}
//******* code error accumulator *****
@ -194,7 +249,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
}
if (gps_word_parityCheck(d_GPS_frame_4bytes)) {
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes,&d_GPS_frame_4bytes,sizeof(char)*4);
d_GPS_FSM.d_preamble_time_ms=d_preamble_phase;
d_GPS_FSM.d_preamble_time_ms=d_preamble_time_seconds*1000.0;
d_GPS_FSM.Event_gps_word_valid();
d_flag_parity=true;
}else{
@ -209,21 +264,18 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
}
// output the frame
consume_each(1); //one by one
consume_each(1); //one by one
if ((d_sample_counter%NAVIGATION_OUTPUT_RATE_MS)==0)
{
gps_synchro.valid_word=(d_flag_frame_sync==true and d_flag_parity==true);
//gps_synchro.preamble_delay_ms=(float)d_preamble_index;
gps_synchro.preamble_delay_ms=(float)d_preamble_index;
gps_synchro.prn_delay_ms=in[3][0];
gps_synchro.satellite_PRN=d_satellite;
gps_synchro.channel_ID=d_channel;
*out[0]=gps_synchro;
return 1;
}else{
return 0;
}
gps_synchro.valid_word=(d_flag_frame_sync==true and d_flag_parity==true);
gps_synchro.flag_preamble=d_flag_preamble;
gps_synchro.preamble_delay_ms=d_preamble_time_seconds*1000.0;
gps_synchro.prn_delay_ms=(in[2][0]-d_preamble_duration_seconds)*1000.0;
gps_synchro.preamble_code_phase_ms=d_preamble_code_phase_seconds*1000.0;
gps_synchro.preamble_code_phase_correction_ms=(in[4][0]-d_preamble_code_phase_seconds)*1000.0;
gps_synchro.satellite_PRN=d_satellite;
gps_synchro.channel_ID=d_channel;
*out[0]=gps_synchro;
return 1;
}

43
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@ -1,12 +1,31 @@
/**
* Copyright notice
/*!
* \file gps_l1_ca_telemetry_decoder_cc.h
* \brief Navigation message demodulator based on the Kay Borre book MATLAB-based GPS receiver
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
/**
* Author: Javier Arribas, 2011. jarribas(at)cttc.es
*/
#ifndef GPS_L1_CA_TELEMETRY_DECODER_CC_H
#define GPS_L1_CA_TELEMETRY_DECODER_CC_H
@ -63,8 +82,11 @@ private:
unsigned int d_GPS_frame_4bytes;
unsigned int d_prev_GPS_frame_4bytes;
bool d_flag_parity;
bool d_flag_preamble;
int d_word_number;
long d_fs_in;
double d_preamble_duration_seconds;
// navigation message vars
gps_navigation_message d_nav;
GpsL1CaSubframeFsm d_GPS_FSM;
@ -76,7 +98,10 @@ private:
int d_satellite;
int d_channel;
float d_preamble_phase;
//std::deque<double> d_prn_start_sample_history;
double d_preamble_time_seconds;
double d_preamble_code_phase_seconds;
std::string d_dump_filename;
std::ofstream d_dump_file;

11
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc
View File

@ -144,16 +144,17 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
{
int subframe_ID;
// NEW GPS SUBFRAME HAS ARRIVED!
subframe_ID=d_nav.subframe_decoder(this->d_subframe); //decode the subframe
std::cout<<"NAVIGATION FSM: received subframe "<<subframe_ID<<" for satellite "<<d_nav.d_satellite_PRN<<std::endl;
d_nav.d_satellite_PRN=d_satellite_PRN;
d_nav.d_channel_ID=d_channel_ID;
if (subframe_ID==1) {
d_nav.d_subframe1_timestamp_ms=this->d_preamble_time_ms-6002;
std::cout<<"FSM: set subframe1 preamble timestamp for sat "<<d_nav.d_satellite_PRN<<std::endl;
d_nav.d_subframe1_timestamp_ms=this->d_preamble_time_ms;
//std::cout<<"NAVIGATION FSM: set subframe 1 preamble timestamp for satellite "<<d_nav.d_satellite_PRN<<std::endl;
}
//TODO: change to subframe 5
/*!
* \todo change satellite validation to subframe 5 because it will have a complete set of ephemeris parameters
*/
if (subframe_ID==3) { // if the subframe is the 5th, then
if (d_nav.satellite_validation()) // if all the satellite ephemeris parameters are good, then
{

12
src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc
View File

@ -34,7 +34,7 @@
*/
#include "gps_l1_ca_dll_fll_pll_tracking.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include <gnuradio/gr_io_signature.h>
@ -71,8 +71,8 @@ GpsL1CaDllFllPllTracking::GpsL1CaDllFllPllTracking(
int order;
item_type = configuration->property(role + ".item_type",default_item_type);
vector_length = configuration->property(role + ".vector_length", 2048);
fs_in = configuration->property(role + ".fs_in", 2048000);
//vector_length = configuration->property(role + ".vector_length", 2048);
fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false);
order = configuration->property(role + ".order", 2);
@ -81,16 +81,18 @@ GpsL1CaDllFllPllTracking::GpsL1CaDllFllPllTracking(
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./tracking.dat";
std::string default_dump_filename = "./track_ch";
dump_filename = configuration->property(role + ".dump_filename",
default_dump_filename); //unused!
vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
//################# MAKE TRACKING GNURadio object ###################
if (item_type.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
tracking_ = gps_l1_ca_dll_fll_pll_make_tracking_cc(satellite_, f_if,
fs_in, vector_length, queue_, dump, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips);
fs_in, vector_length, queue_, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips);
}
else
{

12
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc
View File

@ -34,7 +34,7 @@
*/
#include "gps_l1_ca_dll_pll_tracking.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include <gnuradio/gr_io_signature.h>
@ -69,24 +69,26 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking(
float early_late_space_chips;
item_type = configuration->property(role + ".item_type",default_item_type);
vector_length = configuration->property(role + ".vector_length", 2048);
fs_in = configuration->property(role + ".fs_in", 2048000);
//vector_length = configuration->property(role + ".vector_length", 2048);
fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./tracking.dat";
std::string default_dump_filename = "./track_ch";
dump_filename = configuration->property(role + ".dump_filename",
default_dump_filename); //unused!
vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
//################# MAKE TRACKING GNURadio object ###################
if (item_type.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
tracking_ = gps_l1_ca_dll_pll_make_tracking_cc(satellite_, f_if,
fs_in, vector_length, queue_, dump,pll_bw_hz,dll_bw_hz,early_late_space_chips);
fs_in, vector_length, queue_, dump, dump_filename, pll_bw_hz,dll_bw_hz,early_late_space_chips);
}
else
{

333
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
View File

@ -56,15 +56,17 @@
* \todo Include in definition header file
*/
#define CN0_ESTIMATION_SAMPLES 10
#define MINIMUM_VALID_CN0 25
#define MAXIMUM_LOCK_FAIL_COUNTER 200
using google::LogMessage;
gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
return gps_l1_ca_dll_fll_pll_tracking_cc_sptr(new gps_l1_ca_dll_fll_pll_tracking_cc(satellite, if_freq,
fs_in, vector_length, queue, dump, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips));
fs_in, vector_length, queue, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips));
}
void gps_l1_ca_dll_fll_pll_tracking_cc::forecast (int noutput_items,
@ -73,9 +75,9 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::forecast (int noutput_items,
}
gps_l1_ca_dll_fll_pll_tracking_cc::gps_l1_ca_dll_fll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
gr_block ("gps_l1_ca_dll_fll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
gr_make_io_signature(5, 5, sizeof(float))) {
gr_make_io_signature(5, 5, sizeof(double))) {
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars
@ -86,6 +88,7 @@ gps_l1_ca_dll_fll_pll_tracking_cc::gps_l1_ca_dll_fll_pll_tracking_cc(unsigned in
d_fs_in = fs_in;
d_vector_length = vector_length;
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
d_dump_filename=dump_filename;
// Initialize tracking variables ==========================================
d_carrier_loop_filter.set_params(fll_bw_hz,pll_bw_hz,order);
@ -101,6 +104,7 @@ gps_l1_ca_dll_fll_pll_tracking_cc::gps_l1_ca_dll_fll_pll_tracking_cc(unsigned in
// sample synchronization
d_sample_counter=0;
d_sample_counter_seconds=0;
d_acq_sample_stamp=0;
d_last_seg=0;// this is for debug output only
@ -125,8 +129,9 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
*/
unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp-d_vector_length;
acq_trk_diff_seconds=acq_trk_diff_samples/(float)d_fs_in;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp;//-d_vector_length;
//std::cout<<"acq_trk_diff_samples="<<acq_trk_diff_samples<<"\r\n";
acq_trk_diff_seconds=(float)acq_trk_diff_samples/(float)d_fs_in;
//doppler effect
// Fd=(C/(C+Vr))*F
float radial_velocity;
@ -140,15 +145,24 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
d_next_prn_length_samples=round(T_prn_mod_samples);
//compute the code phase chips prediction
float delta_T_prn_samples;
float delay_correction_samples;
delta_T_prn_samples=fmod((float)acq_trk_diff_samples,T_prn_mod_samples);
delay_correction_samples=T_prn_mod_samples-delta_T_prn_samples;
d_acq_code_phase_samples=d_acq_code_phase_samples-delay_correction_samples;
if (d_acq_code_phase_samples<0){
d_acq_code_phase_samples=d_acq_code_phase_samples+T_prn_mod_samples;
}
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in;
float T_prn_diff_seconds;
T_prn_diff_seconds=T_prn_true_seconds-T_prn_mod_seconds;
float N_prn_diff;
N_prn_diff=acq_trk_diff_seconds/T_prn_true_seconds;
float corrected_acq_phase_samples,delay_correction_samples;
corrected_acq_phase_samples=fmod((d_acq_code_phase_samples+T_prn_diff_seconds*N_prn_diff*(float)d_fs_in),T_prn_true_samples);
if (corrected_acq_phase_samples<0)
{
corrected_acq_phase_samples=T_prn_mod_samples+corrected_acq_phase_samples;
}
delay_correction_samples=d_acq_code_phase_samples-corrected_acq_phase_samples;
d_acq_code_phase_samples=corrected_acq_phase_samples;
d_carrier_doppler_hz=d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz);
@ -168,24 +182,8 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
d_next_rem_code_phase_samples=0;
d_acc_carrier_phase_rad=0;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename="track_ch"; //base path and name for the tracking log file
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
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<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
d_code_phase_samples = d_acq_code_phase_samples;
// DEBUG OUTPUT
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl;
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
@ -194,7 +192,7 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
d_pull_in=true;
d_enable_tracking=true;
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" PULL-IN Code Phase [chips]= "<<d_acq_code_phase_samples<<"\r\n";
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" Code Phase correction [samples]="<<delay_correction_samples<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
}
void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_code()
@ -218,12 +216,13 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_code()
d_late_code[i] = d_ca_code[associated_chip_index];
tcode_chips=tcode_chips+code_phase_step_chips;
}
//d_code_phase_samples=d_code_phase_samples+(float)d_fs_in*GPS_L1_CA_CODE_LENGTH_CHIPS*(1/d_code_freq_hz-1/GPS_L1_CA_CODE_RATE_HZ);
}
void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_carrier()
{
float phase, phase_step;
phase_step = (float)TWO_PI*d_carrier_doppler_hz/d_fs_in;
phase_step = (float)TWO_PI*d_carrier_doppler_hz/(float)d_fs_in;
phase=d_rem_carr_phase;
for(int i = 0; i < d_current_prn_length_samples; i++) {
d_carr_sign[i] = gr_complex(cos(phase),sin(phase));
@ -235,12 +234,12 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_carrier()
gps_l1_ca_dll_fll_pll_tracking_cc::~gps_l1_ca_dll_fll_pll_tracking_cc() {
d_dump_file.close();
delete d_ca_code;
delete d_early_code;
delete d_prompt_code;
delete d_late_code;
delete d_carr_sign;
delete d_Prompt_buffer;
delete[] d_ca_code;
delete[] d_early_code;
delete[] d_prompt_code;
delete[] d_late_code;
delete[] d_carr_sign;
delete[] d_Prompt_buffer;
}
/*! Tracking signal processing
@ -250,23 +249,63 @@ gps_l1_ca_dll_fll_pll_tracking_cc::~gps_l1_ca_dll_fll_pll_tracking_cc() {
int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
// if ((unsigned int)ninput_items[0]<(d_vector_length*2))
// {
// std::cout<<"End of signal detected\r\n";
// const int samples_available = ninput_items[0];
// consume_each(samples_available);
// return 0;
// }
// process vars
float code_error_chips=0;
float correlation_time_s=0;
float PLL_discriminator_hz=0;
float carr_nco_hz=0;
d_Prompt_prev=d_Prompt; // for the FLL discriminator
d_Early=gr_complex(0,0);
d_Prompt=gr_complex(0,0);
d_Late=gr_complex(0,0);
if (d_enable_tracking==true){
/*!
* Receiver signal alignment
*/
if (d_pull_in==true)
{
int samples_offset=round(d_acq_code_phase_samples);
d_sample_counter+=samples_offset; //count for the processed samples
int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples=d_sample_counter-d_acq_sample_stamp;
acq_trk_shif_correction_samples=d_next_prn_length_samples-fmod((float)acq_to_trk_delay_samples,(float)d_next_prn_length_samples);
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
samples_offset=round(d_acq_code_phase_samples+acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset)/(double)d_fs_in);
d_sample_counter=d_sample_counter+samples_offset; //count for the processed samples
d_pull_in=false;
std::cout<<" samples_offset "<<samples_offset<<"\r\n";
//std::cout<<" samples_offset="<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica
return 1;
}
// get the sample in and out pointers
const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer
float **out = (float **) &output_items[0]; //block output streams pointer
double **out = (double **) &output_items[0]; //block output streams pointer
// check for samples consistency
for(int i=0;i<d_current_prn_length_samples;i++) {
if (std::isnan(in[i].real())==true or std::isnan(in[i].imag())==true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{
const int samples_available= ninput_items[0];
d_sample_counter=d_sample_counter+samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<<d_sample_counter;
consume_each(samples_available);
return 0;
}
}
// Update the prn length based on code freq (variable) and
// sampling frequency (fixed)
// variable code PRN sample block size
@ -277,11 +316,6 @@ int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vecto
gr_complex bb_signal_sample(0,0);
d_Prompt_prev=d_Prompt; // for the FLL discriminator
d_Early=gr_complex(0,0);
d_Prompt=gr_complex(0,0);
d_Late=gr_complex(0,0);
// perform Early, Prompt and Late correlation
/*!
* \todo Use SIMD-enabled correlators
@ -299,11 +333,9 @@ int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vecto
* DLL, FLL, and PLL discriminators
*/
// Compute DLL error
float code_error_chips;
code_error_chips=dll_nc_e_minus_l_normalized(d_Early,d_Late);
//compute FLL error
float correlation_time_s;
correlation_time_s=((float)d_current_prn_length_samples)/(float)d_fs_in;
if (d_FLL_wait==1)
{
@ -316,9 +348,7 @@ int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vecto
}
// Compute PLL error
float PLL_discriminator_hz;
PLL_discriminator_hz=pll_cloop_two_quadrant_atan(d_Prompt)/(float)TWO_PI;
//PLL_discriminator_hz=pll_four_quadrant_atan(d_Prompt)/(float)TWO_PI;
/*!
* \todo Update FLL assistance algorithm!
@ -330,11 +360,13 @@ int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vecto
/*!
* DLL and FLL+PLL filter and get current carrier Doppler and code frequency
*/
float carr_nco_hz;
carr_nco_hz=d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz,PLL_discriminator_hz,correlation_time_s);
d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz;
d_code_freq_hz= GPS_L1_CA_CODE_RATE_HZ- (((d_carrier_doppler_hz - (float)d_if_freq)*GPS_L1_CA_CODE_RATE_HZ)/GPS_L1_FREQ_HZ)-code_error_chips;
/*!
* \todo Improve the lock detection algorithm!
*/
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
{
@ -347,13 +379,13 @@ int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vecto
d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
// ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message;
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>MINIMUM_VALID_CN0)
{
d_carrier_lock_fail_counter++;
}else{
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter>300)
if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
tracking_message=3; //loss of lock
@ -365,67 +397,20 @@ int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vecto
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
}
/*!
* \todo Output the CN0
*/
// ########### Output the tracking data to navigation and PVT ##########
// Output channel 1: Prompt correlator output Q
*out[0]=d_Early.real();
// Output channel 2: Prompt correlator output I
*out[1]=d_Early.imag();
// Output channel 3: PRN absolute delay [ms]
*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
// Output channel 4: PRN code error [ms]
*out[3]=d_acc_carrier_phase_rad;
if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I;
float prompt_Q;
float tmp_E,tmp_P,tmp_L;
float tmp_float;
prompt_I=d_Prompt.imag();
prompt_Q=d_Prompt.real();
tmp_E=std::abs<float>(d_Early);
tmp_P=std::abs<float>(d_Prompt);
tmp_L=std::abs<float>(d_Late);
try {
// EPR
d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float));
// PRN start sample stamp
tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&tmp_float, sizeof(float));
// accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
// carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
//PLL commands
d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float));
d_dump_file.write((char*)&carr_nco_hz, sizeof(float));
//DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float));
d_dump_file.write((char*)&code_error_chips, sizeof(float));
// CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes)
tmp_float=d_FLL_discriminator_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=0.0;
d_dump_file.write((char*)&tmp_float, sizeof(float));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
}
}
// Output channel 0: Prompt correlator output Q
*out[0]=(double)d_Prompt.real();
// Output channel 1: Prompt correlator output I
*out[1]=(double)d_Prompt.imag();
// Output channel 2: PRN absolute delay [s]
*out[2]=d_sample_counter_seconds;
// Output channel 3: d_acc_carrier_phase_rad [rad]
*out[3]=(double)d_acc_carrier_phase_rad;
// Output channel 4: PRN code phase [s]
*out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in);
// ########## DEBUG OUTPUT
/*!
@ -437,50 +422,115 @@ int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vecto
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"t="<<d_last_seg<<std::endl;
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
std::cout<<"Current input signal time="<<d_last_seg<<" [s]"<<std::endl;
std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
}else
{
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
std::cout<<"Tracking CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
//predict the next loop PRN period length prediction
//float T_chip_seconds,T_prn_seconds,T_prn_samples;
//T_chip_seconds=1/d_code_freq_hz;
//T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS-d_rem_code_phase_chips*T_chip_seconds;
//T_prn_samples=T_prn_seconds*(float)d_fs_in;
//d_next_prn_length_samples=round(T_prn_samples);
float T_chip_seconds;
float T_prn_seconds;
float T_prn_samples;
float K_blk_samples;
T_chip_seconds=1/d_code_freq_hz;
T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples=T_prn_seconds*d_fs_in;
T_prn_samples=T_prn_seconds*(float)d_fs_in;
d_rem_code_phase_samples=d_next_rem_code_phase_samples;
K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
d_next_prn_length_samples=round(K_blk_samples);
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples;
// Update the current PRN delay (code phase in samples)
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in;
d_code_phase_samples=d_code_phase_samples+T_prn_samples-T_prn_true_samples;
if (d_code_phase_samples<0)
{
d_code_phase_samples=T_prn_true_samples+d_code_phase_samples;
}
d_code_phase_samples=fmod(d_code_phase_samples,T_prn_true_samples);
d_next_prn_length_samples=round(K_blk_samples);//round to a discrete samples
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples; //rounding error
}else{
double **out = (double **) &output_items[0]; //block output streams pointer
*out[0]=0;
*out[1]=0;
*out[2]=0;
*out[3]=0;
*out[4]=0;
}
if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I;
float prompt_Q;
float tmp_E,tmp_P,tmp_L;
float tmp_float;
prompt_I=d_Prompt.imag();
prompt_Q=d_Prompt.real();
tmp_E=std::abs<float>(d_Early);
tmp_P=std::abs<float>(d_Prompt);
tmp_L=std::abs<float>(d_Late);
try {
// EPR
d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float));
// PRN start sample stamp
//tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
// carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
//PLL commands
d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float));
d_dump_file.write((char*)&carr_nco_hz, sizeof(float));
//DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float));
d_dump_file.write((char*)&d_code_phase_samples, sizeof(float));
// CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes)
tmp_float=0;
d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
}
}
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter_seconds = d_sample_counter_seconds + (((double)d_current_prn_length_samples)/(double)d_fs_in);
d_sample_counter+=d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
d_acq_code_phase_samples = code_phase;
d_acq_code_phase_samples=code_phase;
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
}
@ -497,6 +547,23 @@ void gps_l1_ca_dll_fll_pll_tracking_cc::set_satellite(unsigned int satellite) {
void gps_l1_ca_dll_fll_pll_tracking_cc::set_channel(unsigned int channel) {
d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
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<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)

6
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h
View File

@ -62,6 +62,7 @@ gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite,
unsigned int vector_length,
gr_msg_queue_sptr queue,
bool dump,
std::string dump_filename,
int order,
float fll_bw_hz,
float pll_bw_hz,
@ -81,6 +82,7 @@ private:
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
std::string dump_filename,
int order,
float fll_bw_hz,
float pll_bw_hz,
@ -93,6 +95,7 @@ private:
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
std::string dump_filename,
int order,
float fll_bw_hz,
float pll_bw_hz,
@ -130,6 +133,7 @@ private:
float d_carrier_doppler_hz;
float d_code_freq_hz;
float d_code_phase_samples;
int d_current_prn_length_samples;
int d_next_prn_length_samples;
int d_FLL_wait;
@ -148,6 +152,8 @@ private:
float d_acc_carrier_phase_rad;
unsigned long int d_sample_counter;
double d_sample_counter_seconds;
unsigned long int d_acq_sample_stamp;
// CN0 estimation and lock detector

287
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

@ -55,15 +55,18 @@
* \todo Include in definition header file
*/
#define CN0_ESTIMATION_SAMPLES 10
#define MINIMUM_VALID_CN0 25
#define MAXIMUM_LOCK_FAIL_COUNTER 200
using google::LogMessage;
gps_l1_ca_dll_pll_tracking_cc_sptr
gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
return gps_l1_ca_dll_pll_tracking_cc_sptr(new gps_l1_ca_dll_pll_tracking_cc(satellite, if_freq,
fs_in, vector_length, queue, dump, pll_bw_hz, dll_bw_hz, early_late_space_chips));
fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips));
}
void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
@ -72,9 +75,9 @@ void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
}
gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
int vector_length, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
gr_block ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
gr_make_io_signature(5, 5, sizeof(float))) {
gr_make_io_signature(5, 5, sizeof(double))) {
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
@ -85,8 +88,7 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
d_if_freq = if_freq;
d_fs_in = fs_in;
d_vector_length = vector_length;
//std::cout<<"pll_bw_hz= "<<pll_bw_hz<<"dll_bw_hz="<<dll_bw_hz<<"\r\n";
d_dump_filename =dump_filename;
// Initialize tracking ==========================================
@ -118,6 +120,7 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
// sample synchronization
d_sample_counter=0;
d_sample_counter_seconds=0;
d_acq_sample_stamp=0;
d_enable_tracking=false;
@ -141,8 +144,9 @@ void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
*/
unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp-d_vector_length;
acq_trk_diff_seconds=acq_trk_diff_samples/(float)d_fs_in;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp;//-d_vector_length;
std::cout<<"acq_trk_diff_samples="<<acq_trk_diff_samples<<"\r\n";
acq_trk_diff_seconds=(float)acq_trk_diff_samples/(float)d_fs_in;
//doppler effect
// Fd=(C/(C+Vr))*F
float radial_velocity;
@ -155,18 +159,25 @@ void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
T_chip_mod_seconds=1/d_code_freq_hz;
T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips]
d_next_prn_length_samples=round(T_prn_mod_samples);
//compute the code phase chips prediction
float delta_T_prn_samples;
float delay_correction_samples;
delta_T_prn_samples=fmod((float)acq_trk_diff_samples,T_prn_mod_samples);
delay_correction_samples=T_prn_mod_samples-delta_T_prn_samples;
d_acq_code_phase_samples=d_acq_code_phase_samples-delay_correction_samples;
if (d_acq_code_phase_samples<0){
d_acq_code_phase_samples=d_acq_code_phase_samples+T_prn_mod_samples;
}
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in;
float T_prn_diff_seconds;
T_prn_diff_seconds=T_prn_true_seconds-T_prn_mod_seconds;
float N_prn_diff;
N_prn_diff=acq_trk_diff_seconds/T_prn_true_seconds;
float corrected_acq_phase_samples,delay_correction_samples;
corrected_acq_phase_samples=fmod((d_acq_code_phase_samples+T_prn_diff_seconds*N_prn_diff*(float)d_fs_in),T_prn_true_samples);
if (corrected_acq_phase_samples<0)
{
corrected_acq_phase_samples=T_prn_mod_samples+corrected_acq_phase_samples;
}
delay_correction_samples=d_acq_code_phase_samples-corrected_acq_phase_samples;
d_acq_code_phase_samples=corrected_acq_phase_samples;
d_carrier_doppler_hz=d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_carrier_doppler_hz); //initialize the carrier filter
@ -179,35 +190,23 @@ void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
d_carrier_lock_fail_counter=0;
d_rem_code_phase_samples=0;
d_next_rem_code_phase_samples=0;
d_rem_carr_phase_rad=0;
d_rem_code_phase_samples=0;
d_next_rem_code_phase_samples=0;
d_acc_carrier_phase_rad=0;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename="track_ch"; //base path and name for the tracking log file
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
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<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
d_code_phase_samples = d_acq_code_phase_samples;
// DEBUG OUTPUT
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl;
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
// enable tracking
d_pull_in=true;
d_enable_tracking=true;
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" Code Phase correction [samples]="<<delay_correction_samples<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
}
void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
@ -235,7 +234,7 @@ void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
{
float phase_rad, phase_step_rad;
phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz/d_fs_in;
phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz/(float)d_fs_in;
phase_rad=d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++) {
d_carr_sign[i] = gr_complex(cos(phase_rad),sin(phase_rad));
@ -247,12 +246,12 @@ void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
d_dump_file.close();
delete d_ca_code;
delete d_early_code;
delete d_prompt_code;
delete d_late_code;
delete d_carr_sign;
delete d_Prompt_buffer;
delete[] d_ca_code;
delete[] d_early_code;
delete[] d_prompt_code;
delete[] d_late_code;
delete[] d_carr_sign;
delete[] d_Prompt_buffer;
}
/*! Tracking signal processing
@ -261,33 +260,71 @@ gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
// if ((unsigned int)ninput_items[0]<(d_vector_length*2))
// {
// std::cout<<"End of signal detected\r\n";
// const int samples_available = ninput_items[0];
// consume_each(samples_available);
// return 0;
// }
// process vars
float carr_error;
float carr_nco;
float code_error;
float code_nco;
d_Early=gr_complex(0,0);
d_Prompt=gr_complex(0,0);
d_Late=gr_complex(0,0);
if (d_enable_tracking==true){
/*!
* Receiver signal alignment
*/
if (d_pull_in==true)
{
int samples_offset=ceil(d_acq_code_phase_samples);
consume_each(d_acq_code_phase_samples); //shift input to perform alignement with local replica
d_sample_counter+=samples_offset; //count for the processed samples
int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples=d_sample_counter-d_acq_sample_stamp;
acq_trk_shif_correction_samples=d_next_prn_length_samples-fmod((float)acq_to_trk_delay_samples,(float)d_next_prn_length_samples);
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
samples_offset=round(d_acq_code_phase_samples+acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset)/(double)d_fs_in);
d_sample_counter=d_sample_counter+samples_offset; //count for the processed samples
d_pull_in=false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica
return 1;
}
d_current_prn_length_samples=d_next_prn_length_samples;
float carr_error;
float carr_nco;
float code_error;
float code_nco;
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
float **out = (float **) &output_items[0];
double **out = (double **) &output_items[0];
// check for samples consistency
for(int i=0;i<d_current_prn_length_samples;i++) {
if (std::isnan(in[i].real())==true or std::isnan(in[i].imag())==true)// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{
const int samples_available= ninput_items[0];
d_sample_counter=d_sample_counter+samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number "<<d_sample_counter;
consume_each(samples_available);
return 0;
}
}
// Update the prn length based on code freq (variable) and
// sampling frequency (fixed)
// variable code PRN sample block size
d_current_prn_length_samples=d_next_prn_length_samples;
update_local_code();
update_local_carrier();
gr_complex bb_signal_sample(0,0);
d_Early=gr_complex(0,0);
d_Prompt=gr_complex(0,0);
d_Late=gr_complex(0,0);
// perform Early, Prompt and Late correlation
/*!
@ -328,8 +365,20 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
T_prn_samples=T_prn_seconds*d_fs_in;
d_rem_code_phase_samples=d_next_rem_code_phase_samples;
K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
d_next_prn_length_samples=round(K_blk_samples);
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples;
// Update the current PRN delay (code phase in samples)
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS/GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds*(float)d_fs_in;
d_code_phase_samples=d_code_phase_samples+T_prn_samples-T_prn_true_samples;
if (d_code_phase_samples<0)
{
d_code_phase_samples=T_prn_true_samples+d_code_phase_samples;
}
d_code_phase_samples=fmod(d_code_phase_samples,T_prn_true_samples);
d_next_prn_length_samples=round(K_blk_samples); //round to a discrete samples
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples; //rounding error
/*!
* \todo Improve the lock detection algorithm!
@ -345,35 +394,72 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in);
d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
// ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message;
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>MINIMUM_VALID_CN0)
{
d_carrier_lock_fail_counter++;
}else{
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter>200)
if (d_carrier_lock_fail_counter>MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
tracking_message=3; //loss of lock
d_channel_internal_queue->push(tracking_message);
d_carrier_lock_fail_counter=0;
d_current_prn_length_samples=(int)d_vector_length; //original dsp block length
d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
}
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
}
// Output the tracking data to navigation and PVT
// Output channel 1: Prompt correlator output Q
*out[0]=d_Prompt.real();
// Output channel 2: Prompt correlator output I
*out[1]=d_Prompt.imag();
// Output channel 3: Current tracking time [ms]
*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
// Output channel 4: Carrier accumulated phase
*out[3]=d_acc_carrier_phase_rad;
/*!
* \todo Output the CN0
*/
// ########### Output the tracking data to navigation and PVT ##########
// Output channel 0: Prompt correlator output Q
*out[0]=(double)d_Prompt.real();
// Output channel 1: Prompt correlator output I
*out[1]=(double)d_Prompt.imag();
// Output channel 2: PRN absolute delay [s]
*out[2]=d_sample_counter_seconds;
// Output channel 3: d_acc_carrier_phase_rad [rad]
*out[3]=(double)d_acc_carrier_phase_rad;
// Output channel 4: PRN code phase [s]
*out[4]=(double)d_code_phase_samples*(1/(float)d_fs_in);
// ########## DEBUG OUTPUT
/*!
* \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<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
}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<<" CN0="<<d_CN0_SNV_dB_Hz<<" [dB-Hz]"<<std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
}else{
double **out = (double **) &output_items[0]; //block output streams pointer
*out[0]=0;
*out[1]=0;
*out[2]=0;
*out[3]=0;
*out[4]=0;
}
if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file
@ -395,8 +481,8 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
d_dump_file.write((char*)&prompt_I, sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float));
// PRN start sample stamp
tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&tmp_float, sizeof(float));
//tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
@ -417,38 +503,18 @@ int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_in
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes)
tmp_float=0.0;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=0.0;
tmp_float=0;
d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
}
}
// ########## DEBUG OUTPUT
// 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<<"t="<<d_last_seg<<std::endl;
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< 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<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
}
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter+=d_current_prn_length_samples; //count for the processed samples
d_sample_counter_seconds = d_sample_counter_seconds + (((double)d_current_prn_length_samples)/(double)d_fs_in);
d_sample_counter+=d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}
@ -470,6 +536,23 @@ void gps_l1_ca_dll_pll_tracking_cc::set_satellite(unsigned int satellite) {
void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel) {
d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
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<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
}
void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)

13
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
View File

@ -42,8 +42,8 @@
//#include <gnuradio/gr_sync_decimator.h>
#include "gps_sdr_signal_processing.h"
#include "tracking_2rd_DLL_filter.h"
#include "tracking_2rd_PLL_filter.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
#include <queue>
#include <boost/thread/mutex.hpp>
@ -60,6 +60,7 @@ gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq,
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
std::string dump_filename,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
@ -76,6 +77,7 @@ private:
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
std::string dump_filename,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
@ -85,6 +87,7 @@ private:
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
std::string dump_filename,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
@ -123,8 +126,8 @@ private:
float d_rem_carr_phase_rad;
// PLL and DLL filter library
tracking_2rd_DLL_filter d_code_loop_filter;
tracking_2rd_PLL_filter d_carrier_loop_filter;
tracking_2nd_DLL_filter d_code_loop_filter;
tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
float d_acq_code_phase_samples;
@ -134,10 +137,12 @@ private:
float d_code_freq_hz;
float d_carrier_doppler_hz;
float d_acc_carrier_phase_rad;
float d_code_phase_samples;
//PRN period in samples
int d_current_prn_length_samples;
int d_next_prn_length_samples;
double d_sample_counter_seconds;
//processing samples counters
unsigned long int d_sample_counter;

4
src/algorithms/tracking/libs/jamfile.jam
View File

@ -3,5 +3,5 @@ project : build-dir ../../../../build ;
obj tracking_discriminators : tracking_discriminators.cc ;
obj CN_estimators : CN_estimators.cc ;
obj tracking_FLL_PLL_filter : tracking_FLL_PLL_filter.cc ;
obj tracking_2rd_PLL_filter : tracking_2rd_PLL_filter.cc ;
obj tracking_2rd_DLL_filter : tracking_2rd_DLL_filter.cc ;
obj tracking_2nd_PLL_filter : tracking_2nd_PLL_filter.cc ;
obj tracking_2nd_DLL_filter : tracking_2nd_DLL_filter.cc ;

16
src/algorithms/tracking/libs/tracking_2rd_DLL_filter.cc → src/algorithms/tracking/libs/tracking_2nd_DLL_filter.cc
View File

@ -1,5 +1,5 @@
/*!
* \file tracking_2rd_DLL_filter.cc
* \file tracking_2nd_DLL_filter.cc
* \brief Class that implements 2 order DLL filter for code tracking loop.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
@ -34,10 +34,10 @@
*/
#include "tracking_2rd_DLL_filter.h"
#include "tracking_2nd_DLL_filter.h"
void tracking_2rd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
void tracking_2nd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
// Solve natural frequency
float Wn;
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
@ -46,13 +46,13 @@ void tracking_2rd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float
*tau2 = (2.0 * zeta) / Wn;
}
void tracking_2rd_DLL_filter::set_DLL_BW(float dll_bw_hz)
void tracking_2nd_DLL_filter::set_DLL_BW(float dll_bw_hz)
{
//Calculate filter coefficient values
d_dllnoisebandwidth=dll_bw_hz;
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values
}
void tracking_2rd_DLL_filter::initialize(float d_acq_code_phase_samples)
void tracking_2nd_DLL_filter::initialize(float d_acq_code_phase_samples)
{
// code tracking loop parameters
d_old_code_nco = 0.0;
@ -60,7 +60,7 @@ void tracking_2rd_DLL_filter::initialize(float d_acq_code_phase_samples)
}
float tracking_2rd_DLL_filter::get_code_nco(float DLL_discriminator)
float tracking_2nd_DLL_filter::get_code_nco(float DLL_discriminator)
{
float code_nco;
code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(DLL_discriminator - d_old_code_error) + DLL_discriminator * (d_pdi_code/d_tau1_code);
@ -69,13 +69,13 @@ float tracking_2rd_DLL_filter::get_code_nco(float DLL_discriminator)
return code_nco;
}
tracking_2rd_DLL_filter::tracking_2rd_DLL_filter ()
tracking_2nd_DLL_filter::tracking_2nd_DLL_filter ()
{
d_pdi_code = 0.001;// Summation interval for code
d_dlldampingratio=0.7;
}
tracking_2rd_DLL_filter::~tracking_2rd_DLL_filter ()
tracking_2nd_DLL_filter::~tracking_2nd_DLL_filter ()
{
}

12
src/algorithms/tracking/libs/tracking_2rd_DLL_filter.h → src/algorithms/tracking/libs/tracking_2nd_DLL_filter.h
View File

@ -1,5 +1,5 @@
/*!
* \file tracking_2rd_DLL_filter.h
* \file tracking_2nd_DLL_filter.h
* \brief Class that implements 2 order DLL filter for code tracking loop.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
@ -33,10 +33,10 @@
* -------------------------------------------------------------------------
*/
#ifndef TRACKING_2RD_DLL_FILTER_H_
#define TRACKING_2RD_DLL_FILTER_H_
#ifndef TRACKING_2ND_DLL_FILTER_H_
#define TRACKING_2ND_DLL_FILTER_H_
class tracking_2rd_DLL_filter
class tracking_2nd_DLL_filter
{
private:
// PLL filter parameters
@ -54,8 +54,8 @@ public:
void set_DLL_BW(float dll_bw_hz);
void initialize(float d_acq_code_phase_samples);
float get_code_nco(float DLL_discriminator);
tracking_2rd_DLL_filter();
~tracking_2rd_DLL_filter();
tracking_2nd_DLL_filter();
~tracking_2nd_DLL_filter();
};
#endif

16
src/algorithms/tracking/libs/tracking_2rd_PLL_filter.cc → src/algorithms/tracking/libs/tracking_2nd_PLL_filter.cc
View File

@ -1,5 +1,5 @@
/*!
* \file tracking_2rd_PLL_filter.cc
* \file tracking_2nd_PLL_filter.cc
* \brief Class that implements 2 order PLL filter for tracking carrier loop.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
@ -33,10 +33,10 @@
* -------------------------------------------------------------------------
*/
#include "tracking_2rd_PLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
void tracking_2rd_PLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
void tracking_2nd_PLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
// Solve natural frequency
float Wn;
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
@ -45,20 +45,20 @@ void tracking_2rd_PLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float
*tau2 = (2.0 * zeta) / Wn;
}
void tracking_2rd_PLL_filter::set_PLL_BW(float pll_bw_hz)
void tracking_2nd_PLL_filter::set_PLL_BW(float pll_bw_hz)
{
//Calculate filter coefficient values
d_pllnoisebandwidth=pll_bw_hz;
calculate_lopp_coef(&d_tau1_carr, &d_tau2_carr, d_pllnoisebandwidth, d_plldampingratio,0.25);// Calculate filter coefficient values
}
void tracking_2rd_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
void tracking_2nd_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
{
// carrier/Costas loop parameters
d_old_carr_nco = 0.0;
d_old_carr_error = 0.0;
}
float tracking_2rd_PLL_filter::get_carrier_nco(float PLL_discriminator)
float tracking_2nd_PLL_filter::get_carrier_nco(float PLL_discriminator)
{
float carr_nco;
carr_nco = d_old_carr_nco+(d_tau2_carr/d_tau1_carr)*(PLL_discriminator - d_old_carr_error) + PLL_discriminator * (d_pdi_carr/d_tau1_carr);
@ -67,14 +67,14 @@ float tracking_2rd_PLL_filter::get_carrier_nco(float PLL_discriminator)
return carr_nco;
}
tracking_2rd_PLL_filter::tracking_2rd_PLL_filter ()
tracking_2nd_PLL_filter::tracking_2nd_PLL_filter ()
{
//--- PLL variables --------------------------------------------------------
d_pdi_carr = 0.001;// Summation interval for carrier
d_plldampingratio=0.65;
}
tracking_2rd_PLL_filter::~tracking_2rd_PLL_filter ()
tracking_2nd_PLL_filter::~tracking_2nd_PLL_filter ()
{
}

12
src/algorithms/tracking/libs/tracking_2rd_PLL_filter.h → src/algorithms/tracking/libs/tracking_2nd_PLL_filter.h
View File

@ -1,5 +1,5 @@
/*!
* \file tracking_2rd_PLL_filter.h
* \file tracking_2nd_PLL_filter.h
* \brief Class that implements 2 order PLL filter for tracking carrier loop
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
@ -33,10 +33,10 @@
* -------------------------------------------------------------------------
*/
#ifndef TRACKING_2RD_PLL_FILTER_H_
#define TRACKING_2RD_PLL_FILTER_H_
#ifndef TRACKING_2ND_PLL_FILTER_H_
#define TRACKING_2ND_PLL_FILTER_H_
class tracking_2rd_PLL_filter
class tracking_2nd_PLL_filter
{
private:
// PLL filter parameters
@ -55,8 +55,8 @@ public:
void set_PLL_BW(float pll_bw_hz);
void initialize(float d_acq_carrier_doppler_hz);
float get_carrier_nco(float PLL_discriminator);
tracking_2rd_PLL_filter();
~tracking_2rd_PLL_filter();
tracking_2nd_PLL_filter();
~tracking_2nd_PLL_filter();
};
#endif

1
src/algorithms/tracking/libs/tracking_FLL_PLL_filter.cc
View File

@ -76,7 +76,6 @@ void tracking_FLL_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
d_pll_w = d_acq_carrier_doppler_hz;
d_pll_x = 0;
}
std::cout<<" d_pll_x init = "<<d_pll_x<<"\r\n";
}
float tracking_FLL_PLL_filter::get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s)

59
src/core/interfaces/pvt_interface.h Normal file
View File

@ -0,0 +1,59 @@
/*!
* \file pvt_interface.h
* \brief This class represents an interface to a PVT gnss block.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Abstract class for pseudorange_intefaces. Since all its methods are virtual,
* this class cannot be instantiated directly, and a subclass can only be
* instantiated directly if all inherited pure virtual methods have been
* implemented by that class or a parent class.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_PVT_INTERFACE_H_
#define GNSS_SDR_PVT_INTERFACE_H_
#include "gnss_block_interface.h"
/*!
* \brief This class represents an interface to a PVT gnss block.
*
* Abstract class for PVT intefaces, derived from GNSSBlockInterface.
* Since all its methods are virtual,
* this class cannot be instantiated directly, and a subclass can only be
* instantiated directly if all inherited pure virtual methods have been
* implemented by that class or a parent class.
*/
class PvtInterface : public GNSSBlockInterface
{
public:
virtual void reset() = 0;
};
#endif /* GNSS_SDR_PVT_INTERFACE_H_ */

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

@ -47,7 +47,7 @@
using google::LogMessage;
DEFINE_string(config_file, "../conf/mercurio.conf",
DEFINE_string(config_file, "../conf/gnss-sdr.conf",
"Path to the file containing the configuration parameters")
;

2
src/core/receiver/file_configuration.cc
View File

@ -76,7 +76,7 @@ std::string FileConfiguration::property(std::string property_name, std::string d
}
else
{
return ini_reader_->Get("mercurio", property_name, default_value);
return ini_reader_->Get("GNSS-SDR", property_name, default_value);
}
}

13
src/core/receiver/gnss_block_factory.cc
View File

@ -3,6 +3,7 @@
* \brief This class implements a factory that returns instances of GNSS blocks.
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
* Luis Esteve, 2011. luis(at)epsilon-formacion.com
* Javier Arribas, 2011. jarribas(at)cttc.es
*
* This class encapsulates the complexity behind the instantiation
* of GNSS blocks.
@ -58,6 +59,7 @@
#include "gps_l1_ca_dll_fll_pll_tracking.h"
#include "gps_l1_ca_telemetry_decoder.h"
#include "gps_l1_ca_observables.h"
#include "gps_l1_ca_pvt.h"
using google::LogMessage;
@ -195,9 +197,9 @@ std::vector<GNSSBlockInterface*>* GNSSBlockFactory::GetChannels(
channels->push_back(GetChannel(configuration,
acquisition_implementation, tracking, telemetry_decoder, i,
queue));
std::cout << "getchannel_" << i << ", acq_implementation_name: "
<< acquisition_implementation_name << ", implementation: "
<< acquisition_implementation << std::endl;
//std::cout << "getchannel_" << i << ", acq_implementation_name: "
//<< acquisition_implementation_name << ", implementation: "
//<< acquisition_implementation << std::endl;
}
@ -278,6 +280,11 @@ GNSSBlockInterface* GNSSBlockFactory::GetBlock(
block = new GpsL1CaObservables(configuration, role, in_streams,
out_streams, queue);
}
else if (implementation.compare("GPS_L1_CA_PVT") == 0)
{
block = new GpsL1CaPvt(configuration, role, in_streams,
out_streams, queue);
}
else
{
// Log fatal. This causes execution to stop.

2
src/core/receiver/gnss_block_factory.h
View File

@ -2,6 +2,8 @@
* \file gnss_block_factory.h
* \brief This class implements a factory that returns instances of GNSS blocks.
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
* Luis Esteve, 2011. luis(at)epsilon-formacion.com
* Javier Arribas, 2011. jarribas(at)cttc.es
*
* This class encapsulates the complexity behind the instantiation
* of GNSS blocks.

27
src/core/receiver/gnss_flowgraph.cc
View File

@ -253,11 +253,16 @@ void GNSSFlowgraph::connect()
DLOG(INFO) << "Channel " << i
<< " connected to observables and ready for acquisition";
}
// TODO: Enable the observables multichannel output connection to PVT when the PVT implementation is ready
/*!
* Enabled the observables multichannel output connection to PVT
*/
try
{
top_block_->connect(observables()->get_right_block(), 0,
pvt()->get_left_block(), 0);
for (unsigned int i = 0; i < channels_count_; i++)
{
top_block_->connect(observables()->get_right_block(), i,
pvt()->get_left_block(), i);
}
}
catch (std::exception& e)
{
@ -438,14 +443,14 @@ void GNSSFlowgraph::set_satellites_list()
}
}
std::cout << "Cola de satélites: ";
for (std::list<unsigned int>::iterator it =
available_GPS_satellites_IDs_->begin(); it
!= available_GPS_satellites_IDs_->end(); it++)
{
std::cout << *it << ", ";
}
std::cout << std::endl;
// std::cout << "Cola de satélites: ";
// for (std::list<unsigned int>::iterator it =
// available_GPS_satellites_IDs_->begin(); it
// != available_GPS_satellites_IDs_->end(); it++)
// {
// std::cout << *it << ", ";
// }
// std::cout << std::endl;
}

35
src/core/system_parameters/GPS_L1_CA.h
View File

@ -32,17 +32,18 @@
#ifndef GPS_DEFINES_H
#define GPS_DEFINES_H
#define NAVIGATION_OUTPUT_RATE_MS 600
#define NAVIGATION_SOLUTION_RATE_MS 1000
// GPS CONSTANTS
// JAVI: ADD SYSTEM PREFIX
// SEPARATE FILE GPS.H
const float GPS_C_m_s= 299792458.0; // The speed of light, [m/ms]
const float GPS_C_m_s= 299792458.0; // The speed of light, [m/s]
const float GPS_C_m_ms= 299792.4580; // The speed of light, [m/ms]
const float GPS_STARTOFFSET_ms= 68.802; //[ms] Initial sign. travel time
const float GPS_PI = 3.1415926535898; // Pi used in the GPS coordinate system
const double GPS_PI = 3.1415926535898; // Pi used in the GPS coordinate system
// carrier and code frequencies
const float GPS_L1_FREQ_HZ = 1.57542e9;
const float GPS_L2_FREQ_HZ = 1.22760e9;
@ -59,10 +60,19 @@ const double F = -4.442807633e-10; // Constant, [sec/(meter)^(1/2)
// NAVIGATION MESSAGE DEMODULATION AND DECODING
#define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1}
#define GPS_CA_PREAMBLE_LENGTH_BITS 8
#define GPS_CA_TELEMETRY_RATE_BITS_SECOND 50
#define GPS_WORD_LENGTH 4 // CRC + GPS WORD (-2 -1 0 ... 29) Bits = 4 bytes
#define GPS_SUBFRAME_LENGTH 40 // GPS_WORD_LENGTH x 10 = 40 bytes
#define GPS_SUBFRAME_BITS 300
#define GPS_WORD_BITS 30
/*!
* Maximum Time-Of-Arrival (TOA) difference between satellites for a receiveer operated on Earth surface is 20 ms, according to the GPS orbit model descrived in [1] Pag. 32.
* It should be taken into account to set the buffer size for the PRN start timestamp in the pseudorranges block.
*
* [1] J. Bao-Yen Tsui, Fundamentals of Global Positioning System Receivers. A Software Approach, John Wiley & Sons, Inc., Hoboken, NJ, 2n edition, 2005.
*/
#define MAX_TOA_DELAY_MS 20
#define num_of_slices(x) sizeof(x)/sizeof(bits_slice)
@ -85,13 +95,26 @@ typedef struct bits_slice{
typedef struct gnss_synchro
{
float preamble_delay_ms;
float prn_delay_ms;
double preamble_delay_ms;
double prn_delay_ms;
double preamble_code_phase_ms;
double preamble_code_phase_correction_ms;
int satellite_PRN;
int channel_ID;
bool valid_word;
bool flag_preamble;
} gnss_synchro;
/*! @ingroup GPS_DEFINES
* @brief Observables structure, used to feed the PVT block */
typedef struct gnss_pseudorange
{
double pseudorange_m;
double timestamp_ms;
int SV_ID;
bool valid;
} gnss_pseudorange;
/* Constants for scaling the ephemeris found in the data message
the format is the following: TWO_N5 -> 2^-5, TWO_P4 -> 2^4, PI_TWO_N43 -> Pi*2^-43, etc etc

36
src/core/system_parameters/gps_navigation_message.cc
View File

@ -233,27 +233,28 @@ void gps_navigation_message::satpos()
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
for (int ii = 1;ii<11;ii++)
//std::cout<<"d_e_eccentricity="<<d_e_eccentricity<<"\r\n";
for (int ii = 1;ii<20;ii++)
{
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old,2*GPS_PI);
//std::cout<<"dE="<<dE<<std::endl;
if (abs(dE) < 1E-12)
if (fabs(dE) < 1e-12)
{
//Necessary precision is reached, exit from the loop
//std::cout<<"Loop break at ii="<<ii<<"\r\n";
break;
}
}
// Reduce eccentric anomaly to between 0 and 360 deg
E = fmod((E + 2*GPS_PI),(2*GPS_PI));
// Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
// Calculate the true anomaly
nu = atan2(sqrt(1 - d_e_eccentricity*d_e_eccentricity) * sin(E), cos(E)-d_e_eccentricity);
double tmp_Y=sqrt(1.0 - d_e_eccentricity*d_e_eccentricity) * sin(E);
double tmp_X=cos(E)-d_e_eccentricity;
nu = atan2(tmp_Y, tmp_X);
// Compute angle phi
phi = nu + d_OMEGA;
@ -266,6 +267,8 @@ void gps_navigation_message::satpos()
// Correct radius
r = a * (1 - d_e_eccentricity*cos(E)) + d_Crc * cos(2*phi) + d_Crs * sin(2*phi);
// Correct inclination
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) +d_Cis * sin(2*phi);
@ -275,12 +278,19 @@ void gps_navigation_message::satpos()
Omega = fmod((Omega + 2*GPS_PI),(2*GPS_PI));
// debug
//std::cout<<"tk"<<tk<<std::endl;
//std::cout<<"E="<<E<<std::endl;
//std::cout<<"d_dtr="<<d_dtr<<std::endl;
//std::cout<<"nu="<<nu<<std::endl;
//std::cout<<"phi="<<phi<<std::endl;
//std::cout<<"u="<<u<<" r="<<r<<" Omega="<<Omega<<std::endl;
/*
if (this->d_channel_ID==0){
std::cout<<"tk"<<tk<<std::endl;
std::cout<<"E="<<E<<std::endl;
std::cout<<"d_dtr="<<d_dtr<<std::endl;
std::cout<<"nu="<<nu<<std::endl;
std::cout<<"phi="<<phi<<std::endl;
std::cout<<"u="<<u<<" r="<<r<<" Omega="<<Omega<<std::endl;
std::cout<<"i="<<i<<"\r\n";
std::cout<<"tmp_Y="<<tmp_Y<<"\r\n";
std::cout<<"tmp_X="<<tmp_X<<"\r\n";
}
*/
// --- Compute satellite coordinates ------------------------------------
d_satpos_X = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega);
@ -304,7 +314,7 @@ int gps_navigation_message::subframe_decoder(char *subframe)
int subframe_ID=0;
int SV_data_ID=0;
int SV_page=0;
double tmp_TOW;
//double tmp_TOW;
unsigned int gps_word;
// UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE

15
src/main/jamfile.jam
View File

@ -1,5 +1,5 @@
project : build-dir ../../build ;
exe mercurio : main.cc
exe gnss-sdr : main.cc
../algorithms/acquisition/adapters//gps_l1_ca_gps_sdr_acquisition
../algorithms/acquisition/adapters//gps_l1_ca_pcps_acquisition
../algorithms/acquisition/adapters//gps_l1_ca_tong_pcps_acquisition
@ -20,8 +20,9 @@ exe mercurio : main.cc
../algorithms/libs//gps_sdr_x86
../algorithms/observables/adapters//gps_l1_ca_observables
../algorithms/observables/gnuradio_blocks//gps_l1_ca_observables_cc
../algorithms/observables/libs//rinex_2_1_printer
../algorithms/observables/libs//gps_l1_ca_ls_pvt
../algorithms/PVT/libs//rinex_2_1_printer
../algorithms/PVT/libs//kml_printer
../algorithms/PVT/libs//gps_l1_ca_ls_pvt
../algorithms/output_filter/adapters//file_output_filter
../algorithms/output_filter/adapters//null_sink_output_filter
../algorithms/signal_source/adapters//file_signal_source
@ -29,6 +30,8 @@ exe mercurio : main.cc
../algorithms/telemetry_decoder/adapters//gps_l1_ca_telemetry_decoder
../algorithms/telemetry_decoder/gnuradio_blocks//gps_l1_ca_telemetry_decoder_cc
../algorithms/telemetry_decoder/libs//gps_l1_ca_subframe_fsm
../algorithms/PVT/adapters//gps_l1_ca_pvt
../algorithms/PVT/gnuradio_blocks//gps_l1_ca_pvt_cc
../algorithms/tracking/adapters//gps_l1_ca_dll_pll_tracking
../algorithms/tracking/adapters//gps_l1_ca_dll_fll_pll_tracking
../algorithms/tracking/gnuradio_blocks//gps_l1_ca_dll_pll_tracking_cc
@ -36,8 +39,8 @@ exe mercurio : main.cc
../algorithms/tracking/libs//tracking_discriminators
../algorithms/tracking/libs//CN_estimators
../algorithms/tracking/libs//tracking_FLL_PLL_filter
../algorithms/tracking/libs//tracking_2rd_PLL_filter
../algorithms/tracking/libs//tracking_2rd_DLL_filter
../algorithms/tracking/libs//tracking_2nd_PLL_filter
../algorithms/tracking/libs//tracking_2nd_DLL_filter
../core/libs//INIReader
../core/libs//ini
../core/libs//string_converter
@ -54,4 +57,4 @@ exe mercurio : main.cc
../..//gnuradio-core
../..//gnuradio-usrp ;
install ../../install : mercurio ;
install ../../install : gnss-sdr ;

2
src/main/main.cc
View File

@ -64,6 +64,7 @@ int main(int argc, char** argv)
+
"See COPYING file to see a copy of the General Public License\n \n");
std::cout<<"Initializing GNSS-SDR... Please wait"<<"\r\n";
google::SetUsageMessage(intro_help);
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
@ -74,5 +75,6 @@ int main(int argc, char** argv)
control_thread->run();
delete control_thread;
std::cout<<"GNSS-SDR program ended"<<"\r\n";
//google::ShutDownCommandLineFlags();
}

83
src/utils/matlab/gps_l1_ca_dll_fll_pll_plot_sample.m Normal file
View File

@ -0,0 +1,83 @@
% /*!
% * \file gps_l1_ca_dll_fll_pll_plot_sample.m
% * \brief Read GNSS-SDR Tracking dump binary file using the provided
% function and plot some internal variables
% * \author Javier Arribas, 2011. jarribas(at)cttc.es
% * -------------------------------------------------------------------------
% *
% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
% *
% * GNSS-SDR is a software defined Global Navigation
% * Satellite Systems receiver
% *
% * This file is part of GNSS-SDR.
% *
% * GNSS-SDR is free software: you can redistribute it and/or modify
% * it under the terms of the GNU General Public License as published by
% * the Free Software Foundation, either version 3 of the License, or
% * at your option) any later version.
% *
% * GNSS-SDR is distributed in the hope that it will be useful,
% * but WITHOUT ANY WARRANTY; without even the implied warranty of
% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% * GNU General Public License for more details.
% *
% * You should have received a copy of the GNU General Public License
% * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% *
% * -------------------------------------------------------------------------
% */
close all;
clear all;
samplingFreq = 64e6/16; %[Hz]
channels=6;
path='/home/javier/workspace/gnss-sdr/trunk/install/';
clear PRN_absolute_sample_start;
for N=1:1:channels
tracking_log_path=[path 'tracking_ch_' num2str(N-1) '.dat'];
GNSS_tracking(N)= gps_l1_ca_dll_fll_pll_read_tracking_dump(tracking_log_path);
end
% GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels
trackResults(N).status='T'; %fake track
trackResults(N).codeFreq=GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq=GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error_chips.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_phase_samples.';
trackResults(N).pllDiscr = GNSS_tracking(N).PLL_discriminator_hz.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P=GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P=GNSS_tracking(N).prompt_Q.';
trackResults(N).I_E= GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L =zeros(1,length(GNSS_tracking(N).E));
trackResults(N).PRN=N; %fake PRN
% Use original MATLAB tracking plot function
settings.numberOfChannels=channels;
settings.msToProcess=length(GNSS_tracking(N).E);
plotTracking(N,trackResults,settings)
end
% for N=1:1:channels
% figure;
% plot([GNSS_tracking(N).E,GNSS_tracking(N).P,GNSS_tracking(N).L],'-*');
% title(['Early, Prompt, and Late correlator absolute value output for channel ' num2str(N)']);
% figure;
% plot(GNSS_tracking(N).prompt_I,GNSS_tracking(N).prompt_Q,'+');
% title(['Navigation constellation plot for channel ' num2str(N)]);
% figure;
%
% plot(GNSS_tracking(N).prompt_Q,'r');
% hold on;
% plot(GNSS_tracking(N).prompt_I);
% title(['Navigation symbols I(red) Q(blue) for channel ' num2str(N)]);
% end

82
src/utils/matlab/gps_l1_ca_dll_pll_plot_sample.m Normal file
View File

@ -0,0 +1,82 @@
% /*!
% * \file gps_l1_ca_dll_pll_plot_sample.m
% * \brief Read GNSS-SDR Tracking dump binary file using the provided
% function and plot some internal variables
% * \author Javier Arribas, 2011. jarribas(at)cttc.es
% * -------------------------------------------------------------------------
% *
% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
% *
% * GNSS-SDR is a software defined Global Navigation
% * Satellite Systems receiver
% *
% * This file is part of GNSS-SDR.
% *
% * GNSS-SDR is free software: you can redistribute it and/or modify
% * it under the terms of the GNU General Public License as published by
% * the Free Software Foundation, either version 3 of the License, or
% * at your option) any later version.
% *
% * GNSS-SDR is distributed in the hope that it will be useful,
% * but WITHOUT ANY WARRANTY; without even the implied warranty of
% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% * GNU General Public License for more details.
% *
% * You should have received a copy of the GNU General Public License
% * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% *
% * -------------------------------------------------------------------------
% */
close all;
clear all;
samplingFreq = 64e6/16; %[Hz]
channels=6;
path='/home/javier/workspace/gnss-sdr/trunk/install/';
clear PRN_absolute_sample_start;
for N=1:1:channels
tracking_log_path=[path 'tracking_ch_' num2str(N-1) '.dat'];
GNSS_tracking(N)= gps_l1_ca_dll_pll_read_tracking_dump(tracking_log_path);
end
% GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels
trackResults(N).status='T'; %fake track
trackResults(N).codeFreq=GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq=GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P=GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P=GNSS_tracking(N).prompt_Q.';
trackResults(N).I_E= GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L =zeros(1,length(GNSS_tracking(N).E));
trackResults(N).PRN=N; %fake PRN
% Use original MATLAB tracking plot function
settings.numberOfChannels=channels;
settings.msToProcess=length(GNSS_tracking(N).E);
plotTracking(N,trackResults,settings)
end
% for N=1:1:channels
% figure;
% plot([GNSS_tracking(N).E,GNSS_tracking(N).P,GNSS_tracking(N).L],'-*');
% title(['Early, Prompt, and Late correlator absolute value output for channel ' num2str(N)']);
% figure;
% plot(GNSS_tracking(N).prompt_I,GNSS_tracking(N).prompt_Q,'+');
% title(['Navigation constellation plot for channel ' num2str(N)]);
% figure;
%
% plot(GNSS_tracking(N).prompt_Q,'r');
% hold on;
% plot(GNSS_tracking(N).prompt_I);
% title(['Navigation symbols I(red) Q(blue) for channel ' num2str(N)]);
% end

81
src/utils/matlab/gps_l1_ca_pvt_plot_sample_agilent_cap2.m Normal file
View File

@ -0,0 +1,81 @@
% /*!
% * \file gps_l1_ca_pvt_plot_sample.m
% * \brief Read GNSS-SDR PVT dump binary file using the provided
% function and plot some internal variables
% * \author Javier Arribas, 2011. jarribas(at)cttc.es
% * -------------------------------------------------------------------------
% *
% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
% *
% * GNSS-SDR is a software defined Global Navigation
% * Satellite Systems receiver
% *
% * This file is part of GNSS-SDR.
% *
% * GNSS-SDR is free software: you can redistribute it and/or modify
% * it under the terms of the GNU General Public License as published by
% * the Free Software Foundation, either version 3 of the License, or
% * at your option) any later version.
% *
% * GNSS-SDR is distributed in the hope that it will be useful,
% * but WITHOUT ANY WARRANTY; without even the implied warranty of
% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% * GNU General Public License for more details.
% *
% * You should have received a copy of the GNU General Public License
% * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% *
% * -------------------------------------------------------------------------
% */
close all;
clear all;
% True position of the antenna in UTM system (if known). Otherwise enter
% all NaN's and mean position will be used as a reference .
settings.truePosition.E = nan;
settings.truePosition.N = nan;
settings.truePosition.U = nan;
settings.navSolPeriod=100; %[ms]
filename='/home/javier/workspace/gnss-sdr/trunk/install/PVT.dat';
navSolutions = gps_l1_ca_pvt_read_pvt_dump (filename);
% Reference position for Agilent cap2.dat (San Francisco static scenario)
% Scenario latitude is 37.8194388888889 N37 49 9.98
% Scenario longitude is -122.4784944 W122 28 42.58
% Scenario elevation is 35 meters.
lat=[37 49 9.98];
long=[-122 -28 -42.58];
lat_deg=dms2deg(lat);
long_deg=dms2deg(long);
h=35;
%Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
[X, Y, Z]=geo2cart(lat, long, h, 5); % geographical to cartesian conversion
%=== Convert to UTM coordinate system =============================
utmZone = findUtmZone(lat_deg, long_deg);
[settings.truePosition.E, ...
settings.truePosition.N, ...
settings.truePosition.U] = cart2utm(X, Y, Z, utmZone);
for k=1:1:length(navSolutions.X)
[navSolutions.E(k), ...
navSolutions.N(k), ...
navSolutions.U(k)]=cart2utm(navSolutions.X(k), navSolutions.Y(k), navSolutions.Z(k), utmZone);
end
plot_skyplot=0;
plotNavigation(navSolutions,settings,plot_skyplot);

60
src/utils/matlab/libs/geoFunctions/cart2geo.m Normal file
View File

@ -0,0 +1,60 @@
function [phi, lambda, h] = cart2geo(X, Y, Z, i)
%CART2GEO Conversion of Cartesian coordinates (X,Y,Z) to geographical
%coordinates (phi, lambda, h) on a selected reference ellipsoid.
%
%[phi, lambda, h] = cart2geo(X, Y, Z, i);
%
% Choices i of Reference Ellipsoid for Geographical Coordinates
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
%Kai Borre 10-13-98
%Copyright (c) by Kai Borre
%Revision: 1.0 Date: 1998/10/23
%
% CVS record:
% $Id: cart2geo.m,v 1.1.2.3 2007/01/29 15:22:49 dpl Exp $
%==========================================================================
a = [6378388 6378160 6378135 6378137 6378137];
f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563];
lambda = atan2(Y,X);
ex2 = (2-f(i))*f(i)/((1-f(i))^2);
c = a(i)*sqrt(1+ex2);
phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i)))*f(i))));
h = 0.1; oldh = 0;
iterations = 0;
while abs(h-oldh) > 1.e-12
oldh = h;
N = c/sqrt(1+ex2*cos(phi)^2);
phi = atan(Z/((sqrt(X^2+Y^2)*(1-(2-f(i))*f(i)*N/(N+h)))));
h = sqrt(X^2+Y^2)/cos(phi)-N;
iterations = iterations + 1;
if iterations > 100
fprintf('Failed to approximate h with desired precision. h-oldh: %e.\n', h-oldh);
break;
end
end
phi = phi*180/pi;
% b = zeros(1,3);
% b(1,1) = fix(phi);
% b(2,1) = fix(rem(phi,b(1,1))*60);
% b(3,1) = (phi-b(1,1)-b(1,2)/60)*3600;
lambda = lambda*180/pi;
% l = zeros(1,3);
% l(1,1) = fix(lambda);
% l(2,1) = fix(rem(lambda,l(1,1))*60);
% l(3,1) = (lambda-l(1,1)-l(1,2)/60)*3600;
%fprintf('\n phi =%3.0f %3.0f %8.5f',b(1),b(2),b(3))
%fprintf('\n lambda =%3.0f %3.0f %8.5f',l(1),l(2),l(3))
%fprintf('\n h =%14.3f\n',h)
%%%%%%%%%%%%%% end cart2geo.m %%%%%%%%%%%%%%%%%%%

176
src/utils/matlab/libs/geoFunctions/cart2utm.m Normal file
View File

@ -0,0 +1,176 @@
function [E, N, U] = cart2utm(X, Y, Z, zone)
%CART2UTM Transformation of (X,Y,Z) to (N,E,U) in UTM, zone 'zone'.
%
%[E, N, U] = cart2utm(X, Y, Z, zone);
%
% Inputs:
% X,Y,Z - Cartesian coordinates. Coordinates are referenced
% with respect to the International Terrestrial Reference
% Frame 1996 (ITRF96)
% zone - UTM zone of the given position
%
% Outputs:
% E, N, U - UTM coordinates (Easting, Northing, Uping)
%Kai Borre -11-1994
%Copyright (c) by Kai Borre
%
% CVS record:
% $Id: cart2utm.m,v 1.1.1.1.2.6 2007/01/30 09:45:12 dpl Exp $
%This implementation is based upon
%O. Andersson & K. Poder (1981) Koordinattransformationer
% ved Geod\ae{}tisk Institut. Landinspekt\oe{}ren
% Vol. 30: 552--571 and Vol. 31: 76
%
%An excellent, general reference (KW) is
%R. Koenig & K.H. Weise (1951) Mathematische Grundlagen der
% h\"oheren Geod\"asie und Kartographie.
% Erster Band, Springer Verlag
% Explanation of variables used:
% f flattening of ellipsoid
% a semi major axis in m
% m0 1 - scale at central meridian; for UTM 0.0004
% Q_n normalized meridian quadrant
% E0 Easting of central meridian
% L0 Longitude of central meridian
% bg constants for ellipsoidal geogr. to spherical geogr.
% gb constants for spherical geogr. to ellipsoidal geogr.
% gtu constants for ellipsoidal N, E to spherical N, E
% utg constants for spherical N, E to ellipoidal N, E
% tolutm tolerance for utm, 1.2E-10*meridian quadrant
% tolgeo tolerance for geographical, 0.00040 second of arc
% B, L refer to latitude and longitude. Southern latitude is negative
% International ellipsoid of 1924, valid for ED50
a = 6378388;
f = 1/297;
ex2 = (2-f)*f / ((1-f)^2);
c = a * sqrt(1+ex2);
vec = [X; Y; Z-4.5];
alpha = .756e-6;
R = [ 1 -alpha 0;
alpha 1 0;
0 0 1];
trans = [89.5; 93.8; 127.6];
scale = 0.9999988;
v = scale*R*vec + trans; % coordinate vector in ED50
L = atan2(v(2), v(1));
N1 = 6395000; % preliminary value
B = atan2(v(3)/((1-f)^2*N1), norm(v(1:2))/N1); % preliminary value
U = 0.1; oldU = 0;
iterations = 0;
while abs(U-oldU) > 1.e-4
oldU = U;
N1 = c/sqrt(1+ex2*(cos(B))^2);
B = atan2(v(3)/((1-f)^2*N1+U), norm(v(1:2))/(N1+U) );
U = norm(v(1:2))/cos(B)-N1;
iterations = iterations + 1;
if iterations > 100
fprintf('Failed to approximate U with desired precision. U-oldU: %e.\n', U-oldU);
break;
end
end
%Normalized meridian quadrant, KW p. 50 (96), p. 19 (38b), p. 5 (21)
m0 = 0.0004;
n = f / (2-f);
m = n^2 * (1/4 + n*n/64);
w = (a*(-n-m0+m*(1-m0))) / (1+n);
Q_n = a + w;
%Easting and longitude of central meridian
E0 = 500000;
L0 = (zone-30)*6 - 3;
%Check tolerance for reverse transformation
tolutm = pi/2 * 1.2e-10 * Q_n;
tolgeo = 0.000040;
%Coefficients of trigonometric series
%ellipsoidal to spherical geographical, KW p. 186--187, (51)-(52)
% bg[1] = n*(-2 + n*(2/3 + n*(4/3 + n*(-82/45))));
% bg[2] = n^2*(5/3 + n*(-16/15 + n*(-13/9)));
% bg[3] = n^3*(-26/15 + n*34/21);
% bg[4] = n^4*1237/630;
%spherical to ellipsoidal geographical, KW p. 190--191, (61)-(62)
% gb[1] = n*(2 + n*(-2/3 + n*(-2 + n*116/45)));
% gb[2] = n^2*(7/3 + n*(-8/5 + n*(-227/45)));
% gb[3] = n^3*(56/15 + n*(-136/35));
% gb[4] = n^4*4279/630;
%spherical to ellipsoidal N, E, KW p. 196, (69)
% gtu[1] = n*(1/2 + n*(-2/3 + n*(5/16 + n*41/180)));
% gtu[2] = n^2*(13/48 + n*(-3/5 + n*557/1440));
% gtu[3] = n^3*(61/240 + n*(-103/140));
% gtu[4] = n^4*49561/161280;
%ellipsoidal to spherical N, E, KW p. 194, (65)
% utg[1] = n*(-1/2 + n*(2/3 + n*(-37/96 + n*1/360)));
% utg[2] = n^2*(-1/48 + n*(-1/15 + n*437/1440));
% utg[3] = n^3*(-17/480 + n*37/840);
% utg[4] = n^4*(-4397/161280);
%With f = 1/297 we get
bg = [-3.37077907e-3;
4.73444769e-6;
-8.29914570e-9;
1.58785330e-11];
gb = [ 3.37077588e-3;
6.62769080e-6;
1.78718601e-8;
5.49266312e-11];
gtu = [ 8.41275991e-4;
7.67306686e-7;
1.21291230e-9;
2.48508228e-12];
utg = [-8.41276339e-4;
-5.95619298e-8;
-1.69485209e-10;
-2.20473896e-13];
%Ellipsoidal latitude, longitude to spherical latitude, longitude
neg_geo = 'FALSE';
if B < 0
neg_geo = 'TRUE ';
end
Bg_r = abs(B);
[res_clensin] = clsin(bg, 4, 2*Bg_r);
Bg_r = Bg_r + res_clensin;
L0 = L0*pi / 180;
Lg_r = L - L0;
%Spherical latitude, longitude to complementary spherical latitude
% i.e. spherical N, E
cos_BN = cos(Bg_r);
Np = atan2(sin(Bg_r), cos(Lg_r)*cos_BN);
Ep = atanh(sin(Lg_r) * cos_BN);
%Spherical normalized N, E to ellipsoidal N, E
Np = 2 * Np;
Ep = 2 * Ep;
[dN, dE] = clksin(gtu, 4, Np, Ep);
Np = Np/2;
Ep = Ep/2;
Np = Np + dN;
Ep = Ep + dE;
N = Q_n * Np;
E = Q_n*Ep + E0;
if neg_geo == 'TRUE '
N = -N + 20000000;
end;
%%%%%%%%%%%%%%%%%%%% end cart2utm.m %%%%%%%%%%%%%%%%%%%%

28
src/utils/matlab/libs/geoFunctions/check_t.m Normal file
View File

@ -0,0 +1,28 @@
function corrTime = check_t(time)
%CHECK_T accounting for beginning or end of week crossover.
%
%corrTime = check_t(time);
%
% Inputs:
% time - time in seconds
%
% Outputs:
% corrTime - corrected time (seconds)
%Kai Borre 04-01-96
%Copyright (c) by Kai Borre
%
% CVS record:
% $Id: check_t.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
half_week = 302400; % seconds
corrTime = time;
if time > half_week
corrTime = time - 2*half_week;
elseif time < -half_week
corrTime = time + 2*half_week;
end
%%%%%%% end check_t.m %%%%%%%%%%%%%%%%%

38
src/utils/matlab/libs/geoFunctions/clksin.m Normal file
View File

@ -0,0 +1,38 @@
function [re, im] = clksin(ar, degree, arg_real, arg_imag)
%Clenshaw summation of sinus with complex argument
%[re, im] = clksin(ar, degree, arg_real, arg_imag);
% Written by Kai Borre
% December 20, 1995
%
% See also WGS2UTM or CART2UTM
%
% CVS record:
% $Id: clksin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
sin_arg_r = sin(arg_real);
cos_arg_r = cos(arg_real);
sinh_arg_i = sinh(arg_imag);
cosh_arg_i = cosh(arg_imag);
r = 2 * cos_arg_r * cosh_arg_i;
i =-2 * sin_arg_r * sinh_arg_i;
hr1 = 0; hr = 0; hi1 = 0; hi = 0;
for t = degree : -1 : 1
hr2 = hr1;
hr1 = hr;
hi2 = hi1;
hi1 = hi;
z = ar(t) + r*hr1 - i*hi - hr2;
hi = i*hr1 + r*hi1 - hi2;
hr = z;
end
r = sin_arg_r * cosh_arg_i;
i = cos_arg_r * sinh_arg_i;
re = r*hr - i*hi;
im = r*hi + i*hr;

26
src/utils/matlab/libs/geoFunctions/clsin.m Normal file
View File

@ -0,0 +1,26 @@
function result = clsin(ar, degree, argument)
%Clenshaw summation of sinus of argument.
%
%result = clsin(ar, degree, argument);
% Written by Kai Borre
% December 20, 1995
%
% See also WGS2UTM or CART2UTM
%
% CVS record:
% $Id: clsin.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
cos_arg = 2 * cos(argument);
hr1 = 0;
hr = 0;
for t = degree : -1 : 1
hr2 = hr1;
hr1 = hr;
hr = ar(t) + cos_arg*hr1 - hr2;
end
result = hr * sin(argument);
%%%%%%%%%%%%%%%%%%%%%%% end clsin.m %%%%%%%%%%%%%%%%%%%%%

43
src/utils/matlab/libs/geoFunctions/deg2dms.m Normal file
View File

@ -0,0 +1,43 @@
function dmsOutput = deg2dms(deg)
%DEG2DMS Conversion of degrees to degrees, minutes, and seconds.
%The output format (dms format) is: (degrees*100 + minutes + seconds/100)
% Written by Kai Borre
% February 7, 2001
% Updated by Darius Plausinaitis
%%% Save the sign for later processing
neg_arg = false;
if deg < 0
% Only positive numbers should be used while spliting into deg/min/sec
deg = -deg;
neg_arg = true;
end
%%% Split degrees minutes and seconds
int_deg = floor(deg);
decimal = deg - int_deg;
min_part = decimal*60;
min = floor(min_part);
sec_part = min_part - floor(min_part);
sec = sec_part*60;
%%% Check for overflow
if sec == 60
min = min + 1;
sec = 0;
end
if min == 60
int_deg = int_deg + 1;
min = 0;
end
%%% Construct the output
dmsOutput = int_deg * 100 + min + sec/100;
%%% Correct the sign
if neg_arg == true
dmsOutput = -dmsOutput;
end
%%%%%%%%%%%%%%%%%%% end deg2dms.m %%%%%%%%%%%%%%%%

12
src/utils/matlab/libs/geoFunctions/dms2deg.m Normal file
View File

@ -0,0 +1,12 @@
function deg = dms2deg(dms)
%DMS2DEG Conversion of degrees, minutes, and seconds to degrees.
% Written by Javier Arribas 2011
% December 7, 2011
%if (dms(1)>=0)
deg=dms(1)+dms(2)/60+dms(3)/3600;
%else
%deg=dms(1)-dms(2)/60-dms(3)/3600;
%end

104
src/utils/matlab/libs/geoFunctions/dms2mat.m Normal file
View File

@ -0,0 +1,104 @@
function [dout,mout,sout] = dms2mat(dms,n)
%DMS2MAT Converts a dms vector format to a [deg min sec] matrix
%
% [d,m,s] = DMS2MAT(dms) converts a dms vector format to a
% deg:min:sec matrix. The vector format is dms = 100*deg + min + sec/100.
% This allows compressed dms data to be expanded to a d,m,s triple,
% for easier reporting and viewing of the data.
%
% [d,m,s] = DMS2MAT(dms,n) uses n digits in the accuracy of the
% seconds calculation. n = -2 uses accuracy in the hundredths position,
% n = 0 uses accuracy in the units position. Default is n = -5.
% For further discussion of the input n, see ROUNDN.
%
% mat = DMS2MAT(...) returns a single output argument of mat = [d m s].
% This is useful only if the input dms is a single column vector.
%
% See also MAT2DMS
% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown
% $Revision: 1.10 $ $Date: 2002/03/20 21:25:06 $
if nargin == 0
error('Incorrect number of arguments')
elseif nargin == 1
n = -5;
end
% Test for empty arguments
if isempty(dms); dout = []; mout = []; sout = []; return; end
% Test for complex arguments
if ~isreal(dms)
warning('Imaginary parts of complex ANGLE argument ignored')
dms = real(dms);
end
% Don't let seconds be rounded beyond the tens place.
% If you did, then 55 seconds rounds to 100, which is not good.
if n == 2; n = 1; end
% Construct a sign vector which has +1 when dms >= 0 and -1 when dms < 0.
signvec = sign(dms);
signvec = signvec + (signvec == 0); % Ensure +1 when dms = 0
% Decompress the dms data vector
dms = abs(dms);
d = fix(dms/100); % Degrees
m = fix(dms) - abs(100*d); % Minutes
[s,msg] = roundn(100*rem(dms,1),n); % Seconds: Truncate to roundoff error
if ~isempty(msg); error(msg); end
% Adjust for 60 seconds or 60 minutes.
% Test for seconds > 60 to allow for round-off from roundn,
% Test for minutes > 60 as a ripple effect from seconds > 60
indx = find(s >= 60);
if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = s(indx) - 60; end
indx = find(m >= 60);
if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx) - 60; end
% Data consistency checks
if any(m > 59) | any (m < 0)
error('Minutes must be >= 0 and <= 59')
elseif any(s >= 60) | any( s < 0)
error('Seconds must be >= 0 and < 60')
end
% Determine where to store the sign of the angle. It should be
% associated with the largest nonzero component of d:m:s.
dsign = signvec .* (d~=0);
msign = signvec .* (d==0 & m~=0);
ssign = signvec .* (d==0 & m==0 & s~=0);
% In the application of signs below, the comparison with 0 is used so that
% the sign vector contains only +1 and -1. Any zero occurances causes
% data to be lost when the sign has been applied to a higher component
% of d:m:s. Use fix function to eliminate potential round-off errors.
d = ((dsign==0) + dsign).*fix(d); % Apply signs to the degrees
m = ((msign==0) + msign).*fix(m); % Apply signs to minutes
s = ((ssign==0) + ssign).*s; % Apply signs to seconds
% Set the output arguments
if nargout <= 1
dout = [d m s];
elseif nargout == 3
dout = d; mout = m; sout = s;
else
error('Invalid number of output arguments')
end

34
src/utils/matlab/libs/geoFunctions/e_r_corr.m Normal file
View File

@ -0,0 +1,34 @@
function X_sat_rot = e_r_corr(traveltime, X_sat)
%E_R_CORR Returns rotated satellite ECEF coordinates due to Earth
%rotation during signal travel time
%
%X_sat_rot = e_r_corr(traveltime, X_sat);
%
% Inputs:
% travelTime - signal travel time
% X_sat - satellite's ECEF coordinates
%
% Outputs:
% X_sat_rot - rotated satellite's coordinates (ECEF)
%Written by Kai Borre
%Copyright (c) by Kai Borre
%
% CVS record:
% $Id: e_r_corr.m,v 1.1.1.1.2.6 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
Omegae_dot = 7.292115147e-5; % rad/sec
%--- Find rotation angle --------------------------------------------------
omegatau = Omegae_dot * traveltime;
%--- Make a rotation matrix -----------------------------------------------
R3 = [ cos(omegatau) sin(omegatau) 0;
-sin(omegatau) cos(omegatau) 0;
0 0 1];
%--- Do the rotation ------------------------------------------------------
X_sat_rot = R3 * X_sat;
%%%%%%%% end e_r_corr.m %%%%%%%%%%%%%%%%%%%%

72
src/utils/matlab/libs/geoFunctions/findUtmZone.m Normal file
View File

@ -0,0 +1,72 @@
function utmZone = findUtmZone(latitude, longitude)
%Function finds the UTM zone number for given longitude and latitude.
%The longitude value must be between -180 (180 degree West) and 180 (180
%degree East) degree. The latitude must be within -80 (80 degree South) and
%84 (84 degree North).
%
%utmZone = findUtmZone(latitude, longitude);
%
%Latitude and longitude must be in decimal degrees (e.g. 15.5 degrees not
%15 deg 30 min).
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
%This program 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 2
%of the License, or (at your option) any later version.
%
%This program 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 this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA.
%==========================================================================
%CVS record:
%$Id: findUtmZone.m,v 1.1.2.2 2006/08/22 13:45:59 dpl Exp $
%% Check value bounds =====================================================
if ((longitude > 180) || (longitude < -180))
error('Longitude value exceeds limits (-180:180).');
end
if ((latitude > 84) || (latitude < -80))
error('Latitude value exceeds limits (-80:84).');
end
%% Find zone ==============================================================
% Start at 180 deg west = -180 deg
utmZone = fix((180 + longitude)/ 6) + 1;
%% Correct zone numbers for particular areas ==============================
if (latitude > 72)
% Corrections for zones 31 33 35 37
if ((longitude >= 0) && (longitude < 9))
utmZone = 31;
elseif ((longitude >= 9) && (longitude < 21))
utmZone = 33;
elseif ((longitude >= 21) && (longitude < 33))
utmZone = 35;
elseif ((longitude >= 33) && (longitude < 42))
utmZone = 37;
end
elseif ((latitude >= 56) && (latitude < 64))
% Correction for zone 32
if ((longitude >= 3) && (longitude < 12))
utmZone = 32;
end
end

48
src/utils/matlab/libs/geoFunctions/geo2cart.m Normal file
View File

@ -0,0 +1,48 @@
function [X, Y, Z] = geo2cart(phi, lambda, h, i)
%GEO2CART Conversion of geographical coordinates (phi, lambda, h) to
%Cartesian coordinates (X, Y, Z).
%
%[X, Y, Z] = geo2cart(phi, lambda, h, i);
%
%Format for phi and lambda: [degrees minutes seconds].
%h, X, Y, and Z are in meters.
%
%Choices i of Reference Ellipsoid
% 1. International Ellipsoid 1924
% 2. International Ellipsoid 1967
% 3. World Geodetic System 1972
% 4. Geodetic Reference System 1980
% 5. World Geodetic System 1984
%
% Inputs:
% phi - geocentric latitude (format [degrees minutes seconds])
% lambda - geocentric longitude (format [degrees minutes seconds])
% h - height
% i - reference ellipsoid type
%
% Outputs:
% X, Y, Z - Cartesian coordinates (meters)
%Kai Borre 10-13-98
%Copyright (c) by Kai Borre
%
% CVS record:
% $Id: geo2cart.m,v 1.1.2.7 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
b = phi(1) + phi(2)/60 + phi(3)/3600;
b = b*pi / 180;
l = lambda(1) + lambda(2)/60 + lambda(3)/3600;
l = l*pi / 180;
a = [6378388 6378160 6378135 6378137 6378137];
f = [1/297 1/298.247 1/298.26 1/298.257222101 1/298.257223563];
ex2 = (2-f(i))*f(i) / ((1-f(i))^2);
c = a(i) * sqrt(1+ex2);
N = c / sqrt(1 + ex2*cos(b)^2);
X = (N+h) * cos(b) * cos(l);
Y = (N+h) * cos(b) * sin(l);
Z = ((1-f(i))^2*N + h) * sin(b);
%%%%%%%%%%%%%% end geo2cart.m %%%%%%%%%%%%%%%%%%%%%%%%

114
src/utils/matlab/libs/geoFunctions/leastSquarePos.m Normal file
View File

@ -0,0 +1,114 @@
function [pos, el, az, dop] = leastSquarePos(satpos, obs, settings)
%Function calculates the Least Square Solution.
%
%[pos, el, az, dop] = leastSquarePos(satpos, obs, settings);
%
% Inputs:
% satpos - Satellites positions (in ECEF system: [X; Y; Z;] -
% one column per satellite)
% obs - Observations - the pseudorange measurements to each
% satellite:
% (e.g. [20000000 21000000 .... .... .... .... ....])
% settings - receiver settings
%
% Outputs:
% pos - receiver position and receiver clock error
% (in ECEF system: [X, Y, Z, dt])
% el - Satellites elevation angles (degrees)
% az - Satellites azimuth angles (degrees)
% dop - Dilutions Of Precision ([GDOP PDOP HDOP VDOP TDOP])
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%--------------------------------------------------------------------------
%Based on Kai Borre
%Copyright (c) by Kai Borre
%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen
%
% CVS record:
% $Id: leastSquarePos.m,v 1.1.2.12 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
%=== Initialization =======================================================
nmbOfIterations = 7;
dtr = pi/180;
pos = zeros(4, 1);
X = satpos;
nmbOfSatellites = size(satpos, 2);
A = zeros(nmbOfSatellites, 4);
omc = zeros(nmbOfSatellites, 1);
az = zeros(1, nmbOfSatellites);
el = az;
%=== Iteratively find receiver position ===================================
for iter = 1:nmbOfIterations
for i = 1:nmbOfSatellites
if iter == 1
%--- Initialize variables at the first iteration --------------
Rot_X = X(:, i);
trop = 2;
else
%--- Update equations -----------------------------------------
rho2 = (X(1, i) - pos(1))^2 + (X(2, i) - pos(2))^2 + ...
(X(3, i) - pos(3))^2;
traveltime = sqrt(rho2) / settings.c ;
%--- Correct satellite position (do to earth rotation) --------
Rot_X = e_r_corr(traveltime, X(:, i));
%--- Find the elevation angel of the satellite ----------------
[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
if (settings.useTropCorr == 1)
%--- Calculate tropospheric correction --------------------
trop = tropo(sin(el(i) * dtr), ...
0.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
else
% Do not calculate or apply the tropospheric corrections
trop = 0;
end
end % if iter == 1 ... ... else
%--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos(1:3), 'fro') - pos(4) - trop);
%--- Construct the A matrix ---------------------------------------
A(i, :) = [ (-(Rot_X(1) - pos(1))) / obs(i) ...
(-(Rot_X(2) - pos(2))) / obs(i) ...
(-(Rot_X(3) - pos(3))) / obs(i) ...
1 ];
end % for i = 1:nmbOfSatellites
% These lines allow the code to exit gracefully in case of any errors
if rank(A) ~= 4
pos = zeros(1, 4);
return
end
%--- Find position update ---------------------------------------------
x = A \ omc;
%--- Apply position update --------------------------------------------
pos = pos + x;
end % for iter = 1:nmbOfIterations
pos = pos';
%=== Calculate Dilution Of Precision ======================================
if nargout == 4
%--- Initialize output ------------------------------------------------
dop = zeros(1, 5);
%--- Calculate DOP ----------------------------------------------------
Q = inv(A'*A);
dop(1) = sqrt(trace(Q)); % GDOP
dop(2) = sqrt(Q(1,1) + Q(2,2) + Q(3,3)); % PDOP
dop(3) = sqrt(Q(1,1) + Q(2,2)); % HDOP
dop(4) = sqrt(Q(3,3)); % VDOP
dop(5) = sqrt(Q(4,4)); % TDOP
end

125
src/utils/matlab/libs/geoFunctions/mat2dms.m Normal file
View File

@ -0,0 +1,125 @@
function dmsvec = mat2dms(d,m,s,n)
%MAT2DMS Converts a [deg min sec] matrix to vector format
%
% dms = MAT2DMS(d,m,s) converts a deg:min:sec matrix into a vector
% format. The vector format is dms = 100*deg + min + sec/100.
% This allows d,m,s triple to be compressed into a single value,
% which can then be employed similar to a degree or radian vector.
% The inputs d, m and s must be of equal size. Minutes and
% second must be between 0 and 60.
%
% dms = MAT2DMS(mat) assumes and input matrix of [d m s]. This is
% useful only for single column vectors for d, m and s.
%
% dms = MAT2DMS(d,m) and dms = MAT2DMS([d m]) assume that seconds
% are zero, s = 0.
%
% dms = MAT2DMS(d,m,s,n) uses n as the accuracy of the seconds
% calculation. n = -2 uses accuracy in the hundredths position,
% n = 0 uses accuracy in the units position. Default is n = -5.
% For further discussion of the input n, see ROUNDN.
%
% See also DMS2MAT
% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown
% $Revision: 1.10 $ $Date: 2002/03/20 21:25:51 $
if nargin == 0
error('Incorrect number of arguments')
elseif nargin==1
if size(d,2)== 3
s = d(:,3); m = d(:,2); d = d(:,1);
elseif size(d,2)== 2
m = d(:,2); d = d(:,1); s = zeros(size(d));
elseif size(d,2) == 0
d = []; m = []; s = [];
else
error('Single input matrices must be n-by-2 or n-by-3.');
end
n = -5;
elseif nargin == 2
s = zeros(size(d));
n = -5;
elseif nargin == 3
n = -5;
end
% Test for empty arguments
if isempty(d) & isempty(m) & isempty(s); dmsvec = []; return; end
% Don't let seconds be rounded beyond the tens place.
% If you did, then 55 seconds rounds to 100, which is not good.
if n == 2; n = 1; end
% Complex argument tests
if any([~isreal(d) ~isreal(m) ~isreal(s)])
warning('Imaginary parts of complex ANGLE argument ignored')
d = real(d); m = real(m); s = real(s);
end
% Dimension and value tests
if ~isequal(size(d),size(m),size(s))
error('Inconsistent dimensions for input arguments')
elseif any(rem(d(~isnan(d)),1) ~= 0 | rem(m(~isnan(m)),1) ~= 0)
error('Degrees and minutes must be integers')
end
if any(abs(m) > 60) | any (abs(m) < 0) % Actually algorithm allows for
error('Minutes must be >= 0 and < 60') % up to exactly 60 seconds or
% 60 minutes, but the error message
elseif any(abs(s) > 60) | any(abs(s) < 0) % doesn't reflect this so that angst
error('Seconds must be >= 0 and < 60') % is minimized in the user docs
end
% Ensure that only one negative sign is present and at the correct location
if any((s<0 & m<0) | (s<0 & d<0) | (m<0 & d<0) )
error('Multiple negative entries in a DMS specification')
elseif any((s<0 & (m~=0 | d~= 0)) | (m<0 & d~=0))
error('Incorrect negative DMS specification')
end
% Construct a sign vector which has +1 when
% angle >= 0 and -1 when angle < 0. Note that the sign of the
% angle is associated with the largest nonzero component of d:m:s
negvec = (d<0) | (m<0) | (s<0);
signvec = ~negvec - negvec;
% Convert to all positive numbers. Allows for easier
% adjusting at 60 seconds and 60 minutes
d = abs(d); m = abs(m); s = abs(s);
% Truncate seconds to a specified accuracy to eliminate round-off errors
[s,msg] = roundn(s,n);
if ~isempty(msg); error(msg); end
% Adjust for 60 seconds or 60 minutes. If s > 60, this can only be
% from round-off during roundn since s > 60 is already tested above.
% This round-off effect has happened though.
indx = find(s >= 60);
if ~isempty(indx); m(indx) = m(indx) + 1; s(indx) = 0; end
% The user can not put minutes > 60 as input. However, the line
% above may create minutes > 60 (since the user can put in m == 60),
% thus, the test below includes the greater than condition.
indx = find(m >= 60);
if ~isempty(indx); d(indx) = d(indx) + 1; m(indx) = m(indx)-60; end
% Construct the dms vector format
dmsvec = signvec .* (100*d + m + s/100);

46
src/utils/matlab/libs/geoFunctions/roundn.m Normal file
View File

@ -0,0 +1,46 @@
function [x,msg] = roundn(x,n)
%ROUNDN Rounds input data at specified power of 10
%
% y = ROUNDN(x) rounds the input data x to the nearest hundredth.
%
% y = ROUNDN(x,n) rounds the input data x at the specified power
% of tens position. For example, n = -2 rounds the input data to
% the 10E-2 (hundredths) position.
%
% [y,msg] = ROUNDN(...) returns the text of any error condition
% encountered in the output variable msg.
%
% See also ROUND
% Copyright 1996-2002 Systems Planning and Analysis, Inc. and The MathWorks, Inc.
% Written by: E. Byrns, E. Brown
% $Revision: 1.9 $ $Date: 2002/03/20 21:26:19 $
msg = []; % Initialize output
if nargin == 0
error('Incorrect number of arguments')
elseif nargin == 1
n = -2;
end
% Test for scalar n
if max(size(n)) ~= 1
msg = 'Scalar accuracy required';
if nargout < 2; error(msg); end
return
elseif ~isreal(n)
warning('Imaginary part of complex N argument ignored')
n = real(n);
end
% Compute the exponential factors for rounding at specified
% power of 10. Ensure that n is an integer.
factors = 10 ^ (fix(-n));
% Set the significant digits for the input data
x = round(x * factors) / factors;

141
src/utils/matlab/libs/geoFunctions/satpos.m Normal file
View File

@ -0,0 +1,141 @@
function [satPositions, satClkCorr] = satpos(transmitTime, prnList, ...
eph, settings)
%SATPOS Computation of satellite coordinates X,Y,Z at TRANSMITTIME for
%given ephemeris EPH. Coordinates are computed for each satellite in the
%list PRNLIST.
%[satPositions, satClkCorr] = satpos(transmitTime, prnList, eph, settings);
%
% Inputs:
% transmitTime - transmission time
% prnList - list of PRN-s to be processed
% eph - ephemerides of satellites
% settings - receiver settings
%
% Outputs:
% satPositions - position of satellites (in ECEF system [X; Y; Z;])
% satClkCorr - correction of satellite clocks
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%--------------------------------------------------------------------------
%Based on Kai Borre 04-09-96
%Copyright (c) by Kai Borre
%Updated by Darius Plausinaitis, Peter Rinder and Nicolaj Bertelsen
%
% CVS record:
% $Id: satpos.m,v 1.1.2.17 2007/01/30 09:45:12 dpl Exp $
%% Initialize constants ===================================================
numOfSatellites = size(prnList, 2);
% GPS constatns
gpsPi = 3.1415926535898; % Pi used in the GPS coordinate
% system
%--- Constants for satellite position calculation -------------------------
Omegae_dot = 7.2921151467e-5; % Earth rotation rate, [rad/s]
GM = 3.986005e14; % Universal gravitational constant times
% the mass of the Earth, [m^3/s^2]
F = -4.442807633e-10; % Constant, [sec/(meter)^(1/2)]
%% Initialize results =====================================================
satClkCorr = zeros(1, numOfSatellites);
satPositions = zeros(3, numOfSatellites);
%% Process each satellite =================================================
for satNr = 1 : numOfSatellites
prn = prnList(satNr);
%% Find initial satellite clock correction --------------------------------
%--- Find time difference ---------------------------------------------
dt = check_t(transmitTime - eph(prn).t_oc);
%--- Calculate clock correction ---------------------------------------
satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ...
eph(prn).T_GD;
time = transmitTime - satClkCorr(satNr);
%% Find satellite's position ----------------------------------------------
%Restore semi-major axis
a = eph(prn).sqrtA * eph(prn).sqrtA;
%Time correction
tk = check_t(time - eph(prn).t_oe);
%Initial mean motion
n0 = sqrt(GM / a^3);
%Mean motion
n = n0 + eph(prn).deltan;
%Mean anomaly
M = eph(prn).M_0 + n * tk;
%Reduce mean anomaly to between 0 and 360 deg
M = rem(M + 2*gpsPi, 2*gpsPi);
%Initial guess of eccentric anomaly
E = M;
%--- Iteratively compute eccentric anomaly ----------------------------
for ii = 1:10
E_old = E;
E = M + eph(prn).e * sin(E);
dE = rem(E - E_old, 2*gpsPi);
if abs(dE) < 1.e-12
% Necessary precision is reached, exit from the loop
break;
end
end
%Reduce eccentric anomaly to between 0 and 360 deg
E = rem(E + 2*gpsPi, 2*gpsPi);
%Compute relativistic correction term
dtr = F * eph(prn).e * eph(prn).sqrtA * sin(E);
%Calculate the true anomaly
nu = atan2(sqrt(1 - eph(prn).e^2) * sin(E), cos(E)-eph(prn).e);
%Compute angle phi
phi = nu + eph(prn).omega;
%Reduce phi to between 0 and 360 deg
phi = rem(phi, 2*gpsPi);
%Correct argument of latitude
u = phi + ...
eph(prn).C_uc * cos(2*phi) + ...
eph(prn).C_us * sin(2*phi);
%Correct radius
r = a * (1 - eph(prn).e*cos(E)) + ...
eph(prn).C_rc * cos(2*phi) + ...
eph(prn).C_rs * sin(2*phi);
%Correct inclination
i = eph(prn).i_0 + eph(prn).iDot * tk + ...
eph(prn).C_ic * cos(2*phi) + ...
eph(prn).C_is * sin(2*phi);
%Compute the angle between the ascending node and the Greenwich meridian
Omega = eph(prn).omega_0 + (eph(prn).omegaDot - Omegae_dot)*tk - ...
Omegae_dot * eph(prn).t_oe;
%Reduce to between 0 and 360 deg
Omega = rem(Omega + 2*gpsPi, 2*gpsPi);
%--- Compute satellite coordinates ------------------------------------
satPositions(1, satNr) = cos(u)*r * cos(Omega) - sin(u)*r * cos(i)*sin(Omega);
satPositions(2, satNr) = cos(u)*r * sin(Omega) + sin(u)*r * cos(i)*cos(Omega);
satPositions(3, satNr) = sin(u)*r * sin(i);
%% Include relativistic correction in clock correction --------------------
satClkCorr(satNr) = (eph(prn).a_f2 * dt + eph(prn).a_f1) * dt + ...
eph(prn).a_f0 - ...
eph(prn).T_GD + dtr;
end % for satNr = 1 : numOfSatellites

112
src/utils/matlab/libs/geoFunctions/togeod.m Normal file
View File

@ -0,0 +1,112 @@
function [dphi, dlambda, h] = togeod(a, finv, X, Y, Z)
%TOGEOD Subroutine to calculate geodetic coordinates latitude, longitude,
% height given Cartesian coordinates X,Y,Z, and reference ellipsoid
% values semi-major axis (a) and the inverse of flattening (finv).
%
%[dphi, dlambda, h] = togeod(a, finv, X, Y, Z);
%
% The units of linear parameters X,Y,Z,a must all agree (m,km,mi,ft,..etc)
% The output units of angular quantities will be in decimal degrees
% (15.5 degrees not 15 deg 30 min). The output units of h will be the
% same as the units of X,Y,Z,a.
%
% Inputs:
% a - semi-major axis of the reference ellipsoid
% finv - inverse of flattening of the reference ellipsoid
% X,Y,Z - Cartesian coordinates
%
% Outputs:
% dphi - latitude
% dlambda - longitude
% h - height above reference ellipsoid
% Copyright (C) 1987 C. Goad, Columbus, Ohio
% Reprinted with permission of author, 1996
% Fortran code translated into MATLAB
% Kai Borre 03-30-96
%
% CVS record:
% $Id: togeod.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
h = 0;
tolsq = 1.e-10;
maxit = 10;
% compute radians-to-degree factor
rtd = 180/pi;
% compute square of eccentricity
if finv < 1.e-20
esq = 0;
else
esq = (2 - 1/finv) / finv;
end
oneesq = 1 - esq;
% first guess
% P is distance from spin axis
P = sqrt(X^2+Y^2);
% direct calculation of longitude
if P > 1.e-20
dlambda = atan2(Y,X) * rtd;
else
dlambda = 0;
end
if (dlambda < 0)
dlambda = dlambda + 360;
end
% r is distance from origin (0,0,0)
r = sqrt(P^2 + Z^2);
if r > 1.e-20
sinphi = Z/r;
else
sinphi = 0;
end
dphi = asin(sinphi);
% initial value of height = distance from origin minus
% approximate distance from origin to surface of ellipsoid
if r < 1.e-20
h = 0;
return
end
h = r - a*(1-sinphi*sinphi/finv);
% iterate
for i = 1:maxit
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;
end
% Not Converged--Warn user
if i == maxit
fprintf([' Problem in TOGEOD, did not converge in %2.0f',...
' iterations\n'], i);
end
end % for i = 1:maxit
dphi = dphi * rtd;
%%%%%%%% end togeod.m %%%%%%%%%%%%%%%%%%%%%%

57
src/utils/matlab/libs/geoFunctions/topocent.m Normal file
View File

@ -0,0 +1,57 @@
function [Az, El, D] = topocent(X, dx)
%TOPOCENT Transformation of vector dx into topocentric coordinate
% system with origin at X.
% Both parameters are 3 by 1 vectors.
%
%[Az, El, D] = topocent(X, dx);
%
% Inputs:
% X - vector origin corrdinates (in ECEF system [X; Y; Z;])
% dx - vector ([dX; dY; dZ;]).
%
% Outputs:
% D - vector length. Units like units of the input
% Az - azimuth from north positive clockwise, degrees
% El - elevation angle, degrees
%Kai Borre 11-24-96
%Copyright (c) by Kai Borre
%
% CVS record:
% $Id: topocent.m,v 1.1.1.1.2.4 2006/08/22 13:45:59 dpl Exp $
%==========================================================================
dtr = pi/180;
[phi, lambda, h] = togeod(6378137, 298.257223563, X(1), X(2), X(3));
cl = cos(lambda * dtr);
sl = sin(lambda * dtr);
cb = cos(phi * dtr);
sb = sin(phi * dtr);
F = [-sl -sb*cl cb*cl;
cl -sb*sl cb*sl;
0 cb sb];
local_vector = F' * dx;
E = local_vector(1);
N = local_vector(2);
U = local_vector(3);
hor_dis = sqrt(E^2 + N^2);
if hor_dis < 1.e-20
Az = 0;
El = 90;
else
Az = atan2(E, N)/dtr;
El = atan2(U, hor_dis)/dtr;
end
if Az < 0
Az = Az + 360;
end
D = sqrt(dx(1)^2 + dx(2)^2 + dx(3)^2);
%%%%%%%%% end topocent.m %%%%%%%%%

98
src/utils/matlab/libs/geoFunctions/tropo.m Normal file
View File

@ -0,0 +1,98 @@
function ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum)
%TROPO Calculation of tropospheric correction.
% The range correction ddr in m is to be subtracted from
% pseudo-ranges and carrier phases
%
%ddr = tropo(sinel, hsta, p, tkel, hum, hp, htkel, hhum);
%
% Inputs:
% sinel - sin of elevation angle of satellite
% hsta - height of station in km
% p - atmospheric pressure in mb at height hp
% tkel - surface temperature in degrees Kelvin at height htkel
% hum - humidity in % at height hhum
% hp - height of pressure measurement in km
% htkel - height of temperature measurement in km
% hhum - height of humidity measurement in km
%
% Outputs:
% ddr - range correction (meters)
%
% Reference
% Goad, C.C. & Goodman, L. (1974) A Modified Tropospheric
% Refraction Correction Model. Paper presented at the
% American Geophysical Union Annual Fall Meeting, San
% Francisco, December 12-17
% A Matlab reimplementation of a C code from driver.
% Kai Borre 06-28-95
%
% CVS record:
% $Id: tropo.m,v 1.1.1.1.2.4 2006/08/22 13:46:00 dpl Exp $
%==========================================================================
a_e = 6378.137; % semi-major axis of earth ellipsoid
b0 = 7.839257e-5;
tlapse = -6.5;
tkhum = tkel + tlapse*(hhum-htkel);
atkel = 7.5*(tkhum-273.15) / (237.3+tkhum-273.15);
e0 = 0.0611 * hum * 10^atkel;
tksea = tkel - tlapse*htkel;
em = -978.77 / (2.8704e6*tlapse*1.0e-5);
tkelh = tksea + tlapse*hhum;
e0sea = e0 * (tksea/tkelh)^(4*em);
tkelp = tksea + tlapse*hp;
psea = p * (tksea/tkelp)^em;
if sinel < 0
sinel = 0;
end
tropo = 0;
done = 'FALSE';
refsea = 77.624e-6 / tksea;
htop = 1.1385e-5 / refsea;
refsea = refsea * psea;
ref = refsea * ((htop-hsta)/htop)^4;
while 1
rtop = (a_e+htop)^2 - (a_e+hsta)^2*(1-sinel^2);
% check to see if geometry is crazy
if rtop < 0
rtop = 0;
end
rtop = sqrt(rtop) - (a_e+hsta)*sinel;
a = -sinel/(htop-hsta);
b = -b0*(1-sinel^2) / (htop-hsta);
rn = zeros(8,1);
for i = 1:8
rn(i) = rtop^(i+1);
end
alpha = [2*a, 2*a^2+4*b/3, a*(a^2+3*b),...
a^4/5+2.4*a^2*b+1.2*b^2, 2*a*b*(a^2+3*b)/3,...
b^2*(6*a^2+4*b)*1.428571e-1, 0, 0];
if b^2 > 1.0e-35
alpha(7) = a*b^3/2;
alpha(8) = b^4/9;
end
dr = rtop;
dr = dr + alpha*rn;
tropo = tropo + dr*ref*1000;
if done == 'TRUE '
ddr = tropo;
break;
end
done = 'TRUE ';
refsea = (371900.0e-6/tksea-12.92e-6)/tksea;
htop = 1.1385e-5 * (1255/tksea+0.05)/refsea;
ref = refsea * e0sea * ((htop-hsta)/htop)^4;
end;
%%%%%%%%% end tropo.m %%%%%%%%%%%%%%%%%%%

177
src/utils/matlab/libs/gps_l1_ca_dll_fll_pll_read_tracking_dump.m Normal file
View File

@ -0,0 +1,177 @@
% /*!
% * \file gps_l1_ca_dll_fll_pll_read_tracking_dump.m
% * \brief Read GNSS-SDR Tracking dump binary file into MATLAB.
% * \author Javier Arribas, 2011. jarribas(at)cttc.es
% * -------------------------------------------------------------------------
% *
% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
% *
% * GNSS-SDR is a software defined Global Navigation
% * Satellite Systems receiver
% *
% * This file is part of GNSS-SDR.
% *
% * GNSS-SDR is free software: you can redistribute it and/or modify
% * it under the terms of the GNU General Public License as published by
% * the Free Software Foundation, either version 3 of the License, or
% * at your option) any later version.
% *
% * GNSS-SDR is distributed in the hope that it will be useful,
% * but WITHOUT ANY WARRANTY; without even the implied warranty of
% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% * GNU General Public License for more details.
% *
% * You should have received a copy of the GNU General Public License
% * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% *
% * -------------------------------------------------------------------------
% */
function [GNSS_tracking] = gps_l1_ca_dll_fll_pll_read_tracking_dump (filename, count)
%% usage: gps_l1_ca_dll_fll_pll_read_tracking_dump (filename, [count])
%%
%% open GNSS-SDR tracking binary log file .dat and return the contents
%%
m = nargchk (1,2,nargin);
num_float_vars=16;
num_double_vars=1;
double_size_bytes=8;
float_size_bytes=4;
skip_bytes_each_read=float_size_bytes*num_float_vars+double_size_bytes*num_double_vars;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
v1 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v2 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v3 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v4 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v5 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v6 = fread (f, count, 'uint32',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v7 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v8 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v9 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v10 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v11 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v12 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v13 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v14 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v15 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v16 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v17 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
fclose (f);
%%%%%%%% output vars %%%%%%%%
% // EPR
% d_dump_file.write((char*)&tmp_E, sizeof(float));
% d_dump_file.write((char*)&tmp_P, sizeof(float));
% d_dump_file.write((char*)&tmp_L, sizeof(float));
% // PROMPT I and Q (to analyze navigation symbols)
% d_dump_file.write((char*)&prompt_I, sizeof(float));
% d_dump_file.write((char*)&prompt_Q, sizeof(float));
% // PRN start sample stamp
% //tmp_float=(float)d_sample_counter;
% d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
% // accumulated carrier phase
% d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
%
% // carrier and code frequency
% d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
% d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
%
% //PLL commands
% d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float));
% d_dump_file.write((char*)&carr_nco_hz, sizeof(float));
%
% //DLL commands
% d_dump_file.write((char*)&code_error_chips, sizeof(float));
% d_dump_file.write((char*)&d_code_phase_samples, sizeof(float));
%
% // CN0 and carrier lock test
% d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
% d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
%
% // AUX vars (for debug purposes)
% tmp_float=0;
% d_dump_file.write((char*)&tmp_float, sizeof(float));
% d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
E=v1;
P=v2;
L=v3;
prompt_I=v4;
prompt_Q=v5;
PRN_start_sample=v6;
acc_carrier_phase_rad=v7;
carrier_doppler_hz=v8;
code_freq_hz=v9;
PLL_discriminator_hz=v10;
carr_nco_hz=v11;
code_error_chips=v12;
code_phase_samples=v13;
CN0_SNV_dB_Hz=v14;
carrier_lock_test=v15;
var1=v16;
var2=v17;
GNSS_tracking.E=E;
GNSS_tracking.P=P;
GNSS_tracking.L=L;
GNSS_tracking.prompt_I=prompt_I;
GNSS_tracking.prompt_Q=prompt_Q;
GNSS_tracking.PRN_start_sample=PRN_start_sample;
GNSS_tracking.acc_carrier_phase_rad=acc_carrier_phase_rad;
GNSS_tracking.carrier_doppler_hz=carrier_doppler_hz;
GNSS_tracking.code_freq_hz=code_freq_hz;
GNSS_tracking.PLL_discriminator_hz=PLL_discriminator_hz;
GNSS_tracking.carr_nco=carr_nco_hz;
GNSS_tracking.code_error_chips=code_error_chips;
GNSS_tracking.code_phase_samples=code_phase_samples;
GNSS_tracking.CN0_SNV_dB_Hz=CN0_SNV_dB_Hz;
GNSS_tracking.carrier_lock_test=carrier_lock_test;
GNSS_tracking.var1=var1;
GNSS_tracking.var2=var2;
end

45
src/utils/matlab/libs/gps_l1_ca_dll_pll_read_observables_dump.m Normal file
View File

@ -0,0 +1,45 @@
% Javier Arribas 2011
function [preamble_delay_ms,prn_delay_ms] = gps_l1_ca_dll_pll_read_observables_dump (channels, filename, count)
%% usage: read_tracking_dat (filename, [count])
%%
%% open GNSS-SDR tracking binary log file .dat and return the contents
%%
m = nargchk (1,2,nargin);
num_double_vars=2;
double_size_bytes=8;
skip_bytes_each_read=double_size_bytes*num_double_vars*channels;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 3)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
for N=1:1:channels
preamble_delay_ms(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
prn_delay_ms(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
end
fclose (f);
%%%%%%%% output vars %%%%%%%%
% for (unsigned int i=0; i<d_nchannels ; i++)
% {
% tmp_double=in[i][0].preamble_delay_ms;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% tmp_double=in[i][0].prn_delay_ms;
% d_dump_file.write((char*)&tmp_double, sizeof(double));
% }
end

177
src/utils/matlab/libs/gps_l1_ca_dll_pll_read_tracking_dump.m Normal file
View File

@ -0,0 +1,177 @@
% /*!
% * \file gps_l1_ca_dll_pll_read_tracking_dump.m
% * \brief Read GNSS-SDR Tracking dump binary file into MATLAB.
% * \author Javier Arribas, 2011. jarribas(at)cttc.es
% * -------------------------------------------------------------------------
% *
% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
% *
% * GNSS-SDR is a software defined Global Navigation
% * Satellite Systems receiver
% *
% * This file is part of GNSS-SDR.
% *
% * GNSS-SDR is free software: you can redistribute it and/or modify
% * it under the terms of the GNU General Public License as published by
% * the Free Software Foundation, either version 3 of the License, or
% * at your option) any later version.
% *
% * GNSS-SDR is distributed in the hope that it will be useful,
% * but WITHOUT ANY WARRANTY; without even the implied warranty of
% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% * GNU General Public License for more details.
% *
% * You should have received a copy of the GNU General Public License
% * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% *
% * -------------------------------------------------------------------------
% */
function [GNSS_tracking] = gps_l1_ca_dll_pll_read_tracking_dump (filename, count)
%% usage: gps_l1_ca_dll_pll_read_tracking_dump (filename, [count])
%%
%% open GNSS-SDR tracking binary log file .dat and return the contents
%%
m = nargchk (1,2,nargin);
num_float_vars=16;
num_double_vars=1;
double_size_bytes=8;
float_size_bytes=4;
skip_bytes_each_read=float_size_bytes*num_float_vars+double_size_bytes*num_double_vars;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
v1 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v2 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v3 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v4 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v5 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v6 = fread (f, count, 'uint32',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v7 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v8 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v9 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v10 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v11 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v12 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v13 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v14 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v15 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v16 = fread (f, count, 'float',skip_bytes_each_read-float_size_bytes);
bytes_shift=bytes_shift+float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v17 = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
fclose (f);
%%%%%%%% output vars %%%%%%%%
% // EPR
% d_dump_file.write((char*)&tmp_E, sizeof(float));
% d_dump_file.write((char*)&tmp_P, sizeof(float));
% d_dump_file.write((char*)&tmp_L, sizeof(float));
% // PROMPT I and Q (to analyze navigation symbols)
% d_dump_file.write((char*)&prompt_I, sizeof(float));
% d_dump_file.write((char*)&prompt_Q, sizeof(float));
% // PRN start sample stamp
% //tmp_float=(float)d_sample_counter;
% d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
% // accumulated carrier phase
% d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
%
% // carrier and code frequency
% d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
% d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
%
% //PLL commands
% d_dump_file.write((char*)&carr_error, sizeof(float));
% d_dump_file.write((char*)&carr_nco, sizeof(float));
%
% //DLL commands
% d_dump_file.write((char*)&code_error, sizeof(float));
% d_dump_file.write((char*)&code_nco, sizeof(float));
%
% // CN0 and carrier lock test
% d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
% d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
%
% // AUX vars (for debug purposes)
% tmp_float=0;
% d_dump_file.write((char*)&tmp_float, sizeof(float));
% d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
E=v1;
P=v2;
L=v3;
prompt_I=v4;
prompt_Q=v5;
PRN_start_sample=v6;
acc_carrier_phase_rad=v7;
carrier_doppler_hz=v8;
code_freq_hz=v9;
carr_error=v10;
carr_nco=v11;
code_error=v12;
code_nco=v13;
CN0_SNV_dB_Hz=v14;
carrier_lock_test=v15;
var1=v16;
var2=v17;
GNSS_tracking.E=E;
GNSS_tracking.P=P;
GNSS_tracking.L=L;
GNSS_tracking.prompt_I=prompt_I;
GNSS_tracking.prompt_Q=prompt_Q;
GNSS_tracking.PRN_start_sample=PRN_start_sample;
GNSS_tracking.acc_carrier_phase_rad=acc_carrier_phase_rad;
GNSS_tracking.carrier_doppler_hz=carrier_doppler_hz;
GNSS_tracking.code_freq_hz=code_freq_hz;
GNSS_tracking.carr_error=carr_error;
GNSS_tracking.carr_nco=carr_nco;
GNSS_tracking.code_error=code_error;
GNSS_tracking.code_nco=code_nco;
GNSS_tracking.CN0_SNV_dB_Hz=CN0_SNV_dB_Hz;
GNSS_tracking.carrier_lock_test=carrier_lock_test;
GNSS_tracking.var1=var1;
GNSS_tracking.var2=var2;
end

114
src/utils/matlab/libs/gps_l1_ca_pvt_read_pvt_dump.m Normal file
View File

@ -0,0 +1,114 @@
% /*!
% * \file gps_l1_ca_pvt_read_pvt_dump.m
% * \brief Read GNSS-SDR PVT lib dump binary file into MATLAB. The resulting
% structure is compatible with the K.Borre MATLAB-based receiver.
% * \author Javier Arribas, 2011. jarribas(at)cttc.es
% * -------------------------------------------------------------------------
% *
% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
% *
% * GNSS-SDR is a software defined Global Navigation
% * Satellite Systems receiver
% *
% * This file is part of GNSS-SDR.
% *
% * GNSS-SDR is free software: you can redistribute it and/or modify
% * it under the terms of the GNU General Public License as published by
% * the Free Software Foundation, either version 3 of the License, or
% * at your option) any later version.
% *
% * GNSS-SDR is distributed in the hope that it will be useful,
% * but WITHOUT ANY WARRANTY; without even the implied warranty of
% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% * GNU General Public License for more details.
% *
% * You should have received a copy of the GNU General Public License
% * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% *
% * -------------------------------------------------------------------------
% */
function [navSolutions] = gps_l1_ca_pvt_read_pvt_dump (filename, count)
%% usage: gps_l1_ca_pvt_read_pvt_dump (filename, [count])
%%
%% open GNSS-SDR PVT binary log file .dat and return the contents
%%
%
% // 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));
m = nargchk (1,2,nargin);
num_double_vars=8;
double_size_bytes=8;
skip_bytes_each_read=double_size_bytes*num_double_vars;
bytes_shift=0;
if (m)
usage (m);
end
if (nargin < 3)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
GPS_current_time = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
ECEF_X = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
ECEF_Y = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
ECEF_Z = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Clock_Offset = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Lat = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Long = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
Height = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes);
bytes_shift=bytes_shift+double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved
fclose (f);
end
navSolutions.X=ECEF_X.';
navSolutions.Y=ECEF_Y.';
navSolutions.Z=ECEF_Z.';
navSolutions.dt=Clock_Offset.';
navSolutions.latitude=Lat.';
navSolutions.longitude=Long.';
navSolutions.height=Height.';
navSolutions.TransmitTime=GPS_current_time.';

170
src/utils/matlab/libs/plotNavigation.m Normal file
View File

@ -0,0 +1,170 @@
% /*!
% * \file plotNavigation.m
% * \brief
% Functions plots variations of coordinates over time and a 3D position
% plot. It plots receiver coordinates in UTM system or coordinate offsets if
% the true UTM receiver coordinates are provided.
% * \author Darius Plausinaitis
% * Modified by Javier Arribas, 2011. jarribas(at)cttc.es
% * -------------------------------------------------------------------------
% *
% * Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors)
% *
% * GNSS-SDR is a software defined Global Navigation
% * Satellite Systems receiver
% *
% * This file is part of GNSS-SDR.
% *
% * GNSS-SDR is free software: you can redistribute it and/or modify
% * it under the terms of the GNU General Public License as published by
% * the Free Software Foundation, either version 3 of the License, or
% * at your option) any later version.
% *
% * GNSS-SDR is distributed in the hope that it will be useful,
% * but WITHOUT ANY WARRANTY; without even the implied warranty of
% * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% * GNU General Public License for more details.
% *
% * You should have received a copy of the GNU General Public License
% * along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
% *
% * -------------------------------------------------------------------------
% */
function plotNavigation(navSolutions, settings,plot_skyplot)
%Functions plots variations of coordinates over time and a 3D position
%plot. It plots receiver coordinates in UTM system or coordinate offsets if
%the true UTM receiver coordinates are provided.
%
%plotNavigation(navSolutions, settings)
%
% Inputs:
% navSolutions - Results from navigation solution function. It
% contains measured pseudoranges and receiver
% coordinates.
% settings - Receiver settings. The true receiver coordinates
% are contained in this structure.
% plot_skyplot - If ==1 then use satellite coordinates to plot the
% the satellite positions
%% Plot results in the necessary data exists ==============================
if (~isempty(navSolutions))
%% If reference position is not provided, then set reference position
%% to the average postion
if isnan(settings.truePosition.E) || isnan(settings.truePosition.N) ...
|| isnan(settings.truePosition.U)
%=== Compute mean values ==========================================
% Remove NaN-s or the output of the function MEAN will be NaN.
refCoord.E = mean(navSolutions.E(~isnan(navSolutions.E)));
refCoord.N = mean(navSolutions.N(~isnan(navSolutions.N)));
refCoord.U = mean(navSolutions.U(~isnan(navSolutions.U)));
%Also convert geodetic coordinates to deg:min:sec vector format
meanLongitude = dms2mat(deg2dms(...
mean(navSolutions.longitude(~isnan(navSolutions.longitude)))), -5);
meanLatitude = dms2mat(deg2dms(...
mean(navSolutions.latitude(~isnan(navSolutions.latitude)))), -5);
LatLong_str=[num2str(meanLatitude(1)), 'º', ...
num2str(meanLatitude(2)), '''', ...
num2str(meanLatitude(3)), '''''', ...
',', ...
num2str(meanLongitude(1)), 'º', ...
num2str(meanLongitude(2)), '''', ...
num2str(meanLongitude(3)), '''''']
refPointLgText = ['Mean Position\newline Lat: ', ...
num2str(meanLatitude(1)), '{\circ}', ...
num2str(meanLatitude(2)), '{\prime}', ...
num2str(meanLatitude(3)), '{\prime}{\prime}', ...
'\newline Lng: ', ...
num2str(meanLongitude(1)), '{\circ}', ...
num2str(meanLongitude(2)), '{\prime}', ...
num2str(meanLongitude(3)), '{\prime}{\prime}', ...
'\newline Hgt: ', ...
num2str(mean(navSolutions.height(~isnan(navSolutions.height))), '%+6.1f')];
else
% compute the mean error for static receiver
mean_position.E = mean(navSolutions.E(~isnan(navSolutions.E)));
mean_position.N = mean(navSolutions.N(~isnan(navSolutions.N)));
mean_position.U = mean(navSolutions.U(~isnan(navSolutions.U)));
refCoord.E = settings.truePosition.E;
refCoord.N = settings.truePosition.N;
refCoord.U = settings.truePosition.U;
error_meters=sqrt((mean_position.E-refCoord.E)^2+(mean_position.N-refCoord.N)^2+(mean_position.U-refCoord.U)^2);
refPointLgText = ['Reference Position, Mean 3D error = ' num2str(error_meters) ' [m]'];
end
figureNumber = 300;
% The 300 is chosen for more convenient handling of the open
% figure windows, when many figures are closed and reopened. Figures
% drawn or opened by the user, will not be "overwritten" by this
% function if the auto numbering is not used.
%=== Select (or create) and clear the figure ==========================
figure(figureNumber);
clf (figureNumber);
set (figureNumber, 'Name', 'Navigation solutions');
%--- Draw axes --------------------------------------------------------
handles(1, 1) = subplot(4, 2, 1 : 4);
handles(3, 1) = subplot(4, 2, [5, 7]);
handles(3, 2) = subplot(4, 2, [6, 8]);
%% Plot all figures =======================================================
%--- Coordinate differences in UTM system -----------------------------
plot(handles(1, 1), [(navSolutions.E - refCoord.E)', ...
(navSolutions.N - refCoord.N)',...
(navSolutions.U - refCoord.U)']);
title (handles(1, 1), 'Coordinates variations in UTM system');
legend(handles(1, 1), 'E', 'N', 'U');
xlabel(handles(1, 1), ['Measurement period: ', ...
num2str(settings.navSolPeriod), 'ms']);
ylabel(handles(1, 1), 'Variations (m)');
grid (handles(1, 1));
axis (handles(1, 1), 'tight');
%--- Position plot in UTM system --------------------------------------
plot3 (handles(3, 1), navSolutions.E - refCoord.E, ...
navSolutions.N - refCoord.N, ...
navSolutions.U - refCoord.U, '+');
hold (handles(3, 1), 'on');
%Plot the reference point
plot3 (handles(3, 1), 0, 0, 0, 'r+', 'LineWidth', 1.5, 'MarkerSize', 10);
hold (handles(3, 1), 'off');
view (handles(3, 1), 0, 90);
axis (handles(3, 1), 'equal');
grid (handles(3, 1), 'minor');
legend(handles(3, 1), 'Measurements', refPointLgText);
title (handles(3, 1), 'Positions in UTM system (3D plot)');
xlabel(handles(3, 1), 'East (m)');
ylabel(handles(3, 1), 'North (m)');
zlabel(handles(3, 1), 'Upping (m)');
if (plot_skyplot==1)
%--- Satellite sky plot -----------------------------------------------
skyPlot(handles(3, 2), ...
navSolutions.channel.az, ...
navSolutions.channel.el, ...
navSolutions.channel.PRN(:, 1));
title (handles(3, 2), ['Sky plot (mean PDOP: ', ...
num2str(mean(navSolutions.DOP(2,:))), ')']);
end
else
disp('plotNavigation: No navigation data to plot.');
end % if (~isempty(navSolutions))

153
src/utils/matlab/libs/plotTracking.m Normal file
View File

@ -0,0 +1,153 @@
function plotTracking(channelList, trackResults, settings)
%This function plots the tracking results for the given channel list.
%
%plotTracking(channelList, trackResults, settings)
%
% Inputs:
% channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function.
% settings - receiver settings.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
%This program 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 2
%of the License, or (at your option) any later version.
%
%This program 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 this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA.
%--------------------------------------------------------------------------
%CVS record:
%$Id: plotTracking.m,v 1.5.2.23 2006/08/14 14:45:14 dpl Exp $
% Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ==============================================
for channelNr = channelList
%% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by
% this function.
figure(channelNr +200);
clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ...
num2str(trackResults(channelNr).PRN), ...
') results']);
%% Draw axes ==============================================================
% Row 1
handles(1, 1) = subplot(3, 3, 1);
handles(1, 2) = subplot(3, 3, [2 3]);
% Row 2
handles(2, 1) = subplot(3, 3, 4);
handles(2, 2) = subplot(3, 3, [5 6]);
% Row 3
handles(3, 1) = subplot(3, 3, 7);
handles(3, 2) = subplot(3, 3, 8);
handles(3, 3) = subplot(3, 3, 9);
%% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess)/1000;
%----- Discrete-Time Scatter Plot ---------------------------------
plot(handles(1, 1), trackResults(channelNr).I_P,...
trackResults(channelNr).Q_P, ...
'.');
grid (handles(1, 1));
axis (handles(1, 1), 'equal');
title (handles(1, 1), 'Discrete-Time Scatter Plot');
xlabel(handles(1, 1), 'I prompt');
ylabel(handles(1, 1), 'Q prompt');
%----- Nav bits ---------------------------------------------------
plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).I_P);
grid (handles(1, 2));
title (handles(1, 2), 'Bits of the navigation message');
xlabel(handles(1, 2), 'Time (s)');
axis (handles(1, 2), 'tight');
%----- PLL discriminator unfiltered--------------------------------
plot (handles(2, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscr, 'r');
grid (handles(2, 1));
axis (handles(2, 1), 'tight');
xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Amplitude');
title (handles(2, 1), 'Raw PLL discriminator');
%----- Correlation ------------------------------------------------
plot(handles(2, 2), timeAxisInSeconds, ...
[sqrt(trackResults(channelNr).I_E.^2 + ...
trackResults(channelNr).Q_E.^2)', ...
sqrt(trackResults(channelNr).I_P.^2 + ...
trackResults(channelNr).Q_P.^2)', ...
sqrt(trackResults(channelNr).I_L.^2 + ...
trackResults(channelNr).Q_L.^2)'], ...
'-*');
grid (handles(2, 2));
title (handles(2, 2), 'Correlation results');
xlabel(handles(2, 2), 'Time (s)');
axis (handles(2, 2), 'tight');
hLegend = legend(handles(2, 2), '$\sqrt{I_{E}^2 + Q_{E}^2}$', ...
'$\sqrt{I_{P}^2 + Q_{P}^2}$', ...
'$\sqrt{I_{L}^2 + Q_{L}^2}$');
%set interpreter from tex to latex. This will draw \sqrt correctly
set(hLegend, 'Interpreter', 'Latex');
%----- PLL discriminator filtered----------------------------------
plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).pllDiscrFilt, 'b');
grid (handles(3, 1));
axis (handles(3, 1), 'tight');
xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Filtered PLL discriminator');
%----- DLL discriminator unfiltered--------------------------------
plot (handles(3, 2), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscr, 'r');
grid (handles(3, 2));
axis (handles(3, 2), 'tight');
xlabel(handles(3, 2), 'Time (s)');
ylabel(handles(3, 2), 'Amplitude');
title (handles(3, 2), 'Raw DLL discriminator');
%----- DLL discriminator filtered----------------------------------
plot (handles(3, 3), timeAxisInSeconds, ...
trackResults(channelNr).dllDiscrFilt, 'b');
grid (handles(3, 3));
axis (handles(3, 3), 'tight');
xlabel(handles(3, 3), 'Time (s)');
ylabel(handles(3, 3), 'Amplitude');
title (handles(3, 3), 'Filtered DLL discriminator');
end % for channelNr = channelList

7
src/utils/plot_acquisition_magnitudes.m
View File

@ -1,7 +0,0 @@
#!/usr/bin/octave
arg_list = argv ();
x=read_float_binary (arg_list{1});
figure(1);
plot(x);
figure(2)
input "press any key to end..."

15
src/utils/plot_carrier_spectrum.m
View File

@ -1,15 +0,0 @@
# Load number of used satellites
satellites = load("./data/satellites.dat")
samples = load("./data/samples.dat")
sampling_frequency = load("./data/sampling_frequency.dat")
signal_duration = 1/(sampling_frequency/samples)
# Plot carrier spectrum
for i = [0:satellites-1]
figure(i+1);
file_sufix = strcat("_", num2str(i), ".dat");
carrier = read_float_binary(strcat("./data/carrier_signal", file_sufix));
plot_spectrum(carrier, sampling_frequency, signal_duration, ";carrier spectrum;");
endfor
figure(satellites+1);
input "press any key to end..."

15
src/utils/plot_delay_signal.m
View File

@ -1,15 +0,0 @@
# Load number of used satellites
satellites = load("./data/satellites.dat")
# Plot delayed and resampled prn codes
for i = [0:satellites-1]
figure(i+1);
file_sufix = strcat("_", num2str(i), ".dat");
delay = read_float_binary(strcat("./data/delay_signal", file_sufix));
delay = prn_code(1:999);
delay = [prn_code; -0.5; +0.5];
plot(prn_code, "1*");
endfor
figure(satellites+1);
input "press any key to end..."

15
src/utils/plot_delay_spectrum.m
View File

@ -1,15 +0,0 @@
# Load number of used satellites
satellites = load("./data/satellites.dat")
samples = load("./data/samples.dat")
sampling_frequency = load("./data/sampling_frequency.dat")
signal_duration = 1/(sampling_frequency/samples)
# Plot delayed and resampled prn codes spectrum
for i = [0:satellites-1]
figure(i+1);
file_sufix = strcat("_", num2str(i), ".dat");
delay = read_float_binary(strcat("./data/delay_signal", file_sufix));
plot_spectrum(delay, sampling_frequency, signal_duration, ";delayed and resampled prn code spectrum;");
endfor
figure(satellites+1);
input "press any key to end..."

12
src/utils/plot_gps_signal_spectrum.m
View File

@ -1,12 +0,0 @@
# Load number of used satellites
satellites = load("./data/satellites.dat")
samples = load("./data/samples.dat")
sampling_frequency = load("./data/sampling_frequency.dat")
signal_duration = 1/(sampling_frequency/samples)
# Plot gps signal spectrum
figure(1);
gps_signal = read_float_binary("./data/gps_signal.dat");
plot_spectrum(prn_code, sampling_frequency, signal_duration, ";gps signal spectrum;");
figure(2);
input "press any key to end..."

5
src/utils/plot_pfssa.m
View File

@ -1,5 +0,0 @@
x=read_float_binary("./data/pfssa.dat");
figure(1);
plot(x)
figure(2);
input("Press any key...");

15
src/utils/plot_prn_code_signal.m
View File

@ -1,15 +0,0 @@
# Load number of used satellites
satellites = load("./data/satellites.dat")
# Plot prn codes
for i = [0:satellites-1]
figure(i+1);
file_sufix = strcat("_", num2str(i), ".dat");
prn_code = read_float_binary(strcat("./data/prn_code_signal", file_sufix));
prn_code = prn_code(1:99);
prn_code = [prn_code; -2; +2];
plot(prn_code, "1*");
endfor
figure(satellites+1);
input "press any key to end..."

15
src/utils/plot_resampled_prn_code_signal.m
View File

@ -1,15 +0,0 @@
# Load number of used satellites
satellites = load("./data/satellites.dat")
# Plot resampled prn codes
for i = [0:satellites-1]
figure(i+1);
file_sufix = strcat("_", num2str(i), ".dat");
resampled_prn_code = read_float_binary(strcat("./data/resampled_prn_code_signal", file_sufix));
resampled_prn_code = resampled_prn_code(1:599);
resampled_prn_code = [resampled_prn_code; -2; +2];
plot(resampled_prn_code, "1*");
endfor
figure(satellites+1);
input "press any key to end..."

15
src/utils/plot_resampled_prn_code_spectrum.m
View File

@ -1,15 +0,0 @@
# Load number of used satellites
satellites = load("./data/satellites.dat")
samples = load("./data/samples.dat")
sampling_frequency = load("./data/sampling_frequency.dat")
signal_duration = 1/(sampling_frequency/samples)
# Plot resampled prn codes spectrum
for i = [0:satellites-1]
figure(i+1);
file_sufix = strcat("_", num2str(i), ".dat");
resampled_prn_code = read_float_binary(strcat("./data/resampled_prn_code_signal", file_sufix));
plot_spectrum(resampled_prn_code, sampling_frequency, signal_duration, ";resampled prn code spectrum;");
endfor
figure(satellites+1);
input "press any key to end..."

18
src/utils/prn_spectrum_analysis.m
View File

@ -1,18 +0,0 @@
phase=load("./data/prn_code_phase.dat");
samples_per_code=load("./data/prn_code_samples_per_code.dat");
fs=load("./data/prn_code_fs.dat");
signal=read_float_binary("./data/prn_code_signal.dat");
phase
samples_per_code
fs
if( phase == -1 )
for i = [0:1022]
i
prn_code=signal([(i*samples_per_code)+1:(i+1)*samples_per_code]);
spectrum_analysis (prn_code,fs,0.001);
endfor
else
spectrum_analysis(signal, fs,0.001);
endif

Some files were not shown because too many files have changed in this diff Show More