From 7c8855b9acc9b992066397597ffb0fe9517c00c3 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 29 Nov 2017 15:51:30 +0100 Subject: [PATCH 01/55] Add GPS L5 files --- .../telemetry_decoder/adapters/CMakeLists.txt | 3 +- .../adapters/gps_l5_telemetry_decoder.cc | 103 ++++++++ .../adapters/gps_l5_telemetry_decoder.h | 95 +++++++ .../gnuradio_blocks/CMakeLists.txt | 1 + .../galileo_e5a_telemetry_decoder_cc.cc | 10 +- .../gps_l5_telemetry_decoder_cc.cc | 236 ++++++++++++++++++ .../gps_l5_telemetry_decoder_cc.h | 105 ++++++++ .../libs/libswiftcnav/cnav_msg.c | 6 +- .../libs/libswiftcnav/cnav_msg.h | 34 +++ src/core/receiver/gnss_block_factory.cc | 13 + 10 files changed, 595 insertions(+), 11 deletions(-) create mode 100644 src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc create mode 100644 src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h create mode 100644 src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc create mode 100644 src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h diff --git a/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt b/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt index 974adfb53..07dd84bea 100644 --- a/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/adapters/CMakeLists.txt @@ -18,7 +18,8 @@ set(TELEMETRY_DECODER_ADAPTER_SOURCES gps_l1_ca_telemetry_decoder.cc - gps_l2c_telemetry_decoder.cc + gps_l2c_telemetry_decoder.cc + gps_l5_telemetry_decoder.cc galileo_e1b_telemetry_decoder.cc sbas_l1_telemetry_decoder.cc galileo_e5a_telemetry_decoder.cc diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc new file mode 100644 index 000000000..74f262bcf --- /dev/null +++ b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc @@ -0,0 +1,103 @@ +/*! + * \file gps_l5_telemetry_decoder.cc + * \brief Implementation of an adapter of a GPS L5 NAV data decoder block + * to a TelemetryDecoderInterface + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "gps_l5_telemetry_decoder.h" +#include +#include +#include "concurrent_queue.h" +#include "gps_cnav_ephemeris.h" +#include "gps_almanac.h" +#include "gps_cnav_iono.h" +#include "gps_cnav_utc_model.h" +#include "configuration_interface.h" + + +using google::LogMessage; + +GpsL5TelemetryDecoder::GpsL5TelemetryDecoder(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams) : + role_(role), + in_streams_(in_streams), + out_streams_(out_streams) +{ + std::string default_dump_filename = "./navigation.dat"; + DLOG(INFO) << "role " << role; + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); + // make telemetry decoder object + telemetry_decoder_ = gps_l5_make_telemetry_decoder_cc(satellite_, dump_); + DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")"; + + LOG(INFO) << "global navigation message queue assigned to telemetry_decoder (" << telemetry_decoder_->unique_id() << ")" << "role " << role; + channel_ = 0; +} + + +GpsL5TelemetryDecoder::~GpsL5TelemetryDecoder() +{} + + +void GpsL5TelemetryDecoder::set_satellite(const Gnss_Satellite & satellite) +{ + satellite_ = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); + telemetry_decoder_->set_satellite(satellite_); + DLOG(INFO) << "TELEMETRY DECODER: satellite set to " << satellite_; +} + + +void GpsL5TelemetryDecoder::connect(gr::top_block_sptr top_block) +{ + if(top_block) { /* top_block is not null */}; + // Nothing to connect internally + DLOG(INFO) << "nothing to connect internally"; +} + + +void GpsL5TelemetryDecoder::disconnect(gr::top_block_sptr top_block) +{ + if(top_block) { /* top_block is not null */}; + // Nothing to disconnect +} + + +gr::basic_block_sptr GpsL5TelemetryDecoder::get_left_block() +{ + return telemetry_decoder_; +} + + +gr::basic_block_sptr GpsL5TelemetryDecoder::get_right_block() +{ + return telemetry_decoder_; +} diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h new file mode 100644 index 000000000..49a18111c --- /dev/null +++ b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h @@ -0,0 +1,95 @@ +/*! + * \file gps_l5_telemetry_decoder.h + * \brief Interface of an adapter of a GPS L5 (CNAV) data decoder block + * to a TelemetryDecoderInterface + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GPS_L5_TELEMETRY_DECODER_H_ +#define GNSS_SDR_GPS_L5_TELEMETRY_DECODER_H_ + +#include +#include "telemetry_decoder_interface.h" +#include "gps_l5_telemetry_decoder_cc.h" + + +class ConfigurationInterface; + +/*! + * \brief This class implements a NAV data decoder for GPS L5 + */ +class GpsL5TelemetryDecoder : public TelemetryDecoderInterface +{ +public: + GpsL5TelemetryDecoder(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams); + + virtual ~GpsL5TelemetryDecoder(); + + inline std::string role() override + { + return role_; + } + + //! Returns "GPS_L5_Telemetry_Decoder" + inline std::string implementation() override + { + return "GPS_L5_Telemetry_Decoder"; + } + + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; + + void set_satellite(const Gnss_Satellite & satellite) override; + inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); } + + inline void reset() override + { + return; + } + inline size_t item_size() override + { + return 0; + } + +private: + gps_l5_telemetry_decoder_cc_sptr telemetry_decoder_; + Gnss_Satellite satellite_; + int channel_; + bool dump_; + std::string dump_filename_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; +}; + +#endif diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt index 7de1a365b..80f4ae476 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt @@ -19,6 +19,7 @@ set(TELEMETRY_DECODER_GR_BLOCKS_SOURCES gps_l1_ca_telemetry_decoder_cc.cc gps_l2c_telemetry_decoder_cc.cc + gps_l5_telemetry_decoder_cc.cc galileo_e1b_telemetry_decoder_cc.cc sbas_l1_telemetry_decoder_cc.cc galileo_e5a_telemetry_decoder_cc.cc diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc index a058f169b..d390ee7ee 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc @@ -297,14 +297,8 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut d_symbol_counter = 0; flag_bit_start = true; corr_value = 0; - while(d_preamble_init.size() > 0) - { //Clear preamble correlating queue - d_preamble_init.pop_front(); - } - while(d_symbol_history.size() > 0) - { //Clear symbol queue in order to prevent possible symbol discontinuities - d_symbol_history.pop_front(); - } + d_preamble_init.clear(); + d_symbol_history.clear(); LOG(INFO) << "Bit start sync for Galileo E5a satellite " << d_satellite; } else diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc new file mode 100644 index 000000000..313322939 --- /dev/null +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -0,0 +1,236 @@ +/*! + * \file gps_l5_telemetry_decoder_cc.cc + * \brief Implementation of a NAV message demodulator block based on + * Kay Borre book MATLAB-based GPS receiver + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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 . + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#include +#include +#include +#include "gnss_synchro.h" +#include "gps_l5_telemetry_decoder_cc.h" + +using google::LogMessage; + +gps_l5_telemetry_decoder_cc_sptr +gps_l5_make_telemetry_decoder_cc(const Gnss_Satellite & satellite, bool dump) +{ + return gps_l5_telemetry_decoder_cc_sptr(new gps_l5_telemetry_decoder_cc(satellite, dump)); +} + + +gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( + const Gnss_Satellite & satellite, bool dump) : gr::block("gps_l5_telemetry_decoder_cc", + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // Telemetry Bit transition synchronization port out + this->message_port_register_out(pmt::mp("preamble_timestamp_s")); + // Ephemeris data port out + this->message_port_register_out(pmt::mp("telemetry")); + // initialize internal vars + d_dump = dump; + d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); + DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite; + //set_output_multiple (1); + d_channel = 0; + d_flag_valid_word = false; + d_TOW_at_current_symbol = 0; + d_TOW_at_Preamble = 0; + d_state = 0; //initial state + d_crc_error_count = 0; + + //initialize the CNAV frame decoder (libswiftcnav) + cnav_msg_decoder_init(&d_cnav_decoder); +} + + +gps_l5_telemetry_decoder_cc::~gps_l5_telemetry_decoder_cc() +{ + if(d_dump_file.is_open() == true) + { + try + { + d_dump_file.close(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + } + } +} + + +int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + // get pointers on in- and output gnss-synchro objects + Gnss_Synchro *out = reinterpret_cast(output_items[0]); // Get the output buffer pointer + const Gnss_Synchro *in = reinterpret_cast(input_items[0]); // Get the input buffer pointer + + bool flag_new_cnav_frame = false; + cnav_msg_t msg; + u32 delay = 0; + + //add the symbol to the decoder + u8 symbol_clip = static_cast(in[0].Prompt_I > 0) * 255; + flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); + + consume_each(1); //one by one + + // UPDATE GNSS SYNCHRO DATA + Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block + + //1. Copy the current tracking output + current_synchro_data = in[0]; + + //2. Add the telemetry decoder information + //check if new CNAV frame is available + if (flag_new_cnav_frame == true) + { + std::bitset raw_bits; + //Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder + for (u32 i = 0; i < GPS_L5_CNAV_DATA_PAGE_BITS ; i++) + { + raw_bits[GPS_L5_CNAV_DATA_PAGE_BITS - 1 - i] = ((msg.raw_msg[i/8] >> (7 - i%8)) & 1u); + } + + d_CNAV_Message.decode_page(raw_bits); + + //Push the new navigation data to the queues + if (d_CNAV_Message.have_new_ephemeris() == true) + { + // get ephemeris object for this SV + std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_ephemeris()); + std::cout << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << std::endl; + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + + } + if (d_CNAV_Message.have_new_iono() == true) + { + std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_iono()); + std::cout << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << std::endl; + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + + if (d_CNAV_Message.have_new_utc_model() == true) + { + std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_utc_model()); + std::cout << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl; + this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); + } + + //update TOW at the preamble instant + d_TOW_at_Preamble = static_cast(msg.tow); + //* The time of the last input symbol can be computed from the message ToW and + //* delay by the formulae: + //* \code + //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L5_PERIOD + 6 * GPS_L5_PERIOD; + d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; + d_flag_valid_word = true; + } + else + { + d_TOW_at_current_symbol += GPS_L5_PERIOD; + if (current_synchro_data.Flag_valid_symbol_output == false) + { + d_flag_valid_word = false; + } + } + current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; + current_synchro_data.Flag_valid_word = d_flag_valid_word; + + // if (flag_PLL_180_deg_phase_locked == true) + // { + // //correct the accumulated phase for the Costas loop phase shift, if required + // current_synchro_data.Carrier_phase_rads += GPS_PI; + // } + + if(d_dump == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = d_TOW_at_current_symbol; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_synchro_data.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); + tmp_double = d_TOW_at_Preamble; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + catch (const std::ifstream::failure & e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + + //3. Make the output (copy the object contents to the GNURadio reserved memory) + out[0] = current_synchro_data; + return 1; +} + + +void gps_l5_telemetry_decoder_cc::set_satellite(const Gnss_Satellite & satellite) +{ + d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); + LOG(INFO) << "GPS L5 CNAV telemetry decoder in channel " << this->d_channel << " set to satellite " << d_satellite; +} + + +void gps_l5_telemetry_decoder_cc::set_channel(int channel) +{ + d_channel = channel; + LOG(INFO) << "GPS L5 CNAV channel set to " << channel; + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename = "telemetry_L5_"; + d_dump_filename.append(boost::lexical_cast(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); + LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel + << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h new file mode 100644 index 000000000..bf45680d4 --- /dev/null +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h @@ -0,0 +1,105 @@ +/*! + * \file gps_l5_telemetry_decoder_cc.h + * \brief Interface of a CNAV message demodulator block based on + * Kay Borre book MATLAB-based GPS receiver + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GPS_L5_TELEMETRY_DECODER_CC_H +#define GNSS_SDR_GPS_L5_TELEMETRY_DECODER_CC_H + +#include // for copy +#include +#include +#include +#include // for pair +#include +#include +#include "gnss_satellite.h" +#include "gps_cnav_navigation_message.h" +#include "gps_cnav_ephemeris.h" +#include "gps_cnav_iono.h" +#include "concurrent_queue.h" + +extern "C" { + #include "cnav_msg.h" + #include "edc.h" + #include "bits.h" +} + +#include "GPS_L5.h" + +class gps_l5_telemetry_decoder_cc; + +typedef boost::shared_ptr gps_l5_telemetry_decoder_cc_sptr; + +gps_l5_telemetry_decoder_cc_sptr +gps_l5_make_telemetry_decoder_cc(const Gnss_Satellite & satellite, bool dump); + +/*! + * \brief This class implements a block that decodes the SBAS integrity and corrections data defined in RTCA MOPS DO-229 + * + */ +class gps_l5_telemetry_decoder_cc : public gr::block +{ +public: + ~gps_l5_telemetry_decoder_cc(); + void set_satellite(const Gnss_Satellite & satellite); //!< Set satellite PRN + void set_channel(int channel); //!< Set receiver's channel + + /*! + * \brief This is where all signal processing takes place + */ + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + + +private: + friend gps_l5_telemetry_decoder_cc_sptr + gps_l5_make_telemetry_decoder_cc(const Gnss_Satellite & satellite, bool dump); + gps_l5_telemetry_decoder_cc(const Gnss_Satellite & satellite, bool dump); + + bool d_dump; + Gnss_Satellite d_satellite; + int d_channel; + + std::string d_dump_filename; + std::ofstream d_dump_file; + + cnav_msg_decoder_t d_cnav_decoder; + + int d_state; + int d_crc_error_count; + + double d_TOW_at_current_symbol; + double d_TOW_at_Preamble; + bool d_flag_valid_word; + + Gps_CNAV_Navigation_Message d_CNAV_Message; +}; + + +#endif diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index bbb9a92f6..fabf3195b 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -49,9 +49,11 @@ */ /** Viterbi decoder reversed polynomial A */ #define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/ +#define GPS_L5_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/ + /** Viterbi decoder reversed polynomial B */ #define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */ - +#define GPS_L5_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */ /* * GPS L2C message constants. */ @@ -67,7 +69,7 @@ /** GPS LC2 CNAV CRC length in bits */ #define GPS_CNAV_MSG_CRC_LENGTH (24) /** GPS L2C CNAV message payload length in bits */ -#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH-GPS_CNAV_MSG_CRC_LENGTH) +#define GPS_CNAV_MSG_DATA_LENGTH (GPS_CNAV_MSG_LENGTH - GPS_CNAV_MSG_CRC_LENGTH) /** GPS L2C CNAV message lock detector threshold */ #define GPS_CNAV_LOCK_MAX_CRC_FAILS (10) diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h index 8b8141358..4b338a1ee 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h @@ -49,12 +49,16 @@ /** Size of the Viterbi decoder history. */ #define GPS_L2_V27_HISTORY_LENGTH_BITS 64 +#define GPS_L5_V27_HISTORY_LENGTH_BITS 64 /** Bits to accumulate before decoding starts. */ #define GPS_L2C_V27_INIT_BITS (32) +#define GPS_L5_V27_INIT_BITS (32) /** Bits to decode at a time. */ #define GPS_L2C_V27_DECODE_BITS (32) +#define GPS_L5_V27_DECODE_BITS (32) /** Bits in decoder tail. We ignore them. */ #define GPS_L2C_V27_DELAY_BITS (32) +#define GPS_L5_V27_DELAY_BITS (32) /** * GPS CNAV message container. * @@ -69,6 +73,15 @@ typedef struct u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ } cnav_msg_t; +typedef struct +{ + u8 prn; /**< SV PRN. 0..31 */ + u8 msg_id; /**< Message id. 0..31 */ + u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ + bool alert; /**< CNAV message alert flag */ + u8 raw_msg[GPS_L5_V27_DECODE_BITS + GPS_L5_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ +} cnav_L5_msg_t; + /** * GPS CNAV decoder component. * This component controls symbol decoding string. @@ -96,6 +109,27 @@ typedef struct { * do not produce output. */ } cnav_v27_part_t; +typedef struct { + v27_t dec; /**< Viterbi block decoder object */ + v27_decision_t decisions[GPS_L5_V27_HISTORY_LENGTH_BITS]; + /**< Decision graph */ + unsigned char symbols[(GPS_L5_V27_INIT_BITS + GPS_L5_V27_DECODE_BITS) * 2]; + /**< Symbol buffer */ + size_t n_symbols; /**< Count of symbols in the symbol buffer */ + unsigned char decoded[GPS_L5_V27_DECODE_BITS + GPS_L5_V27_DELAY_BITS]; + /**< Decode buffer */ + size_t n_decoded; /**< Number of bits in the decode buffer */ + bool preamble_seen; /**< When true, the decode buffer is aligned on + * preamble. */ + bool invert; /**< When true, indicates the bits are inverted */ + bool message_lock; /**< When true, indicates the message boundary + * is found. */ + bool crc_ok; /**< Flag that the last message had good CRC */ + size_t n_crc_fail; /**< Counter for CRC failures */ + bool init; /**< Initial state flag. When true, initial bits + * do not produce output. */ +} cnav_L5_v27_part_t; + /** * GPS CNAV message lock and decoder object. * diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 6ec273785..db0103fc6 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -88,6 +88,7 @@ #include "gps_l2_m_dll_pll_tracking.h" #include "gps_l1_ca_telemetry_decoder.h" #include "gps_l2c_telemetry_decoder.h" +#include "gps_l5_telemetry_decoder.h" #include "galileo_e1b_telemetry_decoder.h" #include "galileo_e5a_telemetry_decoder.h" #include "sbas_l1_telemetry_decoder.h" @@ -1094,6 +1095,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GPS_L5_Telemetry_Decoder") == 0) + { + std::unique_ptr block_(new GpsL5TelemetryDecoder(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else if (implementation.compare("Galileo_E1B_Telemetry_Decoder") == 0) { std::unique_ptr block_(new GalileoE1BTelemetryDecoder(configuration.get(), role, in_streams, @@ -1368,6 +1375,12 @@ std::unique_ptr GNSSBlockFactory::GetTlmBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GPS_L5_Telemetry_Decoder") == 0) + { + std::unique_ptr block_(new GpsL5TelemetryDecoder(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } else { // Log fatal. This causes execution to stop. From 11486670ec198358da615b1475e63bd32c5520f7 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 30 Nov 2017 16:04:20 +0100 Subject: [PATCH 02/55] Modify CNAV message structure header --- src/algorithms/PVT/libs/rtklib_solver.cc | 52 ++++++++++++++++++- .../gps_l5_telemetry_decoder_cc.cc | 4 +- src/core/system_parameters/GPS_L5.h | 4 +- 3 files changed, 55 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1d29e5a5a..622f7a1c4 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -258,7 +258,10 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ //convert ephemeris from GNSS-SDR class to RTKLIB structure eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); //convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + unsigned char default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0,0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; obs_data[valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, gps_cnav_ephemeris_iter->second.i_GPS_week, @@ -271,6 +274,53 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; } } + //GPS L5 + if(sig_.compare("5X") == 0) + { + gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) + { + // 1. Find the same satellite in GPS L1 band + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != gps_ephemeris_map.cend()) + { + // 2. If found, replace the existing GPS L1 ephemeris with the GPS L5 ephemeris + // (more precise!), and attach the L5 observation to the L1 observation in RTKLIB structure + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) + { + eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + obs_data[i] = insert_obs_to_rtklib(obs_data[i], + gnss_observables_iter->second, + gps_cnav_ephemeris_iter->second.i_GPS_week, + 2);//Band 3 (L5) + break; + } + } + } + else + { + // 3. If not found, insert the GPS L5 ephemeris and the observation + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + unsigned char default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0,0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + gps_cnav_ephemeris_iter->second.i_GPS_week, + 2);//Band 3 (L5) + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } break; } default : diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index 313322939..bb90434dc 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -154,13 +154,13 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( //* delay by the formulae: //* \code //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 - d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L5_PERIOD + 6 * GPS_L5_PERIOD; + d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L5i_PERIOD + 6 * GPS_L5i_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; d_flag_valid_word = true; } else { - d_TOW_at_current_symbol += GPS_L5_PERIOD; + d_TOW_at_current_symbol += GPS_L5i_PERIOD; if (current_synchro_data.Flag_valid_symbol_output == false) { d_flag_valid_word = false; diff --git a/src/core/system_parameters/GPS_L5.h b/src/core/system_parameters/GPS_L5.h index f64b7f379..c053796c9 100644 --- a/src/core/system_parameters/GPS_L5.h +++ b/src/core/system_parameters/GPS_L5.h @@ -37,7 +37,7 @@ #include // std::pair #include "MATH_CONSTANTS.h" #include "gnss_frequencies.h" -#include "GPS_L2C.h" +#include "GPS_L2C.h" // CNAV GPS NAVIGATION MESSAGE STRUCTURE // Physical constants const double GPS_L5_C_m_s = 299792458.0; //!< The speed of light, [m/s] @@ -178,7 +178,7 @@ const int32_t GPS_L5q_INIT_REG[210] = const int GPS_L5_CNAV_DATA_PAGE_BITS = 300; //!< GPS L5 CNAV page length, including preamble and CRC [bits] const int GPS_L5_SYMBOLS_PER_BIT = 2; -const int GPS_L5_SAMPLES_PER_SYMBOL = 1; +const int GPS_L5_SAMPLES_PER_SYMBOL = 10; const int GPS_L5_CNAV_DATA_PAGE_SYMBOLS = 600; const int GPS_L5_CNAV_DATA_PAGE_DURATION_S = 6; From 6395f0e5fc4b88f611ea6c61f0b6802acf6a22a7 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 30 Nov 2017 17:42:19 +0100 Subject: [PATCH 03/55] Add GPS L5 Channel --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 9 +- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 30 +++++- src/core/receiver/gnss_block_factory.cc | 99 ++++++++++++++++++- src/core/receiver/gnss_block_factory.h | 4 + src/core/receiver/gnss_flowgraph.cc | 18 +++- 5 files changed, 152 insertions(+), 8 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index be1c9bd8d..e04dd0765 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -141,14 +141,17 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, */ int gps_1C_count = configuration->property("Channels_1C.count", 0); int gps_2S_count = configuration->property("Channels_2S.count", 0); + int gps_L5_count = configuration->property("Channels_L5.count", 0); int gal_1B_count = configuration->property("Channels_1B.count", 0); int gal_E5a_count = configuration->property("Channels_5X.count", 0); // GPS L5 or Galileo E5a ? int gal_E5b_count = configuration->property("Channels_7X.count", 0); unsigned int type_of_receiver = 0; + // *******************WARNING!!!!!!!*********** + // GPS L5 only configurable for single frequency, single system at the moment!!!!!! if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 1; if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 2; - + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gps_L5_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 3; if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 4; if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 5; if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 6; @@ -194,7 +197,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, int num_bands = 0; if ((gps_1C_count > 0) || (gal_1B_count > 0)) num_bands = 1; if (gps_2S_count > 0) num_bands = 2; - if ((gal_E5a_count > 0) || (gal_E5b_count > 0)) num_bands = 3; + if ((gal_E5a_count > 0) || (gal_E5b_count > 0) || (gps_L5_count > 0)) num_bands = 3; int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ if( (number_of_frequencies < 1) || (number_of_frequencies > 3) ) { @@ -272,7 +275,7 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, int earth_tide = configuration->property(role + ".earth_tide", 0); int nsys = 0; - if ((gps_1C_count > 0) || (gps_2S_count > 0)) nsys += SYS_GPS; + if ((gps_1C_count > 0) || (gps_2S_count > 0) || (gps_L5_count > 0)) nsys += SYS_GPS; if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) nsys += SYS_GAL; int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */ if( (navigation_system < 1) || (navigation_system > 255) ) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */ diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 152fc6f1d..4c1f4bf5d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -345,7 +345,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc() boost::archive::xml_oarchive xml(ofs); xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_cnav_ephemeris_map); ofs.close(); - LOG(INFO) << "Saved GPS L2CM Ephemeris map data"; + LOG(INFO) << "Saved GPS L2CM or L5 Ephemeris map data"; } catch (const std::exception & e) { @@ -354,7 +354,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc() } else { - LOG(WARNING) << "Failed to save GPS L2CM Ephemeris, map is empty"; + LOG(WARNING) << "Failed to save GPS L2CM or L5 Ephemeris, map is empty"; } //save GPS L1 CA ephemeris to XML file @@ -647,6 +647,15 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite b_rinex_header_written = true; // do not write header anymore } } + if(type_of_rx == 3) // GPS L5 only + { + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.cend()) + { + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } if(type_of_rx == 4) // Galileo E1B only { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) @@ -749,6 +758,10 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite { rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); } + if(type_of_rx == 3) // GPS L5 only + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); + } if( (type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) ) // Galileo { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); @@ -799,6 +812,19 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite b_rinex_header_updated = true; } } + if(type_of_rx == 3) // GPS L5 + { + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); + b_rinex_header_updated = true; + } + } if(type_of_rx == 4) // Galileo E1B only { if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index db0103fc6..8248de9a2 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -523,6 +523,74 @@ std::unique_ptr GNSSBlockFactory::GetChannel_5X( return channel_; } +//********* GPS L5 CHANNEL ***************** +std::unique_ptr GNSSBlockFactory::GetChannel_L5( + std::shared_ptr configuration, + std::string acq, std::string trk, std::string tlm, int channel, + boost::shared_ptr queue) +{ + std::stringstream stream; + stream << channel; + std::string id = stream.str(); + LOG(INFO) << "Instantiating Channel " << id << " with Acquisition Implementation: " + << acq << ", Tracking Implementation: " << trk << ", Telemetry Decoder implementation: " << tlm; + std::string aux = configuration->property("Acquisition_L5" + boost::lexical_cast(channel) + ".implementation", std::string("W")); + std::string appendix1; + if(aux.compare("W") != 0) + { + appendix1 = boost::lexical_cast(channel); + } + else + { + appendix1 = ""; + } + aux = configuration->property("Tracking_L5" + boost::lexical_cast(channel) + ".implementation", std::string("W")); + std::string appendix2; + if(aux.compare("W") != 0) + { + appendix2 = boost::lexical_cast(channel); + } + else + { + appendix2 = ""; + } + aux = configuration->property("TelemetryDecoder_L5" + boost::lexical_cast(channel) + ".implementation", std::string("W")); + std::string appendix3; + if(aux.compare("W") != 0) + { + appendix3 = boost::lexical_cast(channel); + } + else + { + appendix3 = ""; + } + // Automatically detect input data type + std::shared_ptr config; + config = std::make_shared(); + std::string default_item_type = "gr_complex"; + std::string acq_item_type = configuration->property("Acquisition_L5" + appendix1 + ".item_type", default_item_type); + std::string trk_item_type = configuration->property("Tracking_L5" + appendix2 + ".item_type", default_item_type); + if(acq_item_type.compare(trk_item_type)) + { + LOG(ERROR) << "Acquisition and Tracking blocks must have the same input data type!"; + } + config->set_property("Channel.item_type", acq_item_type); + + std::unique_ptr pass_through_ = GetBlock(configuration, "Channel", "Pass_Through", 1, 1, queue); + std::unique_ptr acq_ = GetAcqBlock(configuration, "Acquisition_L5" + appendix1, acq, 1, 0); + std::unique_ptr trk_ = GetTrkBlock(configuration, "Tracking_L5" + appendix2, trk, 1, 1); + std::unique_ptr tlm_ = GetTlmBlock(configuration, "TelemetryDecoder_L5" + appendix3, tlm, 1, 1); + + std::unique_ptr channel_(new Channel(configuration.get(), channel, std::move(pass_through_), + std::move(acq_), + std::move(trk_), + std::move(tlm_), + "Channel", "L5", queue)); + + return channel_; +} + + std::unique_ptr>> GNSSBlockFactory::GetChannels( std::shared_ptr configuration, boost::shared_ptr queue) @@ -538,11 +606,13 @@ std::unique_ptr>> GNSSBlockFacto unsigned int Channels_2S_count = configuration->property("Channels_2S.count", 0); unsigned int Channels_1B_count = configuration->property("Channels_1B.count", 0); unsigned int Channels_5X_count = configuration->property("Channels_5X.count", 0); + unsigned int Channels_L5_count = configuration->property("Channels_L5.count", 0); unsigned int total_channels = Channels_1C_count + Channels_2S_count + Channels_1B_count + - Channels_5X_count; + Channels_5X_count + + Channels_L5_count; std::unique_ptr>> channels(new std::vector>(total_channels)); //**************** GPS L1 C/A CHANNELS ********************** @@ -605,7 +675,34 @@ std::unique_ptr>> GNSSBlockFacto queue)); channel_absolute_id++; } + //**************** GPS L5 CHANNELS ********************** + LOG(INFO)<< "Getting " << Channels_L5_count << " GPS L5 channels"; + tracking_implementation = configuration->property("Tracking_L5.implementation", default_implementation); + telemetry_decoder_implementation = configuration->property("TelemetryDecoder_L5.implementation", default_implementation); + acquisition_implementation = configuration->property("Acquisition_L5.implementation", default_implementation); + for (unsigned int i = 0; i < Channels_L5_count; i++) + { + //(i.e. Acquisition_1C0.implementation=xxxx) + std::string acquisition_implementation_specific = configuration->property( + "Acquisition_L5" + boost::lexical_cast(channel_absolute_id) + ".implementation", + acquisition_implementation); + //(i.e. Tracking_1C0.implementation=xxxx) + std::string tracking_implementation_specific = configuration->property( + "Tracking_L5" + boost::lexical_cast(channel_absolute_id) + ".implementation", + tracking_implementation); + std::string telemetry_decoder_implementation_specific = configuration->property( + "TelemetryDecoder_L5" + boost::lexical_cast(channel_absolute_id) + ".implementation", + telemetry_decoder_implementation); + // Push back the channel to the vector of channels + channels->at(channel_absolute_id) = std::move(GetChannel_L5(configuration, + acquisition_implementation_specific, + tracking_implementation_specific, + telemetry_decoder_implementation_specific, + channel_absolute_id, + queue)); + channel_absolute_id++; + } //**************** GALILEO E1 B (I/NAV OS) CHANNELS ********************** LOG(INFO) << "Getting " << Channels_1B_count << " GALILEO E1 B (I/NAV OS) channels"; diff --git a/src/core/receiver/gnss_block_factory.h b/src/core/receiver/gnss_block_factory.h index 13c614ee8..516b9e8e9 100644 --- a/src/core/receiver/gnss_block_factory.h +++ b/src/core/receiver/gnss_block_factory.h @@ -94,6 +94,10 @@ private: std::string acq, std::string trk, std::string tlm, int channel, boost::shared_ptr queue); + std::unique_ptr GetChannel_L5(std::shared_ptr configuration, + std::string acq, std::string trk, std::string tlm, int channel, + boost::shared_ptr queue); + std::unique_ptr GetAcqBlock( std::shared_ptr configuration, std::string role, diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index cf1788cc0..ed505b5f9 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -579,7 +579,8 @@ void GNSSFlowgraph::set_signals_list() unsigned int total_channels = configuration_->property("Channels_1C.count", 0) + configuration_->property("Channels_2S.count", 0) + configuration_->property("Channels_1B.count", 0) + - configuration_->property("Channels_5X.count", 0); + configuration_->property("Channels_5X.count", 0) + + configuration_->property("Channels_L5.count", 0); /* * Loop to create the list of GNSS Signals @@ -672,6 +673,19 @@ void GNSSFlowgraph::set_signals_list() } } + if (configuration_->property("Channels_L5.count", 0) > 0) + { + /* + * Loop to create GPS L5 signals + */ + for (available_gnss_prn_iter = available_gps_prn.cbegin(); + available_gnss_prn_iter != available_gps_prn.cend(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("GPS"), + *available_gnss_prn_iter), std::string("L5"))); + } + } if (configuration_->property("Channels_SBAS.count", 0) > 0) { /* @@ -725,7 +739,7 @@ void GNSSFlowgraph::set_signals_list() { std::string gnss_signal = (configuration_->property("Channel" + boost::lexical_cast(i) + ".signal", std::string("1C"))); std::string gnss_system; - if((gnss_signal.compare("1C") == 0) or (gnss_signal.compare("2S") == 0) ) gnss_system = "GPS"; + if((gnss_signal.compare("1C") == 0) or (gnss_signal.compare("2S") == 0) or (gnss_signal.compare("L5") == 0)) gnss_system = "GPS"; if((gnss_signal.compare("1B") == 0) or (gnss_signal.compare("5X") == 0) ) gnss_system = "Galileo"; unsigned int sat = configuration_->property("Channel" + boost::lexical_cast(i) + ".satellite", 0); LOG(INFO) << "Channel " << i << " system " << gnss_system << ", signal " << gnss_signal <<", sat "< Date: Wed, 6 Dec 2017 13:06:05 +0100 Subject: [PATCH 04/55] Fix bug that was preventing the PRN code to be loaded. Fixes #102 --- src/utils/front-end-cal/main.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/utils/front-end-cal/main.cc b/src/utils/front-end-cal/main.cc index cc53987f4..f4c53cd00 100644 --- a/src/utils/front-end-cal/main.cc +++ b/src/utils/front-end-cal/main.cc @@ -422,6 +422,7 @@ int main(int argc, char** argv) gnss_synchro->PRN = PRN; acquisition->set_gnss_synchro(gnss_synchro); acquisition->init(); + acquisition->set_local_code(); acquisition->reset(); stop = false; try From 26a521907a6a47471bcee8bf7d52a47d308722a5 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 11 Dec 2017 12:17:01 +0100 Subject: [PATCH 05/55] Minor changes --- .../libs/rtklib/rtklib_conversions.cc | 6 +- .../adapters/gps_l5_telemetry_decoder.cc | 4 - .../adapters/gps_l5_telemetry_decoder.h | 1 + .../gps_l5_telemetry_decoder_cc.cc | 75 +++++-- .../gps_l5_telemetry_decoder_cc.h | 10 +- .../libs/libswiftcnav/cnav_msg.c | 2 - .../libs/libswiftcnav/cnav_msg.h | 34 ---- .../adapters/gps_l5i_dll_pll_tracking.cc | 2 +- .../adapters/gps_l5i_dll_pll_tracking.h | 2 +- .../galileo_e5a_dll_pll_tracking_cc.cc | 4 +- .../gps_l5i_dll_pll_tracking_cc.cc | 6 +- src/core/receiver/gnss_block_factory.cc | 2 + src/core/system_parameters/GPS_CNAV.h | 183 ++++++++++++++++++ src/core/system_parameters/GPS_L2C.h | 143 +------------- src/core/system_parameters/GPS_L5.h | 5 +- src/core/system_parameters/MATH_CONSTANTS.h | 1 + .../system_parameters/gps_cnav_ephemeris.cc | 9 +- .../system_parameters/gps_cnav_ephemeris.h | 2 +- .../gps_cnav_navigation_message.cc | 20 +- .../gps_cnav_navigation_message.h | 11 +- src/core/system_parameters/rtcm.cc | 1 + 21 files changed, 285 insertions(+), 238 deletions(-) create mode 100644 src/core/system_parameters/GPS_CNAV.h diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index ad8810fb3..ec2e9df7e 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -35,7 +35,7 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch { rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz; rtklib_obs.P[band] = gnss_synchro.Pseudorange_m; - rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / (2.0 * PI); + rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / PI_2; switch(band) { case 0: @@ -165,8 +165,6 @@ eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph) rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); - //printf("EPHEMERIS TIME [%i]: %s,%f\n\r",rtklib_sat.sat,time_str(rtklib_sat.toe,3),rtklib_sat.toe.sec); - return rtklib_sat; } @@ -183,7 +181,7 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph) rtklib_sat.OMG0 = gps_cnav_eph.d_OMEGA0; // Compute the angle between the ascending node and the Greenwich meridian const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164 - double d_OMEGA_DOT = OMEGA_DOT_REF * GPS_L2_PI + gps_cnav_eph.d_DELTA_OMEGA_DOT; + double d_OMEGA_DOT = OMEGA_DOT_REF * PI + gps_cnav_eph.d_DELTA_OMEGA_DOT; rtklib_sat.OMGd = d_OMEGA_DOT; rtklib_sat.omg = gps_cnav_eph.d_OMEGA; rtklib_sat.i0 = gps_cnav_eph.d_i_0; diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc index 74f262bcf..fe4f356f1 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.cc @@ -34,10 +34,6 @@ #include #include #include "concurrent_queue.h" -#include "gps_cnav_ephemeris.h" -#include "gps_almanac.h" -#include "gps_cnav_iono.h" -#include "gps_cnav_utc_model.h" #include "configuration_interface.h" diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h index 49a18111c..4041e253e 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/gps_l5_telemetry_decoder.h @@ -36,6 +36,7 @@ #include #include "telemetry_decoder_interface.h" #include "gps_l5_telemetry_decoder_cc.h" +#include "gnss_satellite.h" class ConfigurationInterface; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index bb90434dc..6bd77a4b5 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -37,6 +37,8 @@ #include #include "gnss_synchro.h" #include "gps_l5_telemetry_decoder_cc.h" +#include "gps_cnav_ephemeris.h" +#include "gps_cnav_iono.h" using google::LogMessage; @@ -70,6 +72,19 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( //initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); + for(int aux = 0; aux < GPS_L5_NH_CODE_LENGTH; aux++) + { + if(GPS_L5_NH_CODE[aux] == 0) + { + bits_NH[aux] = -1.0; + } + else + { + bits_NH[aux] = 1.0; + } + } + sync_NH = false; + new_sym = false; } @@ -96,26 +111,58 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( Gnss_Synchro *out = reinterpret_cast(output_items[0]); // Get the output buffer pointer const Gnss_Synchro *in = reinterpret_cast(input_items[0]); // Get the input buffer pointer + // UPDATE GNSS SYNCHRO DATA + Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block + //1. Copy the current tracking output + current_synchro_data = in[0]; + consume_each(1); //one by one + sym_hist.push_back(in[0].Prompt_I); + double symbol_value = 0.0; + + if(sym_hist.size() == GPS_L5_NH_CODE_LENGTH) + { + std::deque::iterator it; + int corr_NH = 0; + it = sym_hist.begin(); + for(int i = 0; i < GPS_L5_NH_CODE_LENGTH; i++) + { + if((bits_NH[i] * (*it)) > 0.0){corr_NH += 1;} + else{corr_NH -= 1;} + it++; + } + if(abs(corr_NH) == GPS_L5_NH_CODE_LENGTH){sync_NH = true;} + else + { + sym_hist.pop_front(); + sync_NH = false; + } + if (sync_NH) + { + new_sym = true; + for(it = sym_hist.begin(); it != sym_hist.end(); it++) + { + symbol_value += (*it); + } + sym_hist.clear(); + } + } + bool flag_new_cnav_frame = false; cnav_msg_t msg; u32 delay = 0; //add the symbol to the decoder - u8 symbol_clip = static_cast(in[0].Prompt_I > 0) * 255; - flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); - - consume_each(1); //one by one - - // UPDATE GNSS SYNCHRO DATA - Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block - - //1. Copy the current tracking output - current_synchro_data = in[0]; - + if(new_sym) + { + u8 symbol_clip = static_cast(symbol_value > 0.0) * 255; + flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); + new_sym = false; + } //2. Add the telemetry decoder information //check if new CNAV frame is available if (flag_new_cnav_frame == true) { + std::cout << "NEW CNAV FRAME" << std::endl; std::bitset raw_bits; //Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder for (u32 i = 0; i < GPS_L5_CNAV_DATA_PAGE_BITS ; i++) @@ -169,12 +216,6 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_synchro_data.Flag_valid_word = d_flag_valid_word; - // if (flag_PLL_180_deg_phase_locked == true) - // { - // //correct the accumulated phase for the Costas loop phase shift, if required - // current_synchro_data.Carrier_phase_rads += GPS_PI; - // } - if(d_dump == true) { // MULTIPLEXED FILE RECORDING - Record results to file diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h index bf45680d4..77f91783b 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h @@ -31,17 +31,15 @@ #ifndef GNSS_SDR_GPS_L5_TELEMETRY_DECODER_CC_H #define GNSS_SDR_GPS_L5_TELEMETRY_DECODER_CC_H -#include // for copy +#include #include #include #include -#include // for pair +#include #include #include #include "gnss_satellite.h" #include "gps_cnav_navigation_message.h" -#include "gps_cnav_ephemeris.h" -#include "gps_cnav_iono.h" #include "concurrent_queue.h" extern "C" { @@ -99,6 +97,10 @@ private: bool d_flag_valid_word; Gps_CNAV_Navigation_Message d_CNAV_Message; + double bits_NH[GPS_L5_NH_CODE_LENGTH]; + std::deque sym_hist; + bool sync_NH; + bool new_sym; }; diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index fabf3195b..fbc3a22ab 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -49,11 +49,9 @@ */ /** Viterbi decoder reversed polynomial A */ #define GPS_L2C_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/ -#define GPS_L5_V27_POLY_A (0x4F) /* 0b01001111 - reversed 0171*/ /** Viterbi decoder reversed polynomial B */ #define GPS_L2C_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */ -#define GPS_L5_V27_POLY_B (0x6D) /* 0b01101101 - reversed 0133 */ /* * GPS L2C message constants. */ diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h index 4b338a1ee..8b8141358 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.h @@ -49,16 +49,12 @@ /** Size of the Viterbi decoder history. */ #define GPS_L2_V27_HISTORY_LENGTH_BITS 64 -#define GPS_L5_V27_HISTORY_LENGTH_BITS 64 /** Bits to accumulate before decoding starts. */ #define GPS_L2C_V27_INIT_BITS (32) -#define GPS_L5_V27_INIT_BITS (32) /** Bits to decode at a time. */ #define GPS_L2C_V27_DECODE_BITS (32) -#define GPS_L5_V27_DECODE_BITS (32) /** Bits in decoder tail. We ignore them. */ #define GPS_L2C_V27_DELAY_BITS (32) -#define GPS_L5_V27_DELAY_BITS (32) /** * GPS CNAV message container. * @@ -73,15 +69,6 @@ typedef struct u8 raw_msg[GPS_L2C_V27_DECODE_BITS + GPS_L2C_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ } cnav_msg_t; -typedef struct -{ - u8 prn; /**< SV PRN. 0..31 */ - u8 msg_id; /**< Message id. 0..31 */ - u32 tow; /**< GPS ToW in 6-second units. Multiply to 6 to get seconds. */ - bool alert; /**< CNAV message alert flag */ - u8 raw_msg[GPS_L5_V27_DECODE_BITS + GPS_L5_V27_DELAY_BITS]; /**< RAW MSG for GNSS-SDR */ -} cnav_L5_msg_t; - /** * GPS CNAV decoder component. * This component controls symbol decoding string. @@ -109,27 +96,6 @@ typedef struct { * do not produce output. */ } cnav_v27_part_t; -typedef struct { - v27_t dec; /**< Viterbi block decoder object */ - v27_decision_t decisions[GPS_L5_V27_HISTORY_LENGTH_BITS]; - /**< Decision graph */ - unsigned char symbols[(GPS_L5_V27_INIT_BITS + GPS_L5_V27_DECODE_BITS) * 2]; - /**< Symbol buffer */ - size_t n_symbols; /**< Count of symbols in the symbol buffer */ - unsigned char decoded[GPS_L5_V27_DECODE_BITS + GPS_L5_V27_DELAY_BITS]; - /**< Decode buffer */ - size_t n_decoded; /**< Number of bits in the decode buffer */ - bool preamble_seen; /**< When true, the decode buffer is aligned on - * preamble. */ - bool invert; /**< When true, indicates the bits are inverted */ - bool message_lock; /**< When true, indicates the message boundary - * is found. */ - bool crc_ok; /**< Flag that the last message had good CRC */ - size_t n_crc_fail; /**< Counter for CRC failures */ - bool init; /**< Initial state flag. When true, initial bits - * do not produce output. */ -} cnav_L5_v27_part_t; - /** * GPS CNAV message lock and decoder object. * diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc index e15ff4bb1..be96227d8 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.cc @@ -1,5 +1,5 @@ /*! - * \file gps_l5idll_pll_tracking.cc + * \file gps_l5i_dll_pll_tracking.cc * \brief Interface of an adapter of a DLL+PLL tracking loop block * for GPS L5i to a TrackingInterface * \author Javier Arribas, 2017. jarribas(at)cttc.es diff --git a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h index fcc4b3c81..681ddca42 100644 --- a/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l5i_dll_pll_tracking.h @@ -62,7 +62,7 @@ public: return role_; } - //! Returns "GPS_L2_M_DLL_PLL_Tracking" + //! Returns "GPS_L5i_DLL_PLL_Tracking" inline std::string implementation() override { return "GPS_L5i_DLL_PLL_Tracking"; diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc index bdc4b7d00..f36dc1139 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc @@ -642,8 +642,8 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute // The first Prompt output not equal to 0 is synchronized with the transition of a navigation data bit. if (d_secondary_lock && d_first_transition) { - current_synchro_data.Prompt_I = static_cast((d_Prompt_data).real()); - current_synchro_data.Prompt_Q = static_cast((d_Prompt_data).imag()); + current_synchro_data.Prompt_I = static_cast(d_Prompt_data.real()); + current_synchro_data.Prompt_Q = static_cast(d_Prompt_data.imag()); current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc index a515093a3..3fd0a151c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc @@ -257,14 +257,14 @@ void gps_l5i_dll_pll_tracking_cc::start_tracking() sys = sys_.substr(0,1); // DEBUG OUTPUT - std::cout << "Tracking of GPS L2CM signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; - LOG(INFO) << "Starting GPS L2CM tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + std::cout << "Tracking of GPS L5i signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting GPS L5i tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; // enable tracking d_pull_in = true; d_enable_tracking = true; - LOG(INFO) << "GPS L2CM PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + LOG(INFO) << "GPS L5i 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; } diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 666abc9a1..c3e3889fb 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -240,6 +240,7 @@ std::unique_ptr GNSSBlockFactory::GetObservables(std::shared Galileo_channels += configuration->property("Channels_5X.count", 0); unsigned int GPS_channels = configuration->property("Channels_1C.count", 0); GPS_channels += configuration->property("Channels_2S.count", 0); + GPS_channels += configuration->property("Channels_L5.count", 0); return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels, Galileo_channels + GPS_channels); } @@ -254,6 +255,7 @@ std::unique_ptr GNSSBlockFactory::GetPVT(std::shared_ptrproperty("Channels_5X.count", 0); unsigned int GPS_channels = configuration->property("Channels_1C.count", 0); GPS_channels += configuration->property("Channels_2S.count", 0); + GPS_channels += configuration->property("Channels_L5.count", 0); return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels, 0); } diff --git a/src/core/system_parameters/GPS_CNAV.h b/src/core/system_parameters/GPS_CNAV.h new file mode 100644 index 000000000..d3f953f2b --- /dev/null +++ b/src/core/system_parameters/GPS_CNAV.h @@ -0,0 +1,183 @@ +/*! + * \file GPS_CNAV.h + * \brief Defines parameters for GPS CNAV + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GPS_CNAV_H_ +#define GNSS_SDR_GPS_CNAV_H_ + +#include +#include +#include // std::pair +#include "MATH_CONSTANTS.h" + +// CNAV GPS NAVIGATION MESSAGE STRUCTURE +// NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix III) + +#define GPS_CNAV_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} +#define GPS_CNAV_PREAMBLE_STR "10001011" +#define GPS_CNAV_INV_PREAMBLE_STR "01110100" + +const int GPS_CNAV_DATA_PAGE_BITS = 300; + +// common to all messages +const std::vector > CNAV_PRN( { {9,6} } ); +const std::vector > CNAV_MSG_TYPE( { {15,6} } ); +const std::vector > CNAV_TOW( { {21,17} } ); //GPS Time Of Week in seconds +const double CNAV_TOW_LSB = 6.0; +const std::vector > CNAV_ALERT_FLAG( { {38,1} } ); + +// MESSAGE TYPE 10 (Ephemeris 1) + +const std::vector > CNAV_WN({{39,13}}); +const std::vector > CNAV_HEALTH({{52,3}}); +const std::vector > CNAV_TOP1({{55,11}}); +const double CNAV_TOP1_LSB = 300.0; +const std::vector > CNAV_URA({{66,5}}); + +const std::vector > CNAV_TOE1({{71,11}}); +const double CNAV_TOE1_LSB = 300.0; + +const std::vector > CNAV_DELTA_A({{82,26}}); //Relative to AREF = 26,559,710 meters +const double CNAV_DELTA_A_LSB = TWO_N9; + +const std::vector > CNAV_A_DOT({{108,25}}); +const double CNAV_A_DOT_LSB = TWO_N21; + +const std::vector > CNAV_DELTA_N0({{133,17}}); +const double CNAV_DELTA_N0_LSB = TWO_N44*PI; //semi-circles to radians +const std::vector > CNAV_DELTA_N0_DOT({{150,23}}); +const double CNAV_DELTA_N0_DOT_LSB = TWO_N57*PI;//semi-circles to radians +const std::vector > CNAV_M0({{173,33}}); +const double CNAV_M0_LSB = TWO_N32*PI;//semi-circles to radians +const std::vector > CNAV_E_ECCENTRICITY({{206,33}}); +const double CNAV_E_ECCENTRICITY_LSB = TWO_N34; +const std::vector > CNAV_OMEGA({{239,33}}); +const double CNAV_OMEGA_LSB = TWO_N32*PI;//semi-circles to radians +const std::vector > CNAV_INTEGRITY_FLAG({{272,1}}); +const std::vector > CNAV_L2_PHASING_FLAG({{273,1}}); + +// MESSAGE TYPE 11 (Ephemeris 2) + +const std::vector > CNAV_TOE2({{39,11}}); +const double CNAV_TOE2_LSB = 300.0; +const std::vector > CNAV_OMEGA0({{50,33}}); +const double CNAV_OMEGA0_LSB = TWO_N32*PI;//semi-circles to radians +const std::vector > CNAV_I0({{83,33}}); +const double CNAV_I0_LSB = TWO_N32*PI;//semi-circles to radians +const std::vector > CNAV_DELTA_OMEGA_DOT({{116,17}}); //Relative to REF = -2.6 x 10-9 semi-circles/second. +const double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44*PI;//semi-circles to radians +const std::vector > CNAV_I0_DOT({{133,15}}); +const double CNAV_I0_DOT_LSB = TWO_N44*PI;//semi-circles to radians +const std::vector > CNAV_CIS({{148,16}}); +const double CNAV_CIS_LSB = TWO_N30; +const std::vector > CNAV_CIC({{164,16}}); +const double CNAV_CIC_LSB = TWO_N30; +const std::vector > CNAV_CRS({{180,24}}); +const double CNAV_CRS_LSB = TWO_N8; +const std::vector > CNAV_CRC({{204,24}}); +const double CNAV_CRC_LSB = TWO_N8; +const std::vector > CNAV_CUS({{228,21}}); +const double CNAV_CUS_LSB = TWO_N30; +const std::vector > CNAV_CUC({{249,21}}); +const double CNAV_CUC_LSB = TWO_N30; + + +// MESSAGE TYPE 30 (CLOCK, IONO, GRUP DELAY) + +const std::vector > CNAV_TOP2({{39,11}}); +const double CNAV_TOP2_LSB = 300.0; +const std::vector > CNAV_URA_NED0({{50,5}}); +const std::vector > CNAV_URA_NED1({{55,3}}); +const std::vector > CNAV_URA_NED2({{58,3}}); +const std::vector > CNAV_TOC({{61,11}}); +const double CNAV_TOC_LSB = 300.0; +const std::vector > CNAV_AF0({{72,26}}); +const double CNAV_AF0_LSB = TWO_N60; +const std::vector > CNAV_AF1({{98,20}}); +const double CNAV_AF1_LSB = TWO_N48; +const std::vector > CNAV_AF2({{118,10}}); +const double CNAV_AF2_LSB = TWO_N35; +const std::vector > CNAV_TGD({{128,13}}); +const double CNAV_TGD_LSB = TWO_N35; +const std::vector > CNAV_ISCL1({{141,13}}); +const double CNAV_ISCL1_LSB = TWO_N35; +const std::vector > CNAV_ISCL2({{154,13}}); +const double CNAV_ISCL2_LSB = TWO_N35; +const std::vector > CNAV_ISCL5I({{167,13}}); +const double CNAV_ISCL5I_LSB = TWO_N35; +const std::vector > CNAV_ISCL5Q({{180,13}}); +const double CNAV_ISCL5Q_LSB = TWO_N35; +//Ionospheric parameters +const std::vector > CNAV_ALPHA0({{193,8}}); +const double CNAV_ALPHA0_LSB = TWO_N30; +const std::vector > CNAV_ALPHA1({{201,8}}); +const double CNAV_ALPHA1_LSB = TWO_N27; +const std::vector > CNAV_ALPHA2({{209,8}}); +const double CNAV_ALPHA2_LSB = TWO_N24; +const std::vector > CNAV_ALPHA3({{217,8}}); +const double CNAV_ALPHA3_LSB = TWO_N24; +const std::vector > CNAV_BETA0({{225,8}}); +const double CNAV_BETA0_LSB = TWO_P11; +const std::vector > CNAV_BETA1({{233,8}}); +const double CNAV_BETA1_LSB = TWO_P14; +const std::vector > CNAV_BETA2({{241,8}}); +const double CNAV_BETA2_LSB = TWO_P16; +const std::vector > CNAV_BETA3({{249,8}}); +const double CNAV_BETA3_LSB = TWO_P16; +const std::vector > CNAV_WNOP({{257,8}}); + + +// MESSAGE TYPE 33 (CLOCK and UTC) + +const std::vector > CNAV_A0({{128,16}}); +const double CNAV_A0_LSB = TWO_N35; +const std::vector > CNAV_A1({{144,13}}); +const double CNAV_A1_LSB = TWO_N51; +const std::vector > CNAV_A2({{157,7}}); +const double CNAV_A2_LSB = TWO_N68; +const std::vector > CNAV_DELTA_TLS({{164,8}}); +const double CNAV_DELTA_TLS_LSB = 1; +const std::vector > CNAV_TOT({{172,16}}); +const double CNAV_TOT_LSB = TWO_P4; +const std::vector > CNAV_WN_OT({{188,13}}); +const double CNAV_WN_OT_LSB = 1; +const std::vector > CNAV_WN_LSF({{201,13}}); +const double CNAV_WN_LSF_LSB = 1; +const std::vector > CNAV_DN({{214,4}}); +const double CNAV_DN_LSB = 1; +const std::vector > CNAV_DELTA_TLSF({{218,8}}); +const double CNAV_DELTA_TLSF_LSB = 1; + + +// TODO: Add more frames (Almanac, etc...) + + + +#endif /* GNSS_SDR_GPS_CNAV_H_ */ diff --git a/src/core/system_parameters/GPS_L2C.h b/src/core/system_parameters/GPS_L2C.h index babbc59ce..70a8e5ff3 100644 --- a/src/core/system_parameters/GPS_L2C.h +++ b/src/core/system_parameters/GPS_L2C.h @@ -37,6 +37,7 @@ #include // std::pair #include "MATH_CONSTANTS.h" #include "gnss_frequencies.h" +#include "GPS_CNAV.h" // Physical constants const double GPS_L2_C_m_s = 299792458.0; //!< The speed of light, [m/s] @@ -92,152 +93,10 @@ const int32_t GPS_L2C_M_INIT_REG[115] = 0706202440, 0705056276, 0020373522, 0746013617, 0132720621, 0434015513, 0566721727, 0140633660}; -// CNAV GPS NAVIGATION MESSAGE STRUCTURE -// NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix III) - -#define GPS_CNAV_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1} -#define GPS_CNAV_PREAMBLE_STR "10001011" -#define GPS_CNAV_INV_PREAMBLE_STR "01110100" - const int GPS_L2_CNAV_DATA_PAGE_BITS = 300; //!< GPS L2 CNAV page length, including preamble and CRC [bits] const int GPS_L2_SYMBOLS_PER_BIT = 2; const int GPS_L2_SAMPLES_PER_SYMBOL = 1; const int GPS_L2_CNAV_DATA_PAGE_SYMBOLS = 600; const int GPS_L2_CNAV_DATA_PAGE_DURATION_S = 12; -// common to all messages -const std::vector > CNAV_PRN( { {9,6} } ); -const std::vector > CNAV_MSG_TYPE( { {15,6} } ); -const std::vector > CNAV_TOW( { {21,17} } ); //GPS Time Of Week in seconds -const double CNAV_TOW_LSB = 6.0; -const std::vector > CNAV_ALERT_FLAG( { {38,1} } ); - -// MESSAGE TYPE 10 (Ephemeris 1) - -const std::vector > CNAV_WN({{39,13}}); -const std::vector > CNAV_HEALTH({{52,3}}); -const std::vector > CNAV_TOP1({{55,11}}); -const double CNAV_TOP1_LSB = 300.0; -const std::vector > CNAV_URA({{66,5}}); - -const std::vector > CNAV_TOE1({{71,11}}); -const double CNAV_TOE1_LSB = 300.0; - -const std::vector > CNAV_DELTA_A({{82,26}}); //Relative to AREF = 26,559,710 meters -const double CNAV_DELTA_A_LSB = TWO_N9; - -const std::vector > CNAV_A_DOT({{108,25}}); -const double CNAV_A_DOT_LSB = TWO_N21; - -const std::vector > CNAV_DELTA_N0({{133,17}}); -const double CNAV_DELTA_N0_LSB = TWO_N44*GPS_L2_PI; //semi-circles to radians -const std::vector > CNAV_DELTA_N0_DOT({{150,23}}); -const double CNAV_DELTA_N0_DOT_LSB = TWO_N57*GPS_L2_PI;//semi-circles to radians -const std::vector > CNAV_M0({{173,33}}); -const double CNAV_M0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians -const std::vector > CNAV_E_ECCENTRICITY({{206,33}}); -const double CNAV_E_ECCENTRICITY_LSB = TWO_N34; -const std::vector > CNAV_OMEGA({{239,33}}); -const double CNAV_OMEGA_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians -const std::vector > CNAV_INTEGRITY_FLAG({{272,1}}); -const std::vector > CNAV_L2_PHASING_FLAG({{273,1}}); - -// MESSAGE TYPE 11 (Ephemeris 2) - -const std::vector > CNAV_TOE2({{39,11}}); -const double CNAV_TOE2_LSB = 300.0; -const std::vector > CNAV_OMEGA0({{50,33}}); -const double CNAV_OMEGA0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians -const std::vector > CNAV_I0({{83,33}}); -const double CNAV_I0_LSB = TWO_N32*GPS_L2_PI;//semi-circles to radians -const std::vector > CNAV_DELTA_OMEGA_DOT({{116,17}}); //Relative to REF = -2.6 x 10-9 semi-circles/second. -const double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44*GPS_L2_PI;//semi-circles to radians -const std::vector > CNAV_I0_DOT({{133,15}}); -const double CNAV_I0_DOT_LSB = TWO_N44*GPS_L2_PI;//semi-circles to radians -const std::vector > CNAV_CIS({{148,16}}); -const double CNAV_CIS_LSB = TWO_N30; -const std::vector > CNAV_CIC({{164,16}}); -const double CNAV_CIC_LSB = TWO_N30; -const std::vector > CNAV_CRS({{180,24}}); -const double CNAV_CRS_LSB = TWO_N8; -const std::vector > CNAV_CRC({{204,24}}); -const double CNAV_CRC_LSB = TWO_N8; -const std::vector > CNAV_CUS({{228,21}}); -const double CNAV_CUS_LSB = TWO_N30; -const std::vector > CNAV_CUC({{249,21}}); -const double CNAV_CUC_LSB = TWO_N30; - - -// MESSAGE TYPE 30 (CLOCK, IONO, GRUP DELAY) - -const std::vector > CNAV_TOP2({{39,11}}); -const double CNAV_TOP2_LSB = 300.0; - -const std::vector > CNAV_URA_NED0({{50,5}}); -const std::vector > CNAV_URA_NED1({{55,3}}); -const std::vector > CNAV_URA_NED2({{58,3}}); -const std::vector > CNAV_TOC({{61,11}}); -const double CNAV_TOC_LSB = 300.0; -const std::vector > CNAV_AF0({{72,26}}); -const double CNAV_AF0_LSB = TWO_N60; -const std::vector > CNAV_AF1({{98,20}}); -const double CNAV_AF1_LSB = TWO_N48; -const std::vector > CNAV_AF2({{118,10}}); -const double CNAV_AF2_LSB = TWO_N35; -const std::vector > CNAV_TGD({{128,13}}); -const double CNAV_TGD_LSB = TWO_N35; -const std::vector > CNAV_ISCL1({{141,13}}); -const double CNAV_ISCL1_LSB = TWO_N35; -const std::vector > CNAV_ISCL2({{154,13}}); -const double CNAV_ISCL2_LSB = TWO_N35; -const std::vector > CNAV_ISCL5I({{167,13}}); -const double CNAV_ISCL5I_LSB = TWO_N35; -const std::vector > CNAV_ISCL5Q({{180,13}}); -const double CNAV_ISCL5Q_LSB = TWO_N35; -//Ionospheric parameters -const std::vector > CNAV_ALPHA0({{193,8}}); -const double CNAV_ALPHA0_LSB = TWO_N30; -const std::vector > CNAV_ALPHA1({{201,8}}); -const double CNAV_ALPHA1_LSB = TWO_N27; -const std::vector > CNAV_ALPHA2({{209,8}}); -const double CNAV_ALPHA2_LSB = TWO_N24; -const std::vector > CNAV_ALPHA3({{217,8}}); -const double CNAV_ALPHA3_LSB = TWO_N24; -const std::vector > CNAV_BETA0({{225,8}}); -const double CNAV_BETA0_LSB = TWO_P11; -const std::vector > CNAV_BETA1({{233,8}}); -const double CNAV_BETA1_LSB = TWO_P14; -const std::vector > CNAV_BETA2({{241,8}}); -const double CNAV_BETA2_LSB = TWO_P16; -const std::vector > CNAV_BETA3({{249,8}}); -const double CNAV_BETA3_LSB = TWO_P16; -const std::vector > CNAV_WNOP({{257,8}}); - - -// MESSAGE TYPE 33 (CLOCK and UTC) - -const std::vector > CNAV_A0({{128,16}}); -const double CNAV_A0_LSB = TWO_N35; -const std::vector > CNAV_A1({{144,13}}); -const double CNAV_A1_LSB = TWO_N51; -const std::vector > CNAV_A2({{157,7}}); -const double CNAV_A2_LSB = TWO_N68; -const std::vector > CNAV_DELTA_TLS({{164,8}}); -const double CNAV_DELTA_TLS_LSB = 1; -const std::vector > CNAV_TOT({{172,16}}); -const double CNAV_TOT_LSB = TWO_P4; -const std::vector > CNAV_WN_OT({{188,13}}); -const double CNAV_WN_OT_LSB = 1; -const std::vector > CNAV_WN_LSF({{201,13}}); -const double CNAV_WN_LSF_LSB = 1; -const std::vector > CNAV_DN({{214,4}}); -const double CNAV_DN_LSB = 1; -const std::vector > CNAV_DELTA_TLSF({{218,8}}); -const double CNAV_DELTA_TLSF_LSB = 1; - - -// TODO: Add more frames (Almanac, etc...) - - - #endif /* GNSS_SDR_GPS_L2C_H_ */ diff --git a/src/core/system_parameters/GPS_L5.h b/src/core/system_parameters/GPS_L5.h index f8ad4413f..a65d3d3f6 100644 --- a/src/core/system_parameters/GPS_L5.h +++ b/src/core/system_parameters/GPS_L5.h @@ -35,7 +35,7 @@ #include #include "MATH_CONSTANTS.h" #include "gnss_frequencies.h" -#include "GPS_L2C.h" // CNAV GPS NAVIGATION MESSAGE STRUCTURE +#include "GPS_CNAV.h" // Physical constants const double GPS_L5_C_m_s = 299792458.0; //!< The speed of light, [m/s] @@ -179,6 +179,7 @@ const int GPS_L5_SYMBOLS_PER_BIT = 2; const int GPS_L5_SAMPLES_PER_SYMBOL = 10; const int GPS_L5_CNAV_DATA_PAGE_SYMBOLS = 600; const int GPS_L5_CNAV_DATA_PAGE_DURATION_S = 6; - +const int GPS_L5_NH_CODE_LENGTH = 10; +const int GPS_L5_NH_CODE[10] = {0, 0, 0, 0, 1, 1, 0, 1, 0, 1}; #endif /* GNSS_SDR_GPS_L5_H_ */ diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 9be1a41cf..2e080f5ae 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -42,6 +42,7 @@ */ const double PI = 3.1415926535897932; //!< pi +const double PI_2 = 2.0 * PI; //!< 2 * pi const double TWO_P4 = (16); //!< 2^4 const double TWO_P11 = (2048); //!< 2^11 diff --git a/src/core/system_parameters/gps_cnav_ephemeris.cc b/src/core/system_parameters/gps_cnav_ephemeris.cc index 7669fb825..f46cfc310 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.cc +++ b/src/core/system_parameters/gps_cnav_ephemeris.cc @@ -32,7 +32,6 @@ #include "gps_cnav_ephemeris.h" #include -#include "GPS_L2C.h" #include Gps_CNAV_Ephemeris::Gps_CNAV_Ephemeris() @@ -168,7 +167,7 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) { E_old = E; E = M + d_e_eccentricity * sin(E); - dE = fmod(E - E_old, 2.0 * GPS_L2_PI); + dE = fmod(E - E_old, 2.0 * PI); if (fabs(dE) < 1e-12) { //Necessary precision is reached, exit from the loop @@ -239,7 +238,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) { E_old = E; E = M + d_e_eccentricity * sin(E); - dE = fmod(E - E_old, 2 * GPS_L2_PI); + dE = fmod(E - E_old, 2 * PI); if (fabs(dE) < 1e-12) { //Necessary precision is reached, exit from the loop @@ -268,7 +267,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi); // Compute the angle between the ascending node and the Greenwich meridian - double d_OMEGA_DOT = OMEGA_DOT_REF*GPS_L2_PI + d_DELTA_OMEGA_DOT; + double d_OMEGA_DOT = OMEGA_DOT_REF*PI + d_DELTA_OMEGA_DOT; Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe1; // Reduce to between 0 and 2*pi rad @@ -291,7 +290,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; /* relativity correction */ - dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_L2_C_m_s * GPS_L2_C_m_s); + dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (SPEED_OF_LIGHT * SPEED_OF_LIGHT); return dtr_s; } diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index 05f2c5029..b8f304951 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -32,7 +32,7 @@ #ifndef GNSS_SDR_GPS_CNAV_EPHEMERIS_H_ #define GNSS_SDR_GPS_CNAV_EPHEMERIS_H_ -#include "GPS_L2C.h" +#include "GPS_CNAV.h" #include "boost/assign.hpp" #include diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index a48ba348f..6ffe5cad1 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -73,11 +73,11 @@ Gps_CNAV_Navigation_Message::Gps_CNAV_Navigation_Message() -bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) +bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) { bool value; - if (bits[GPS_L2_CNAV_DATA_PAGE_BITS - parameter[0].first] == 1) + if (bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first] == 1) { value = true; } @@ -89,7 +89,7 @@ bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) +unsigned long int Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector> parameter) { unsigned long int value = 0; int num_of_slices = parameter.size(); @@ -98,7 +98,7 @@ unsigned long int Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bit for (int j = 0; j < parameter[i].second; j++) { value <<= 1; //shift left - if (bits[GPS_L2_CNAV_DATA_PAGE_BITS - parameter[i].first - j] == 1) + if (bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j] == 1) { value += 1; // insert the bit } @@ -108,7 +108,7 @@ unsigned long int Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bit } -signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset bits, const std::vector> parameter) +signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset bits, const std::vector> parameter) { signed long int value = 0; int num_of_slices = parameter.size(); @@ -117,7 +117,7 @@ signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset< if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system { // read the MSB and perform the sign extension - if (bits[GPS_L2_CNAV_DATA_PAGE_BITS - parameter[0].first] == 1) + if (bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first] == 1) { value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable } @@ -132,7 +132,7 @@ signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset< { value <<= 1; //shift left value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable) - if (bits[GPS_L2_CNAV_DATA_PAGE_BITS - parameter[i].first - j] == 1) + if (bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j] == 1) { value += 1; // insert the bit } @@ -142,7 +142,7 @@ signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset< else // we assume we are in a 32 bits system { // read the MSB and perform the sign extension - if (bits[GPS_L2_CNAV_DATA_PAGE_BITS - parameter[0].first] == 1) + if (bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first] == 1) { value ^= 0xFFFFFFFF; } @@ -157,7 +157,7 @@ signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset< { value <<= 1; //shift left value &= 0xFFFFFFFE; //reset the corresponding bit - if (bits[GPS_L2_CNAV_DATA_PAGE_BITS - parameter[i].first - j] == 1) + if (bits[GPS_CNAV_DATA_PAGE_BITS - parameter[i].first - j] == 1) { value += 1; // insert the bit } @@ -168,7 +168,7 @@ signed long int Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset< } -void Gps_CNAV_Navigation_Message::decode_page(std::bitset data_bits) +void Gps_CNAV_Navigation_Message::decode_page(std::bitset data_bits) { int PRN; int page_type; diff --git a/src/core/system_parameters/gps_cnav_navigation_message.h b/src/core/system_parameters/gps_cnav_navigation_message.h index a3710065c..3fd1fa913 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.h +++ b/src/core/system_parameters/gps_cnav_navigation_message.h @@ -38,8 +38,7 @@ #include #include #include -#include "GPS_L2C.h" -#include "GPS_L5.h" +#include "GPS_CNAV.h" #include "gps_cnav_ephemeris.h" #include "gps_cnav_iono.h" #include "gps_cnav_utc_model.h" @@ -56,9 +55,9 @@ class Gps_CNAV_Navigation_Message { private: - unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); - signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); - bool read_navigation_bool(std::bitset bits, const std::vector> parameter); + unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); + signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); + bool read_navigation_bool(std::bitset bits, const std::vector> parameter); Gps_CNAV_Ephemeris ephemeris_record; Gps_CNAV_Iono iono_record; @@ -90,7 +89,7 @@ public: // public functions void reset(); - void decode_page(std::bitset data_bits); + void decode_page(std::bitset data_bits); /*! * \brief Obtain a GPS SV Ephemeris class filled with current SV data */ diff --git a/src/core/system_parameters/rtcm.cc b/src/core/system_parameters/rtcm.cc index 9999b3671..9a2b5f97f 100644 --- a/src/core/system_parameters/rtcm.cc +++ b/src/core/system_parameters/rtcm.cc @@ -41,6 +41,7 @@ #include #include #include "Galileo_E1.h" +#include "GPS_L2C.h" using google::LogMessage; From 52aabf05c169ce115a7e3434d4fc147139dc982a Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 14 Dec 2017 16:39:34 +0100 Subject: [PATCH 06/55] Fix GPS L5 --- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 7 ++- src/algorithms/PVT/libs/rtklib_solver.cc | 3 +- src/algorithms/libs/gps_l5_signal.cc | 8 +-- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 10 +++- .../gps_l2c_telemetry_decoder_cc.cc | 10 +--- .../gps_l5_telemetry_decoder_cc.cc | 60 +++++++++---------- .../gps_l5_telemetry_decoder_cc.h | 9 +-- .../libs/libswiftcnav/cnav_msg.c | 3 +- src/core/system_parameters/GPS_L5.h | 5 +- .../gps_cnav_navigation_message.cc | 1 + 10 files changed, 53 insertions(+), 63 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 4c1f4bf5d..2b94ab34d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -123,7 +123,7 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); // update/insert new ephemeris record to the global ephemeris map d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; - LOG(INFO) << "New GPS CNAV ephemeris record has arrived "; + DLOG(INFO) << "New GPS CNAV ephemeris record has arrived "; } else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) { @@ -335,7 +335,7 @@ rtklib_pvt_cc::~rtklib_pvt_cc() msgctl(sysv_msqid, IPC_RMID, NULL); //save GPS L2CM ephemeris to XML file - std::string file_name = "eph_GPS_L2CM.xml"; + std::string file_name = "eph_GPS_L2CM_L5.xml"; if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) { @@ -470,7 +470,8 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if(((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0)) - || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0))) + || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0)) + || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("L5") == 0))) { // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 622f7a1c4..726225998 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -275,7 +275,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ } } //GPS L5 - if(sig_.compare("5X") == 0) + if(sig_.compare("L5") == 0) { gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.cend()) @@ -346,7 +346,6 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */ nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */ } - result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); if(result == 0) { diff --git a/src/algorithms/libs/gps_l5_signal.cc b/src/algorithms/libs/gps_l5_signal.cc index 33396ca23..5aa15b96c 100644 --- a/src/algorithms/libs/gps_l5_signal.cc +++ b/src/algorithms/libs/gps_l5_signal.cc @@ -186,7 +186,7 @@ void gps_l5i_code_gen_complex(std::complex* _dest, unsigned int _prn) if (_prn > 0 and _prn < 51) { - make_l5i(_code, _prn); + make_l5i(_code, _prn - 1); } for (signed int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++) @@ -206,7 +206,7 @@ void gps_l5i_code_gen_complex_sampled(std::complex* _dest, unsigned int _ int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5i(_code, _prn); + make_l5i(_code, _prn - 1); } signed int _samplesPerCode, _codeValueIndex; @@ -253,7 +253,7 @@ void gps_l5q_code_gen_complex(std::complex* _dest, unsigned int _prn) if (_prn > 0 and _prn < 51) { - make_l5q(_code, _prn); + make_l5q(_code, _prn - 1); } for (signed int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++) @@ -273,7 +273,7 @@ void gps_l5q_code_gen_complex_sampled(std::complex* _dest, unsigned int _ int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS]; if (_prn > 0 and _prn < 51) { - make_l5q(_code, _prn); + make_l5q(_code, _prn - 1); } signed int _samplesPerCode, _codeValueIndex; diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 056dc2901..4199dcb96 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -97,7 +97,13 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ - if (NFREQ >= 3 && (sys & (SYS_GAL | SYS_SBS))) j = 2; + if (sys & (SYS_GAL | SYS_SBS)) {j = 2;} + + if (sys == SYS_GPS) + { + if(obs->code[1] != CODE_NONE) {j = 1;} + else if(obs->code[2] != CODE_NONE) {j = 2;} + } if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) { @@ -132,7 +138,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, P2_C2 = nav->cbias[obs->sat-1][2]; /* if no P1-P2 DCB, use TGD instead */ - if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) + if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) //CHECK! { P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav); } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc index 693990124..242aca3ac 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc @@ -170,12 +170,6 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_synchro_data.Flag_valid_word = d_flag_valid_word; - // if (flag_PLL_180_deg_phase_locked == true) - // { - // //correct the accumulated phase for the Costas loop phase shift, if required - // current_synchro_data.Carrier_phase_rads += GPS_PI; - // } - if(d_dump == true) { // MULTIPLEXED FILE RECORDING - Record results to file @@ -192,7 +186,7 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( } catch (const std::ifstream::failure & e) { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + LOG(WARNING) << "Exception writing Telemetry GPS L2 dump file " << e.what(); } } @@ -230,7 +224,7 @@ void gps_l2c_telemetry_decoder_cc::set_channel(int channel) } catch (const std::ifstream::failure &e) { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + LOG(WARNING) << "channel " << d_channel << " Exception opening Telemetry GPS L2 dump file " << e.what(); } } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index 6bd77a4b5..30d549508 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -62,14 +62,10 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc( d_dump = dump; d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite; - //set_output_multiple (1); d_channel = 0; d_flag_valid_word = false; - d_TOW_at_current_symbol = 0; - d_TOW_at_Preamble = 0; - d_state = 0; //initial state - d_crc_error_count = 0; - + d_TOW_at_current_symbol = 0.0; + d_TOW_at_Preamble = 0.0; //initialize the CNAV frame decoder (libswiftcnav) cnav_msg_decoder_init(&d_cnav_decoder); for(int aux = 0; aux < GPS_L5_NH_CODE_LENGTH; aux++) @@ -117,33 +113,30 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( current_synchro_data = in[0]; consume_each(1); //one by one sym_hist.push_back(in[0].Prompt_I); - double symbol_value = 0.0; + int corr_NH = 0; + int symbol_value = 0; + //Search correlation with Neuman-Hofman Code (see IS-GPS-705D) if(sym_hist.size() == GPS_L5_NH_CODE_LENGTH) { - std::deque::iterator it; - int corr_NH = 0; - it = sym_hist.begin(); for(int i = 0; i < GPS_L5_NH_CODE_LENGTH; i++) { - if((bits_NH[i] * (*it)) > 0.0){corr_NH += 1;} - else{corr_NH -= 1;} - it++; + if((bits_NH[i] * sym_hist.at(i)) > 0.0) {corr_NH += 1;} + else {corr_NH -= 1;} + } + if(abs(corr_NH) == GPS_L5_NH_CODE_LENGTH) + { + sync_NH = true; + if(corr_NH > 0) {symbol_value = 1;} + else {symbol_value = -1;} + new_sym = true; + sym_hist.clear(); } - if(abs(corr_NH) == GPS_L5_NH_CODE_LENGTH){sync_NH = true;} else { sym_hist.pop_front(); sync_NH = false; - } - if (sync_NH) - { - new_sym = true; - for(it = sym_hist.begin(); it != sym_hist.end(); it++) - { - symbol_value += (*it); - } - sym_hist.clear(); + new_sym = false; } } @@ -154,7 +147,7 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( //add the symbol to the decoder if(new_sym) { - u8 symbol_clip = static_cast(symbol_value > 0.0) * 255; + u8 symbol_clip = static_cast(symbol_value > 0) * 255; flag_new_cnav_frame = cnav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay); new_sym = false; } @@ -162,7 +155,6 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( //check if new CNAV frame is available if (flag_new_cnav_frame == true) { - std::cout << "NEW CNAV FRAME" << std::endl; std::bitset raw_bits; //Expand packet bits to bitsets. Notice the reverse order of the bits sequence, required by the CNAV message decoder for (u32 i = 0; i < GPS_L5_CNAV_DATA_PAGE_BITS ; i++) @@ -177,31 +169,31 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( { // get ephemeris object for this SV std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_ephemeris()); - std::cout << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << std::endl; + std::cout << "New GPS L5 CNAV message received: ephemeris from satellite " << d_satellite << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_CNAV_Message.have_new_iono() == true) { std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_iono()); - std::cout << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << std::endl; + std::cout << "New GPS L5 CNAV message received: iono model parameters from satellite " << d_satellite << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } if (d_CNAV_Message.have_new_utc_model() == true) { std::shared_ptr tmp_obj = std::make_shared(d_CNAV_Message.get_utc_model()); - std::cout << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl; + std::cout << "New GPS L5 CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl; this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); } //update TOW at the preamble instant - d_TOW_at_Preamble = static_cast(msg.tow); + d_TOW_at_Preamble = static_cast(msg.tow) * 6.0; //* The time of the last input symbol can be computed from the message ToW and //* delay by the formulae: //* \code - //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 - d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L5i_PERIOD + 6 * GPS_L5i_PERIOD; + //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + d_TOW_at_current_symbol = (static_cast(msg.tow) * 6.0) + (static_cast(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; d_flag_valid_word = true; } @@ -232,7 +224,7 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( } catch (const std::ifstream::failure & e) { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what(); } } @@ -246,12 +238,14 @@ void gps_l5_telemetry_decoder_cc::set_satellite(const Gnss_Satellite & satellite { d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); LOG(INFO) << "GPS L5 CNAV telemetry decoder in channel " << this->d_channel << " set to satellite " << d_satellite; + d_CNAV_Message.reset(); } void gps_l5_telemetry_decoder_cc::set_channel(int channel) { d_channel = channel; + d_CNAV_Message.reset(); LOG(INFO) << "GPS L5 CNAV channel set to " << channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) @@ -270,7 +264,7 @@ void gps_l5_telemetry_decoder_cc::set_channel(int channel) } catch (const std::ifstream::failure &e) { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + LOG(WARNING) << "channel " << d_channel << " Exception opening Telemetry GPS L5 dump file " << e.what(); } } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h index 77f91783b..3d60b95ce 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h @@ -58,7 +58,7 @@ gps_l5_telemetry_decoder_cc_sptr gps_l5_make_telemetry_decoder_cc(const Gnss_Satellite & satellite, bool dump); /*! - * \brief This class implements a block that decodes the SBAS integrity and corrections data defined in RTCA MOPS DO-229 + * \brief This class implements a GPS L5 Telemetry decoder * */ class gps_l5_telemetry_decoder_cc : public gr::block @@ -67,10 +67,6 @@ public: ~gps_l5_telemetry_decoder_cc(); void set_satellite(const Gnss_Satellite & satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - - /*! - * \brief This is where all signal processing takes place - */ int general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); @@ -89,9 +85,6 @@ private: cnav_msg_decoder_t d_cnav_decoder; - int d_state; - int d_crc_error_count; - double d_TOW_at_current_symbol; double d_TOW_at_Preamble; bool d_flag_valid_word; diff --git a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c index fbc3a22ab..e1e62da30 100644 --- a/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c +++ b/src/algorithms/telemetry_decoder/libs/libswiftcnav/cnav_msg.c @@ -411,7 +411,8 @@ void cnav_msg_decoder_init(cnav_msg_decoder_t *dec) * The time of the last input symbol can be computed from the message ToW and * delay by the formulae: * \code - * symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + * symbolTime_ms = msg->tow * 6000 + *pdelay * 20 (L2) + * symbolTime_ms = msg->tow * 6000 + *pdelay * 10 (L5) * \endcode * * \param[in,out] dec Decoder object. diff --git a/src/core/system_parameters/GPS_L5.h b/src/core/system_parameters/GPS_L5.h index a65d3d3f6..1c6ceb8ff 100644 --- a/src/core/system_parameters/GPS_L5.h +++ b/src/core/system_parameters/GPS_L5.h @@ -51,8 +51,9 @@ const double GPS_L5_F = -4.442807633e-10; //!< Constant, [s/(m)^(1 const double GPS_L5_FREQ_HZ = FREQ5; //!< L5 [Hz] const double GPS_L5i_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s] -const int GPS_L5i_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips] -const double GPS_L5i_PERIOD = 0.001; //!< GPS L5 code period [seconds] +const int GPS_L5i_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips] +const double GPS_L5i_PERIOD = 0.001; //!< GPS L5 code period [seconds] +const double GPS_L5i_SYMBOL_PERIOD = 0.01; //!< GPS L5 symbol period [seconds] const double GPS_L5q_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s] const int GPS_L5q_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips] diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 6ffe5cad1..a78ade0c7 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -40,6 +40,7 @@ void Gps_CNAV_Navigation_Message::reset() b_flag_ephemeris_1 = false; b_flag_ephemeris_2 = false; b_flag_iono_valid = false; + b_flag_utc_valid = false; // satellite positions d_satpos_X = 0; From b2b889ec79445be41d48c12d256b6faf82755836 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 18 Dec 2017 11:07:29 +0100 Subject: [PATCH 07/55] Minor changes in Telemetry comments --- .../gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc | 2 +- .../gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc index 242aca3ac..01b4bed0b 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc @@ -154,7 +154,7 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( //* The time of the last input symbol can be computed from the message ToW and //* delay by the formulae: //* \code - //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; d_flag_valid_word = true; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc index 30d549508..2d604b221 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc @@ -192,7 +192,7 @@ int gps_l5_telemetry_decoder_cc::general_work (int noutput_items __attribute__(( //* The time of the last input symbol can be computed from the message ToW and //* delay by the formulae: //* \code - //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory d_TOW_at_current_symbol = (static_cast(msg.tow) * 6.0) + (static_cast(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; d_flag_valid_word = true; From cef7ae5f7bacfbf523d5bcb63c33cd42d6cba62e Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 18 Dec 2017 15:40:14 +0100 Subject: [PATCH 08/55] Add LOG lines in channel_fsm --- src/algorithms/channel/libs/channel_fsm.cc | 5 ++ src/core/receiver/gnss_flowgraph.cc | 61 +++++++++++----------- src/core/system_parameters/gnss_signal.h | 2 +- 3 files changed, 37 insertions(+), 31 deletions(-) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 187d419f4..44ee7bce5 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -150,23 +150,27 @@ void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); //std::cout<<"Ev_channel_start_acquisition launched"<process_event(Ev_channel_valid_acquisition()); + DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; } void ChannelFsm::Event_failed_acquisition_repeat() { this->process_event(Ev_channel_failed_acquisition_repeat()); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; } void ChannelFsm::Event_failed_acquisition_no_repeat() { this->process_event(Ev_channel_failed_acquisition_no_repeat()); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; } @@ -174,6 +178,7 @@ void ChannelFsm::Event_failed_acquisition_no_repeat() void ChannelFsm::Event_failed_tracking_standby() { this->process_event(Ev_channel_failed_tracking_standby()); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; } //void ChannelFsm::Event_failed_tracking_reacq() { diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index ed505b5f9..a535c90fd 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -374,11 +374,10 @@ bool GNSSFlowgraph::send_telemetry_msg(pmt::pmt_t msg) void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) { DLOG(INFO) << "received " << what << " from " << who; - switch (what) { case 0: - LOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); available_GNSS_signals_.push_back(channels_.at(who)->get_signal()); //TODO: Optimize the channel and signal matching! while ( channels_.at(who)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) @@ -388,15 +387,33 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } channels_.at(who)->set_signal(available_GNSS_signals_.front()); available_GNSS_signals_.pop_front(); - usleep(100); - LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); channels_.at(who)->start_acquisition(); break; + case 1: - LOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); + DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); channels_state_[who] = 2; acq_channels_count_--; - if (!available_GNSS_signals_.empty() && acq_channels_count_ < max_acq_channels_) + for (unsigned int i = 0; i < channels_count_; i++) + { + if(!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_) && (channels_state_[i] == 0)) + { + channels_state_[i] = 1; + while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0) + { + available_GNSS_signals_.push_back(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + } + channels_.at(i)->set_signal(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + acq_channels_count_++; + channels_.at(i)->start_acquisition(); + } + DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; + } + /* + if (!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_)) { for (unsigned int i = 0; i < channels_count_; i++) { @@ -417,11 +434,12 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } } - + */ break; case 2: - LOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + DLOG(INFO) << "Number of channels in acquisition = " << acq_channels_count_; if (acq_channels_count_ < max_acq_channels_) { channels_state_[who] = 1; @@ -433,17 +451,13 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_state_[who] = 0; available_GNSS_signals_.push_back( channels_.at(who)->get_signal() ); } - - // for (unsigned int i = 0; i < channels_count_; i++) - // { - // LOG(INFO) << "Channel " << i << " in state " << channels_state_[i] << std::endl; - // } break; default: break; } DLOG(INFO) << "Number of available signals: " << available_GNSS_signals_.size(); + applied_actions_++; } @@ -555,7 +569,6 @@ void GNSSFlowgraph::init() set_signals_list(); set_channels_state(); applied_actions_ = 0; - DLOG(INFO) << "Blocks instantiated. " << channels_count_ << " channels."; } @@ -704,7 +717,7 @@ void GNSSFlowgraph::set_signals_list() if (configuration_->property("Channels_1B.count", 0) > 0) { /* - * Loop to create the list of Galileo E1 B signals + * Loop to create the list of Galileo E1B signals */ for (available_gnss_prn_iter = available_galileo_prn.cbegin(); available_gnss_prn_iter != available_galileo_prn.cend(); @@ -718,7 +731,7 @@ void GNSSFlowgraph::set_signals_list() if (configuration_->property("Channels_5X.count", 0) > 0 ) { /* - * Loop to create the list of Galileo E1 B signals + * Loop to create the list of Galileo E5a signals */ for (available_gnss_prn_iter = available_galileo_prn.cbegin(); available_gnss_prn_iter != available_galileo_prn.cend(); @@ -754,14 +767,6 @@ void GNSSFlowgraph::set_signals_list() gnss_it = available_GNSS_signals_.insert(gnss_it, signal_value); } } - - // **** FOR DEBUGGING THE LIST OF GNSS SIGNALS **** - // std::list::const_iterator available_gnss_list_iter; - // for (available_gnss_list_iter = available_GNSS_signals_.cbegin(); available_gnss_list_iter - // != available_GNSS_signals_.cend(); available_gnss_list_iter++) - // { - // std::cout << *available_gnss_list_iter << std::endl; - // } } @@ -776,12 +781,8 @@ void GNSSFlowgraph::set_channels_state() channels_state_.reserve(channels_count_); for (unsigned int i = 0; i < channels_count_; i++) { - if (i < max_acq_channels_) - { - channels_state_.push_back(1); - } - else - channels_state_.push_back(0); + if (i < max_acq_channels_) {channels_state_.push_back(1);} + else {channels_state_.push_back(0);} DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } acq_channels_count_ = max_acq_channels_; diff --git a/src/core/system_parameters/gnss_signal.h b/src/core/system_parameters/gnss_signal.h index 4922e5399..25288a704 100644 --- a/src/core/system_parameters/gnss_signal.h +++ b/src/core/system_parameters/gnss_signal.h @@ -51,7 +51,7 @@ public: Gnss_Signal(const std::string& signal_); Gnss_Signal(const Gnss_Satellite& satellite_, const std::string& signal_); ~Gnss_Signal(); - std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1B" for Galileo E1B, "5X" for Galileo E5a} + std::string get_signal_str() const; //!< Get the satellite signal {"1C" for GPS L1 C/A, "2S" for GPS L2C (M), "1B" for Galileo E1B, "5X" for Galileo E5a, "L5" for GPS L5} Gnss_Satellite get_satellite() const; //!< Get the Gnss_Satellite associated to the signal friend bool operator== (const Gnss_Signal &, const Gnss_Signal &); //!< operator== for comparison friend std::ostream& operator<<(std::ostream &, const Gnss_Signal &); //!< operator<< for pretty printing From 8bb0c880c032c2a4045058203cf17947d18624b2 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 20 Dec 2017 16:12:08 +0100 Subject: [PATCH 09/55] Minor changes in DLOG lines --- src/algorithms/PVT/libs/rtklib_solver.cc | 2 +- src/algorithms/channel/libs/channel_fsm.cc | 3 +-- .../channel/libs/channel_msg_receiver_cc.cc | 4 ---- src/core/receiver/gnss_flowgraph.cc | 15 ++++++++------- 4 files changed, 10 insertions(+), 14 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 726225998..93685eb24 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -349,7 +349,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); if(result == 0) { - LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; + DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; this->set_time_offset_s(0.0); //reset rx time estimation this->set_num_valid_observations(0); } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 44ee7bce5..85905bfd8 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -115,8 +115,7 @@ public: typedef sc::transition reactions; - channel_waiting_fsm_S3(my_context ctx) : - my_base(ctx) + channel_waiting_fsm_S3(my_context ctx) : my_base(ctx) { //std::cout << "Enter Channel_waiting_S3 " << std::endl; context ().request_satellite(); diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc index 573c1aa84..d22f2dd73 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc @@ -50,13 +50,9 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) switch (message) { case 1: //positive acquisition - //DLOG(INFO) << "Channel " << channel_ << " ACQ SUCCESS satellite " << - // gnss_synchro_.System << " " << gnss_synchro_.PRN; d_channel_fsm->Event_valid_acquisition(); break; case 2: //negative acquisition - //DLOG(INFO) << "Channel " << channel_ - // << " ACQ FAILED satellite " << gnss_synchro_.System << " " << gnss_synchro_.PRN; if (d_repeat == true) { d_channel_fsm->Event_failed_acquisition_repeat(); diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index a535c90fd..609908097 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -91,12 +91,6 @@ void GNSSFlowgraph::start() void GNSSFlowgraph::stop() { - // for (unsigned int i = 0; i < channels_count_; i++) - // { - // channels_.at(i)->stop_channel(); - // LOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; - // } - // LOG(INFO) << "Threads finished. Return to main program."; top_block_->stop(); running_ = false; } @@ -373,11 +367,13 @@ bool GNSSFlowgraph::send_telemetry_msg(pmt::pmt_t msg) */ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) { - DLOG(INFO) << "received " << what << " from " << who; + DLOG(INFO) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_; + VLOG(-100) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_; switch (what) { case 0: DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + VLOG(-100) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); available_GNSS_signals_.push_back(channels_.at(who)->get_signal()); //TODO: Optimize the channel and signal matching! while ( channels_.at(who)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) @@ -388,11 +384,13 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_.at(who)->set_signal(available_GNSS_signals_.front()); available_GNSS_signals_.pop_front(); DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + VLOG(-100) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); channels_.at(who)->start_acquisition(); break; case 1: DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); + VLOG(-100) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); channels_state_[who] = 2; acq_channels_count_--; for (unsigned int i = 0; i < channels_count_; i++) @@ -440,6 +438,9 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) case 2: DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); DLOG(INFO) << "Number of channels in acquisition = " << acq_channels_count_; + VLOG(-100) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + VLOG(-100) << "Number of channels in acquisition = " << acq_channels_count_; + if (acq_channels_count_ < max_acq_channels_) { channels_state_[who] = 1; From 2d8141d9b518853e02cb3fb5883f78b115c34e24 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 21 Dec 2017 13:05:21 +0100 Subject: [PATCH 10/55] Migrate msg_queue boost smart pointers to gr sptr --- src/algorithms/channel/adapters/channel.cc | 2 +- src/algorithms/channel/adapters/channel.h | 2 +- src/algorithms/channel/libs/channel_fsm.cc | 2 +- src/algorithms/channel/libs/channel_fsm.h | 4 ++-- src/core/receiver/control_message_factory.cc | 6 +++--- src/core/receiver/control_message_factory.h | 2 +- src/core/receiver/control_thread.cc | 4 ++-- src/core/receiver/control_thread.h | 4 ++-- src/core/receiver/gnss_flowgraph.cc | 3 +-- src/core/receiver/gnss_flowgraph.h | 2 +- 10 files changed, 15 insertions(+), 16 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 425dbb968..f66353f7e 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -40,7 +40,7 @@ using google::LogMessage; Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, std::shared_ptr pass_through, std::shared_ptr acq, std::shared_ptr trk, std::shared_ptr nav, - std::string role, std::string implementation, boost::shared_ptr queue) + std::string role, std::string implementation, gr::msg_queue::sptr queue) { pass_through_ = pass_through; acq_ = acq; diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index a05697eed..8e0893843 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -105,7 +105,7 @@ private: bool connected_; bool repeat_; ChannelFsm channel_fsm_; - boost::shared_ptr queue_; + gr::msg_queue::sptr queue_; }; #endif /*GNSS_SDR_CHANNEL_H_*/ diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 85905bfd8..f147a57ef 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -194,7 +194,7 @@ void ChannelFsm::set_tracking(std::shared_ptr tracking) trk_ = tracking; } -void ChannelFsm::set_queue(boost::shared_ptr queue) +void ChannelFsm::set_queue(gr::msg_queue::sptr queue) { queue_ = queue; } diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 868388b7b..997056723 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -59,7 +59,7 @@ public: void set_acquisition(std::shared_ptr acquisition); void set_tracking(std::shared_ptr tracking); - void set_queue(boost::shared_ptr queue); + void set_queue(gr::msg_queue::sptr queue); void set_channel(unsigned int channel); void start_acquisition(); void start_tracking(); @@ -77,7 +77,7 @@ public: private: std::shared_ptr acq_; std::shared_ptr trk_; - boost::shared_ptr queue_; + gr::msg_queue::sptr queue_; unsigned int channel_; }; diff --git a/src/core/receiver/control_message_factory.cc b/src/core/receiver/control_message_factory.cc index 6f7994c29..a7da90ab1 100644 --- a/src/core/receiver/control_message_factory.cc +++ b/src/core/receiver/control_message_factory.cc @@ -44,18 +44,18 @@ ControlMessageFactory::~ControlMessageFactory() {} -boost::shared_ptr ControlMessageFactory::GetQueueMessage(unsigned int who, unsigned int what) +gr::message::sptr ControlMessageFactory::GetQueueMessage(unsigned int who, unsigned int what) { std::shared_ptr control_message = std::make_shared(); control_message->who = who; control_message->what = what; - boost::shared_ptr queue_message = gr::message::make(0, 0, 0, sizeof(ControlMessage)); + gr::message::sptr queue_message = gr::message::make(0, 0, 0, sizeof(ControlMessage)); memcpy(queue_message->msg(), control_message.get(), sizeof(ControlMessage)); return queue_message; } -std::shared_ptr>> ControlMessageFactory::GetControlMessages(boost::shared_ptr queue_message) +std::shared_ptr>> ControlMessageFactory::GetControlMessages(gr::message::sptr queue_message) { std::shared_ptr>> control_messages = std::make_shared>>(); unsigned int control_messages_count = queue_message->length() / sizeof(ControlMessage); diff --git a/src/core/receiver/control_message_factory.h b/src/core/receiver/control_message_factory.h index 2ff294a8c..9bdd49d32 100644 --- a/src/core/receiver/control_message_factory.h +++ b/src/core/receiver/control_message_factory.h @@ -58,7 +58,7 @@ public: //! Virtual destructor virtual ~ControlMessageFactory(); - boost::shared_ptr GetQueueMessage(unsigned int who, unsigned int what); + gr::message::sptr GetQueueMessage(unsigned int who, unsigned int what); std::shared_ptr>> GetControlMessages(gr::message::sptr queue_message); }; diff --git a/src/core/receiver/control_thread.cc b/src/core/receiver/control_thread.cc index 44cdc36d3..73b05651c 100644 --- a/src/core/receiver/control_thread.cc +++ b/src/core/receiver/control_thread.cc @@ -158,7 +158,7 @@ void ControlThread::run() } -void ControlThread::set_control_queue(boost::shared_ptr control_queue) +void ControlThread::set_control_queue(gr::msg_queue::sptr control_queue) { if (flowgraph_->running()) { @@ -445,7 +445,7 @@ void ControlThread::init() void ControlThread::read_control_messages() { DLOG(INFO) << "Reading control messages from queue"; - boost::shared_ptr queue_message = control_queue_->delete_head(); + gr::message::sptr queue_message = control_queue_->delete_head(); if (queue_message != 0) { control_messages_ = control_message_factory_->GetControlMessages(queue_message); diff --git a/src/core/receiver/control_thread.h b/src/core/receiver/control_thread.h index 1580695df..ea0decec8 100644 --- a/src/core/receiver/control_thread.h +++ b/src/core/receiver/control_thread.h @@ -89,7 +89,7 @@ public: * * \param[in] boost::shared_ptr control_queue */ - void set_control_queue(boost::shared_ptr control_queue); + void set_control_queue(gr::msg_queue::sptr control_queue); unsigned int processed_control_messages() @@ -146,7 +146,7 @@ private: void apply_action(unsigned int what); std::shared_ptr flowgraph_; std::shared_ptr configuration_; - boost::shared_ptr control_queue_; + gr::msg_queue::sptr control_queue_; std::shared_ptr control_message_factory_; std::shared_ptr>> control_messages_; bool stop_; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 609908097..8bbd9a20c 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -51,8 +51,7 @@ using google::LogMessage; -GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr configuration, - boost::shared_ptr queue) +GNSSFlowgraph::GNSSFlowgraph(std::shared_ptr configuration, gr::msg_queue::sptr queue) { connected_ = false; running_ = false; diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index d765d3cf4..4998d7170 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -139,7 +139,7 @@ private: std::vector> channels_; gnss_sdr_sample_counter_sptr ch_out_sample_counter; gr::top_block_sptr top_block_; - boost::shared_ptr queue_; + gr::msg_queue::sptr queue_; std::list available_GNSS_signals_; std::vector channels_state_; }; From 8932e9a51bcf6ce68fbf1d51027e4837eb95147c Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 21 Dec 2017 16:20:11 +0100 Subject: [PATCH 11/55] Clean acquisition interface --- src/core/interfaces/acquisition_interface.h | 1 - src/core/receiver/gnss_block_factory.cc | 16 ++++++++-------- src/core/receiver/gnss_block_factory.h | 18 +++++++++--------- 3 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/core/interfaces/acquisition_interface.h b/src/core/interfaces/acquisition_interface.h index 722b99777..b6d9e4e72 100644 --- a/src/core/interfaces/acquisition_interface.h +++ b/src/core/interfaces/acquisition_interface.h @@ -53,7 +53,6 @@ templateclass concurrent_queue; class AcquisitionInterface: public GNSSBlockInterface { public: - //virtual void set_active(bool active) = 0; virtual void set_gnss_synchro(Gnss_Synchro* gnss_synchro) = 0; virtual void set_channel(unsigned int channel) = 0; virtual void set_threshold(float threshold) = 0; diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index c3e3889fb..bdf34dc22 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -151,7 +151,7 @@ GNSSBlockFactory::~GNSSBlockFactory() std::unique_ptr GNSSBlockFactory::GetSignalSource( - std::shared_ptr configuration, boost::shared_ptr queue, int ID) + std::shared_ptr configuration, gr::msg_queue::sptr queue, int ID) { std::string default_implementation = "File_Signal_Source"; std::string role = "SignalSource"; //backwards compatibility for old conf files @@ -264,7 +264,7 @@ std::unique_ptr GNSSBlockFactory::GetPVT(std::shared_ptr GNSSBlockFactory::GetChannel_1C( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { //"appendix" is added to the "role" with the aim of Acquisition, Tracking and Telemetry Decoder adapters //can find their specific configurations when they read the config @@ -332,7 +332,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_1C( std::unique_ptr GNSSBlockFactory::GetChannel_2S( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { LOG(INFO) << "Instantiating Channel " << channel << " with Acquisition Implementation: " @@ -397,7 +397,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_2S( std::unique_ptr GNSSBlockFactory::GetChannel_1B( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { std::stringstream stream; stream << channel; @@ -464,7 +464,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_1B( std::unique_ptr GNSSBlockFactory::GetChannel_5X( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { std::stringstream stream; stream << channel; @@ -531,7 +531,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_5X( std::unique_ptr GNSSBlockFactory::GetChannel_L5( std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue) + gr::msg_queue::sptr queue) { std::stringstream stream; stream << channel; @@ -597,7 +597,7 @@ std::unique_ptr GNSSBlockFactory::GetChannel_L5( std::unique_ptr>> GNSSBlockFactory::GetChannels( - std::shared_ptr configuration, boost::shared_ptr queue) + std::shared_ptr configuration, gr::msg_queue::sptr queue) { std::string default_implementation = "Pass_Through"; std::string tracking_implementation; @@ -783,7 +783,7 @@ std::unique_ptr GNSSBlockFactory::GetBlock( std::shared_ptr configuration, std::string role, std::string implementation, unsigned int in_streams, - unsigned int out_streams, boost::shared_ptr queue) + unsigned int out_streams, gr::msg_queue::sptr queue) { std::unique_ptr block; diff --git a/src/core/receiver/gnss_block_factory.h b/src/core/receiver/gnss_block_factory.h index 516b9e8e9..bba584d7a 100644 --- a/src/core/receiver/gnss_block_factory.h +++ b/src/core/receiver/gnss_block_factory.h @@ -57,7 +57,7 @@ public: GNSSBlockFactory(); virtual ~GNSSBlockFactory(); std::unique_ptr GetSignalSource(std::shared_ptr configuration, - boost::shared_ptr queue, int ID = -1); + gr::msg_queue::sptr queue, int ID = -1); std::unique_ptr GetSignalConditioner(std::shared_ptr configuration, int ID = -1); @@ -66,7 +66,7 @@ public: std::unique_ptr GetObservables(std::shared_ptr configuration); std::unique_ptr>> GetChannels(std::shared_ptr configuration, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); /* * \brief Returns the block with the required configuration and implementation @@ -74,29 +74,29 @@ public: std::unique_ptr GetBlock(std::shared_ptr configuration, std::string role, std::string implementation, unsigned int in_streams, unsigned int out_streams, - boost::shared_ptr queue = nullptr); + gr::msg_queue::sptr queue = nullptr); private: std::unique_ptr GetChannel_1C(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_2S(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_1B(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_5X(std::shared_ptr configuration, std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + gr::msg_queue::sptr queue); std::unique_ptr GetChannel_L5(std::shared_ptr configuration, - std::string acq, std::string trk, std::string tlm, int channel, - boost::shared_ptr queue); + std::string acq, std::string trk, std::string tlm, int channel, + gr::msg_queue::sptr queue); std::unique_ptr GetAcqBlock( std::shared_ptr configuration, From 3eafe2047aa2753d66d23bc4edbb1763eb9b74df Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 2 Jan 2018 12:29:15 +0100 Subject: [PATCH 12/55] Fix deadlock flowgraph --- src/algorithms/channel/libs/channel_fsm.cc | 19 +++++-------------- .../gps_l1_ca_telemetry_decoder_cc.cc | 2 +- src/core/receiver/gnss_flowgraph.cc | 18 +++++++----------- 3 files changed, 13 insertions(+), 26 deletions(-) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index f147a57ef..daef2f9ad 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -62,10 +62,8 @@ struct channel_idle_fsm_S0: public sc::state public: // sc::transition(event, next state) typedef sc::transition reactions; - channel_idle_fsm_S0(my_context ctx) : my_base(ctx) - { - //std::cout << "Enter Channel_Idle_S0 " << std::endl; - } + channel_idle_fsm_S0(my_context ctx) : my_base(ctx){} + }; @@ -78,13 +76,10 @@ public: channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx) { - //std::cout << "Enter Channel_Acq_S1 " << std::endl; context ().start_acquisition(); } - ~channel_acquiring_fsm_S1() - { - //std::cout << "Exit Channel_Acq_S1 " << std::endl; - } + ~channel_acquiring_fsm_S1(){} + }; @@ -96,13 +91,11 @@ public: channel_tracking_fsm_S2(my_context ctx) : my_base(ctx) { - //std::cout << "Enter Channel_tracking_S2 " << std::endl; context ().start_tracking(); } ~channel_tracking_fsm_S2() { - //std::cout << "Exit Channel_tracking_S2 " << std::endl; context ().notify_stop_tracking(); } @@ -117,10 +110,9 @@ public: channel_waiting_fsm_S3(my_context ctx) : my_base(ctx) { - //std::cout << "Enter Channel_waiting_S3 " << std::endl; context ().request_satellite(); } - // ~channel_waiting_fsm_S3(){} + ~channel_waiting_fsm_S3(){} }; @@ -148,7 +140,6 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); - //std::cout<<"Ev_channel_start_acquisition launched"<start_acquisition(); available_GNSS_signals_.pop_front(); - LOG(INFO) << "Channel " << i << " assigned to " << available_GNSS_signals_.front(); + LOG(INFO) << "Channel " << i << " assigned to " << channels_.at(i)->get_signal(); LOG(INFO) << "Channel " << i << " connected to observables and ready for acquisition"; } else @@ -367,12 +367,10 @@ bool GNSSFlowgraph::send_telemetry_msg(pmt::pmt_t msg) void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) { DLOG(INFO) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_; - VLOG(-100) << "Received " << what << " from " << who << ". Number of applied actions = " << applied_actions_; switch (what) { case 0: - DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); - VLOG(-100) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + LOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); available_GNSS_signals_.push_back(channels_.at(who)->get_signal()); //TODO: Optimize the channel and signal matching! while ( channels_.at(who)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) @@ -382,14 +380,12 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } channels_.at(who)->set_signal(available_GNSS_signals_.front()); available_GNSS_signals_.pop_front(); - DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); - VLOG(-100) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); channels_.at(who)->start_acquisition(); break; case 1: - DLOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); - VLOG(-100) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); + LOG(INFO) << "Channel " << who << " ACQ SUCCESS satellite " << channels_.at(who)->get_signal().get_satellite(); channels_state_[who] = 2; acq_channels_count_--; for (unsigned int i = 0; i < channels_count_; i++) @@ -435,20 +431,20 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) break; case 2: - DLOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); + LOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); DLOG(INFO) << "Number of channels in acquisition = " << acq_channels_count_; - VLOG(-100) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); - VLOG(-100) << "Number of channels in acquisition = " << acq_channels_count_; if (acq_channels_count_ < max_acq_channels_) { channels_state_[who] = 1; acq_channels_count_++; + LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); channels_.at(who)->start_acquisition(); } else { channels_state_[who] = 0; + LOG(INFO) << "Channel "<< who << " Idle state"; available_GNSS_signals_.push_back( channels_.at(who)->get_signal() ); } break; From b28c244df5cea69f9f1d8236271d14d7001979d9 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 11:40:17 +0100 Subject: [PATCH 13/55] Debug log lines --- .../acquisition/gnuradio_blocks/pcps_acquisition_cc.cc | 4 +++- .../acquisition/gnuradio_blocks/pcps_acquisition_cc.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index a0461f9c5..fdcd895b4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -329,6 +329,7 @@ int pcps_acquisition_cc::general_work(int noutput_items, d_input_power = 0.0; d_test_statistics = 0.0; d_state = 1; + LOG(INFO) << "ACQ. general_work changing to state 1 (active)"; } d_sample_counter += d_fft_size * ninput_items[0]; // sample counter @@ -368,8 +369,9 @@ int pcps_acquisition_cc::general_work(int noutput_items, if( d_blocking ) { + LOG(INFO) << "ACQ. general_work waiting for acquisition_core"; d_cond.wait( lk, [&]{ return !this->d_new_data_available; } ); - + LOG(INFO) << "ACQ. Exiting general_work"; } } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 486468379..1c6162492 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -60,6 +60,7 @@ #include #include #include "gnss_synchro.h" +#include class pcps_acquisition_cc; @@ -194,6 +195,7 @@ public: { gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_active = active; + LOG(INFO) << "ACQ RESET"; } /*! From 47c752c141ba95bb3c25c424c7d6f5c4c8ea1e5b Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 12:44:58 +0100 Subject: [PATCH 14/55] More debug lines --- src/algorithms/channel/adapters/channel.cc | 1 + src/algorithms/channel/libs/channel_fsm.cc | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index f66353f7e..ed8832c3f 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -202,5 +202,6 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { channel_fsm_.Event_start_acquisition(); + LOG(INFO) << "Channel start_acquisition()"; } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index daef2f9ad..0d5a1606e 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -140,6 +140,7 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); + LOG(INFO) << "FSM Event_start_acquisition" DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; } @@ -198,6 +199,7 @@ void ChannelFsm::set_channel(unsigned int channel) void ChannelFsm::start_acquisition() { acq_->reset(); + LOG(INFO) << "FSM. start_acquisition()"; } void ChannelFsm::start_tracking() From 3ddea680ad9ad9437e5420b3f2489c263e26154b Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 12:48:04 +0100 Subject: [PATCH 15/55] debug1 --- src/algorithms/channel/libs/channel_fsm.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 0d5a1606e..dd3dbbd29 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -140,7 +140,7 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { this->process_event(Ev_channel_start_acquisition()); - LOG(INFO) << "FSM Event_start_acquisition" + LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; } From 6dd3abfdea8e0bf6cbb2ef4fbf5000a95f015c04 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 3 Jan 2018 15:47:16 +0100 Subject: [PATCH 16/55] debug2 --- src/algorithms/channel/libs/channel_fsm.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index dd3dbbd29..68e0798aa 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -139,6 +139,7 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : void ChannelFsm::Event_start_acquisition() { + initiate(); this->process_event(Ev_channel_start_acquisition()); LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; From 540221e227d0cba95bed6a6f6d1504801e6d9b6d Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 4 Jan 2018 10:51:47 +0100 Subject: [PATCH 17/55] New Channel FSM --- src/algorithms/channel/adapters/channel.cc | 6 +-- src/algorithms/channel/libs/channel_fsm.cc | 49 ++++++++++++---------- src/algorithms/channel/libs/channel_fsm.h | 30 ++++++------- 3 files changed, 42 insertions(+), 43 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index ed8832c3f..8689a8d94 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -103,10 +103,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, // Destructor -Channel::~Channel() -{ - channel_fsm_.terminate(); -} +Channel::~Channel(){} void Channel::connect(gr::top_block_sptr top_block) @@ -139,7 +136,6 @@ void Channel::connect(gr::top_block_sptr top_block) top_block->msg_connect(nav_->get_left_block(), pmt::mp("preamble_timestamp_s"), trk_->get_right_block(), pmt::mp("preamble_timestamp_s")); DLOG(INFO) << "MSG FEEDBACK CHANNEL telemetry_decoder -> tracking"; - //std::cout<<"has port: "<get_right_block()->has_msg_port(pmt::mp("events"))<msg_connect(acq_->get_right_block(), pmt::mp("events"), channel_msg_rx, pmt::mp("events")); top_block->msg_connect(trk_->get_right_block(), pmt::mp("events"), channel_msg_rx, pmt::mp("events")); diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 68e0798aa..e5cb34493 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -30,15 +30,10 @@ #include "channel_fsm.h" #include -#include -#include -#include -#include -#include #include #include "control_message_factory.h" - +/* struct Ev_channel_start_acquisition: sc::event {}; @@ -53,10 +48,9 @@ struct Ev_channel_failed_acquisition_no_repeat: sc::event {}; +*/ -//struct Ev_channel_failed_tracking_reacq: sc::event -//{}; - +/* struct channel_idle_fsm_S0: public sc::state { public: @@ -114,7 +108,7 @@ public: } ~channel_waiting_fsm_S3(){} }; - +*/ ChannelFsm::ChannelFsm() @@ -122,7 +116,7 @@ ChannelFsm::ChannelFsm() acq_ = nullptr; trk_ = nullptr; channel_ = 0; - initiate(); //start the FSM + d_state = 0; } @@ -132,51 +126,60 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : { trk_ = nullptr; channel_ = 0; - initiate(); //start the FSM + d_state = 0; } void ChannelFsm::Event_start_acquisition() { - initiate(); - this->process_event(Ev_channel_start_acquisition()); + mx.lock(); + d_state = 1; + start_acquisition(); LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; + mx.unlock(); } void ChannelFsm::Event_valid_acquisition() { - this->process_event(Ev_channel_valid_acquisition()); + mx.lock(); + d_state = 2; + start_tracking(); DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; + mx.unlock(); } void ChannelFsm::Event_failed_acquisition_repeat() { - this->process_event(Ev_channel_failed_acquisition_repeat()); + mx.lock(); + d_state = 1; + start_acquisition(); DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; + mx.unlock(); } void ChannelFsm::Event_failed_acquisition_no_repeat() { - this->process_event(Ev_channel_failed_acquisition_no_repeat()); + mx.lock(); + d_state = 3; + request_satellite(); DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; + mx.unlock(); } -// Something is wrong here, we are using a memory after it ts freed void ChannelFsm::Event_failed_tracking_standby() { - this->process_event(Ev_channel_failed_tracking_standby()); + mx.lock(); + d_state = 0; + notify_stop_tracking(); DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; + mx.unlock(); } -//void ChannelFsm::Event_failed_tracking_reacq() { -// this->process_event(Ev_channel_failed_tracking_reacq()); -//} - void ChannelFsm::set_acquisition(std::shared_ptr acquisition) { acq_ = acquisition; diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 997056723..02be4f1fc 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -1,12 +1,12 @@ /*! * \file channel_fsm.h - * \brief Interface of the State Machine for channel using boost::statechart - * \author Luis Esteve, 2011. luis(at)epsilon-formacion.com + * \brief Interface of the State Machine for channel + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es * * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -32,26 +32,23 @@ #ifndef GNSS_SDR_CHANNEL_FSM_H #define GNSS_SDR_CHANNEL_FSM_H - -#include +#include #include #include "acquisition_interface.h" #include "tracking_interface.h" #include "telemetry_decoder_interface.h" - -namespace sc = boost::statechart; -namespace mpl = boost::mpl; - +/* struct channel_idle_fsm_S0; struct channel_acquiring_fsm_S1; struct channel_tracking_fsm_S2; struct channel_waiting_fsm_S3; +*/ /*! * \brief This class implements a State Machine for channel using boost::statechart */ -class ChannelFsm: public sc::state_machine +class ChannelFsm { public: ChannelFsm(); @@ -61,24 +58,27 @@ public: void set_tracking(std::shared_ptr tracking); void set_queue(gr::msg_queue::sptr queue); void set_channel(unsigned int channel); - void start_acquisition(); - void start_tracking(); - void request_satellite(); - void notify_stop_tracking(); //FSM EVENTS void Event_start_acquisition(); void Event_valid_acquisition(); void Event_failed_acquisition_repeat(); void Event_failed_acquisition_no_repeat(); - //void Event_gps_failed_tracking_reacq(); void Event_failed_tracking_standby(); private: + + void start_acquisition(); + void start_tracking(); + void request_satellite(); + void notify_stop_tracking(); + std::shared_ptr acq_; std::shared_ptr trk_; gr::msg_queue::sptr queue_; unsigned int channel_; + unsigned int d_state; + std::mutex mx; }; #endif /*GNSS_SDR_CHANNEL_FSM_H*/ From bbf8587970d6bfd4ac2d0b9a7df238b80a441d98 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 4 Jan 2018 12:16:12 +0100 Subject: [PATCH 18/55] Clean code --- .../gnuradio_blocks/pcps_acquisition_cc.cc | 3 - .../gnuradio_blocks/pcps_acquisition_cc.h | 1 - src/algorithms/channel/libs/channel_fsm.cc | 85 +------------------ src/algorithms/channel/libs/channel_fsm.h | 9 +- src/core/receiver/gnss_flowgraph.cc | 23 ----- 5 files changed, 4 insertions(+), 117 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index fdcd895b4..60ad2e82c 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -329,7 +329,6 @@ int pcps_acquisition_cc::general_work(int noutput_items, d_input_power = 0.0; d_test_statistics = 0.0; d_state = 1; - LOG(INFO) << "ACQ. general_work changing to state 1 (active)"; } d_sample_counter += d_fft_size * ninput_items[0]; // sample counter @@ -369,9 +368,7 @@ int pcps_acquisition_cc::general_work(int noutput_items, if( d_blocking ) { - LOG(INFO) << "ACQ. general_work waiting for acquisition_core"; d_cond.wait( lk, [&]{ return !this->d_new_data_available; } ); - LOG(INFO) << "ACQ. Exiting general_work"; } } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 1c6162492..e33746f2b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -195,7 +195,6 @@ public: { gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_active = active; - LOG(INFO) << "ACQ RESET"; } /*! diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index e5cb34493..75ac0e3da 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -1,11 +1,11 @@ /*! * \file channel_fsm.cc - * \brief Implementation of a State Machine for channel using boost::statechart - * \author Luis Esteve, 2011. luis(at)epsilon-formacion.com + * \brief Implementation of a State Machine for channel + * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -33,83 +33,6 @@ #include #include "control_message_factory.h" -/* -struct Ev_channel_start_acquisition: sc::event -{}; - -struct Ev_channel_valid_acquisition: sc::event -{}; - -struct Ev_channel_failed_acquisition_repeat: sc::event -{}; - -struct Ev_channel_failed_acquisition_no_repeat: sc::event -{}; - -struct Ev_channel_failed_tracking_standby: sc::event -{}; -*/ - -/* -struct channel_idle_fsm_S0: public sc::state -{ -public: - // sc::transition(event, next state) - typedef sc::transition reactions; - channel_idle_fsm_S0(my_context ctx) : my_base(ctx){} - -}; - - -struct channel_acquiring_fsm_S1: public sc::state -{ -public: - typedef mpl::list, - sc::transition, - sc::transition > reactions; - - channel_acquiring_fsm_S1(my_context ctx) : my_base(ctx) - { - context ().start_acquisition(); - } - ~channel_acquiring_fsm_S1(){} - -}; - - -struct channel_tracking_fsm_S2: public sc::state -{ -public: - typedef mpl::list, - sc::transition> reactions; - - channel_tracking_fsm_S2(my_context ctx) : my_base(ctx) - { - context ().start_tracking(); - } - - ~channel_tracking_fsm_S2() - { - context ().notify_stop_tracking(); - } - -}; - - -struct channel_waiting_fsm_S3: public sc::state -{ -public: - typedef sc::transition reactions; - - channel_waiting_fsm_S3(my_context ctx) : my_base(ctx) - { - context ().request_satellite(); - } - ~channel_waiting_fsm_S3(){} -}; -*/ - ChannelFsm::ChannelFsm() { @@ -136,7 +59,6 @@ void ChannelFsm::Event_start_acquisition() mx.lock(); d_state = 1; start_acquisition(); - LOG(INFO) << "FSM Event_start_acquisition"; DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; mx.unlock(); } @@ -203,7 +125,6 @@ void ChannelFsm::set_channel(unsigned int channel) void ChannelFsm::start_acquisition() { acq_->reset(); - LOG(INFO) << "FSM. start_acquisition()"; } void ChannelFsm::start_tracking() diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 02be4f1fc..55068dc85 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -38,15 +38,8 @@ #include "tracking_interface.h" #include "telemetry_decoder_interface.h" -/* -struct channel_idle_fsm_S0; -struct channel_acquiring_fsm_S1; -struct channel_tracking_fsm_S2; -struct channel_waiting_fsm_S3; -*/ - /*! - * \brief This class implements a State Machine for channel using boost::statechart + * \brief This class implements a State Machine for channel */ class ChannelFsm { diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index ed47b1a3e..c63b56435 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -405,29 +405,6 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) } DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } - /* - if (!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_)) - { - for (unsigned int i = 0; i < channels_count_; i++) - { - if (channels_state_[i] == 0) - { - channels_state_[i] = 1; - while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) - { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - } - channels_.at(i)->set_signal(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - acq_channels_count_++; - channels_.at(i)->start_acquisition(); - break; - } - DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; - } - } - */ break; case 2: From 3a37345d4c4bf8269f192ff30de10d82529e8e2e Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 4 Jan 2018 12:33:13 +0100 Subject: [PATCH 19/55] Minor changes --- src/algorithms/channel/adapters/channel.cc | 2 +- src/algorithms/channel/libs/channel_fsm.cc | 3 ++- src/algorithms/channel/libs/channel_fsm.h | 4 ++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 8689a8d94..f4c7fa77d 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -198,6 +198,6 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { channel_fsm_.Event_start_acquisition(); - LOG(INFO) << "Channel start_acquisition()"; + DLOG(INFO) << "Channel start_acquisition()"; } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 75ac0e3da..f218981c6 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -1,7 +1,8 @@ /*! * \file channel_fsm.cc * \brief Implementation of a State Machine for channel - * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * \authors Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * Luis Esteve, 2011. luis(at)epsilon-formacion.com * * ------------------------------------------------------------------------- * diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 55068dc85..5cca3b53e 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -1,8 +1,8 @@ /*! * \file channel_fsm.h * \brief Interface of the State Machine for channel - * \author Antonio Ramos, 2017. antonio.ramos(at)cttc.es - * + * \authors Antonio Ramos, 2017. antonio.ramos(at)cttc.es + * Luis Esteve, 2011. luis(at)epsilon-formacion.com * * ------------------------------------------------------------------------- * From cfc0a4a498b856060933426d1408b96fe3ead774 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 5 Jan 2018 11:50:52 +0100 Subject: [PATCH 20/55] Clean flowgraph --- src/algorithms/channel/libs/channel_fsm.cc | 1 - src/algorithms/channel/libs/channel_fsm.h | 1 + src/core/receiver/gnss_flowgraph.cc | 41 +++++++++------------- src/core/receiver/gnss_flowgraph.h | 4 +-- 4 files changed, 20 insertions(+), 27 deletions(-) diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index f218981c6..499d55881 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -30,7 +30,6 @@ */ #include "channel_fsm.h" -#include #include #include "control_message_factory.h" diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 5cca3b53e..29f741bfc 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -33,6 +33,7 @@ #define GNSS_SDR_CHANNEL_FSM_H #include +#include #include #include "acquisition_interface.h" #include "tracking_interface.h" diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index c63b56435..cf1226102 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -33,7 +33,6 @@ */ #include "gnss_flowgraph.h" -#include #include #include #include @@ -284,12 +283,7 @@ void GNSSFlowgraph::connect() } std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str(); // use channel's implicit signal! - while (gnss_signal.compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) - { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - } - channels_.at(i)->set_signal(available_GNSS_signals_.front()); + channels_.at(i)->set_signal(search_next_signal(gnss_signal, false)); if (channels_state_[i] == 1) { @@ -370,17 +364,10 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) switch (what) { case 0: - LOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + DLOG(INFO) << "Channel " << who << " ACQ FAILED satellite " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); available_GNSS_signals_.push_back(channels_.at(who)->get_signal()); - //TODO: Optimize the channel and signal matching! - while ( channels_.at(who)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) - { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - } - channels_.at(who)->set_signal(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); + channels_.at(who)->set_signal(search_next_signal(channels_.at(who)->get_signal().get_signal_str(), true)); + DLOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); channels_.at(who)->start_acquisition(); break; @@ -393,13 +380,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) if(!available_GNSS_signals_.empty() && (acq_channels_count_ < max_acq_channels_) && (channels_state_[i] == 0)) { channels_state_[i] = 1; - while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0) - { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - } - channels_.at(i)->set_signal(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); + channels_.at(i)->set_signal(search_next_signal(channels_.at(i)->get_signal().get_signal_str(), true)); acq_channels_count_++; channels_.at(i)->start_acquisition(); } @@ -761,3 +742,15 @@ void GNSSFlowgraph::set_channels_state() acq_channels_count_ = max_acq_channels_; DLOG(INFO) << acq_channels_count_ << " channels in acquisition state"; } + +Gnss_Signal GNSSFlowgraph::search_next_signal(std::string searched_signal, bool pop) +{ + while(searched_signal.compare(available_GNSS_signals_.front().get_signal_str()) != 0) + { + available_GNSS_signals_.push_back(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + } + Gnss_Signal result = available_GNSS_signals_.front(); + if(pop){available_GNSS_signals_.pop_front();} + return result; +} diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index 4998d7170..abb4a6ef5 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -63,8 +63,7 @@ public: /*! * \brief Constructor that initializes the receiver flowgraph */ - GNSSFlowgraph(std::shared_ptr configuration, - boost::shared_ptr queue); + GNSSFlowgraph(std::shared_ptr configuration, gr::msg_queue::sptr queue); /*! * \brief Virtual destructor @@ -119,6 +118,7 @@ private: void set_signals_list(); void set_channels_state(); // Initializes the channels state (start acquisition or keep standby) // using the configuration parameters (number of channels and max channels in acquisition) + Gnss_Signal search_next_signal(std::string searched_signal, bool pop); bool connected_; bool running_; int sources_count_; From 74a1f76282fee1b42407820b3cf18deb09bf7052 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 8 Jan 2018 10:57:01 +0100 Subject: [PATCH 21/55] Avoid multithreading collision --- src/algorithms/channel/adapters/channel.cc | 8 +- src/algorithms/channel/libs/channel_fsm.cc | 103 ++++++++++++------ src/algorithms/channel/libs/channel_fsm.h | 10 +- .../channel/libs/channel_msg_receiver_cc.cc | 13 ++- 4 files changed, 90 insertions(+), 44 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index f4c7fa77d..353f5a080 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -197,7 +197,13 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { - channel_fsm_.Event_start_acquisition(); + bool result = false; + result = channel_fsm_.Event_start_acquisition(); + if(!result) + { + LOG(WARNING) << "Invalid channel event"; + return; + } DLOG(INFO) << "Channel start_acquisition()"; } diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 499d55881..935c696ce 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -53,53 +53,88 @@ ChannelFsm::ChannelFsm(std::shared_ptr acquisition) : } - -void ChannelFsm::Event_start_acquisition() +bool ChannelFsm::Event_start_acquisition() { - mx.lock(); - d_state = 1; - start_acquisition(); - DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; - mx.unlock(); + std::lock_guard lk(mx); + if((d_state == 1) || (d_state == 2)) + { + return false; + } + else + { + d_state = 1; + start_acquisition(); + DLOG(INFO) << "CH = " << channel_ << ". Ev start acquisition"; + return true; + } } -void ChannelFsm::Event_valid_acquisition() +bool ChannelFsm::Event_valid_acquisition() { - mx.lock(); - d_state = 2; - start_tracking(); - DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; - mx.unlock(); + std::lock_guard lk(mx); + if(d_state != 1) + { + return false; + } + else + { + d_state = 2; + start_tracking(); + DLOG(INFO) << "CH = " << channel_ << ". Ev valid acquisition"; + return true; + } } -void ChannelFsm::Event_failed_acquisition_repeat() +bool ChannelFsm::Event_failed_acquisition_repeat() { - mx.lock(); - d_state = 1; - start_acquisition(); - DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; - mx.unlock(); -} - -void ChannelFsm::Event_failed_acquisition_no_repeat() -{ - mx.lock(); - d_state = 3; - request_satellite(); - DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; - mx.unlock(); + std::lock_guard lk(mx); + if(d_state != 1) + { + return false; + } + else + { + d_state = 1; + start_acquisition(); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition repeat"; + return true; + } } -void ChannelFsm::Event_failed_tracking_standby() +bool ChannelFsm::Event_failed_acquisition_no_repeat() { - mx.lock(); - d_state = 0; - notify_stop_tracking(); - DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; - mx.unlock(); + std::lock_guard lk(mx); + if(d_state != 1) + { + return false; + } + else + { + d_state = 3; + request_satellite(); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed acquisition no repeat"; + return true; + } +} + + +bool ChannelFsm::Event_failed_tracking_standby() +{ + std::lock_guard lk(mx); + if(d_state != 2) + { + return false; + } + else + { + d_state = 0; + notify_stop_tracking(); + DLOG(INFO) << "CH = " << channel_ << ". Ev failed tracking standby"; + return true; + } } void ChannelFsm::set_acquisition(std::shared_ptr acquisition) diff --git a/src/algorithms/channel/libs/channel_fsm.h b/src/algorithms/channel/libs/channel_fsm.h index 29f741bfc..23b92c394 100644 --- a/src/algorithms/channel/libs/channel_fsm.h +++ b/src/algorithms/channel/libs/channel_fsm.h @@ -54,11 +54,11 @@ public: void set_channel(unsigned int channel); //FSM EVENTS - void Event_start_acquisition(); - void Event_valid_acquisition(); - void Event_failed_acquisition_repeat(); - void Event_failed_acquisition_no_repeat(); - void Event_failed_tracking_standby(); + bool Event_start_acquisition(); + bool Event_valid_acquisition(); + bool Event_failed_acquisition_repeat(); + bool Event_failed_acquisition_no_repeat(); + bool Event_failed_tracking_standby(); private: diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc index d22f2dd73..a88fc6d88 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc @@ -44,26 +44,27 @@ channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fs void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) { + bool result = false; try { long int message = pmt::to_long(msg); switch (message) { case 1: //positive acquisition - d_channel_fsm->Event_valid_acquisition(); + result = d_channel_fsm->Event_valid_acquisition(); break; case 2: //negative acquisition if (d_repeat == true) { - d_channel_fsm->Event_failed_acquisition_repeat(); + result = d_channel_fsm->Event_failed_acquisition_repeat(); } else { - d_channel_fsm->Event_failed_acquisition_no_repeat(); + result = d_channel_fsm->Event_failed_acquisition_no_repeat(); } break; case 3: // tracking loss of lock event - d_channel_fsm->Event_failed_tracking_standby(); + result = d_channel_fsm->Event_failed_tracking_standby(); break; default: LOG(WARNING) << "Default case, invalid message."; @@ -74,6 +75,10 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) { LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; } + if(!result) + { + LOG(WARNING) << "msg_handler_telemetry invalid event"; + } } From 320b4e2d7bc9e7a9510df1458c14b1066399e1de Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 8 Jan 2018 12:03:32 +0100 Subject: [PATCH 22/55] Migrate channel_fsm pointer to std::shared_ptr --- src/algorithms/channel/adapters/channel.cc | 13 +++++++------ src/algorithms/channel/adapters/channel.h | 2 +- .../channel/libs/channel_msg_receiver_cc.cc | 4 ++-- .../channel/libs/channel_msg_receiver_cc.h | 8 ++++---- 4 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 353f5a080..45e2c0833 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -50,6 +50,7 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, implementation_ = implementation; channel_ = channel; queue_ = queue; + channel_fsm_ = std::make_shared(); flag_enable_fpga = configuration->property("Channel.enable_FPGA", false); acq_->set_channel(channel_); @@ -89,16 +90,16 @@ Channel::Channel(ConfigurationInterface *configuration, unsigned int channel, repeat_ = configuration->property("Acquisition_" + implementation_ + boost::lexical_cast(channel_) + ".repeat_satellite", false); DLOG(INFO) << "Channel " << channel_ << " satellite repeat = " << repeat_; - channel_fsm_.set_acquisition(acq_); - channel_fsm_.set_tracking(trk_); - channel_fsm_.set_channel(channel_); - channel_fsm_.set_queue(queue_); + channel_fsm_->set_acquisition(acq_); + channel_fsm_->set_tracking(trk_); + channel_fsm_->set_channel(channel_); + channel_fsm_->set_queue(queue_); connected_ = false; gnss_signal_ = Gnss_Signal(implementation_); - channel_msg_rx = channel_msg_receiver_make_cc(&channel_fsm_, repeat_); + channel_msg_rx = channel_msg_receiver_make_cc(channel_fsm_, repeat_); } @@ -198,7 +199,7 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { bool result = false; - result = channel_fsm_.Event_start_acquisition(); + result = channel_fsm_->Event_start_acquisition(); if(!result) { LOG(WARNING) << "Invalid channel event"; diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index 8e0893843..405385814 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -104,7 +104,7 @@ private: Gnss_Signal gnss_signal_; bool connected_; bool repeat_; - ChannelFsm channel_fsm_; + std::shared_ptr channel_fsm_; gr::msg_queue::sptr queue_; }; diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc index a88fc6d88..7a80cfceb 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.cc +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.cc @@ -37,7 +37,7 @@ using google::LogMessage; -channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat) +channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat) { return channel_msg_receiver_cc_sptr(new channel_msg_receiver_cc(channel_fsm, repeat)); } @@ -82,7 +82,7 @@ void channel_msg_receiver_cc::msg_handler_events(pmt::pmt_t msg) } -channel_msg_receiver_cc::channel_msg_receiver_cc(ChannelFsm* channel_fsm, bool repeat) : +channel_msg_receiver_cc::channel_msg_receiver_cc(std::shared_ptr channel_fsm, bool repeat) : gr::block("channel_msg_receiver_cc", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) { this->message_port_register_in(pmt::mp("events")); diff --git a/src/algorithms/channel/libs/channel_msg_receiver_cc.h b/src/algorithms/channel/libs/channel_msg_receiver_cc.h index 195d41e7c..77a91379a 100644 --- a/src/algorithms/channel/libs/channel_msg_receiver_cc.h +++ b/src/algorithms/channel/libs/channel_msg_receiver_cc.h @@ -38,7 +38,7 @@ class channel_msg_receiver_cc; typedef boost::shared_ptr channel_msg_receiver_cc_sptr; -channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat); +channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat); /*! * \brief GNU Radio block that receives asynchronous channel messages from acquisition and tracking blocks @@ -46,11 +46,11 @@ channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fs class channel_msg_receiver_cc : public gr::block { private: - ChannelFsm* d_channel_fsm; + std::shared_ptr d_channel_fsm; bool d_repeat; // todo: change FSM to include repeat value - friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(ChannelFsm* channel_fsm, bool repeat); + friend channel_msg_receiver_cc_sptr channel_msg_receiver_make_cc(std::shared_ptr channel_fsm, bool repeat); void msg_handler_events(pmt::pmt_t msg); - channel_msg_receiver_cc(ChannelFsm* channel_fsm, bool repeat); + channel_msg_receiver_cc(std::shared_ptr channel_fsm, bool repeat); public: ~channel_msg_receiver_cc (); //!< Default destructor From a90edd025cf12d6eeff9492be177370a33b249dc Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 8 Jan 2018 12:54:35 +0100 Subject: [PATCH 23/55] Minor changes --- src/algorithms/channel/adapters/channel.cc | 2 ++ src/algorithms/channel/adapters/channel.h | 6 ++++-- src/algorithms/channel/libs/channel_fsm.cc | 4 ++++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/algorithms/channel/adapters/channel.cc b/src/algorithms/channel/adapters/channel.cc index 45e2c0833..e66074b7a 100644 --- a/src/algorithms/channel/adapters/channel.cc +++ b/src/algorithms/channel/adapters/channel.cc @@ -184,6 +184,7 @@ gr::basic_block_sptr Channel::get_right_block() void Channel::set_signal(const Gnss_Signal& gnss_signal) { + std::lock_guard lk(mx); gnss_signal_ = gnss_signal; std::string str_aux = gnss_signal_.get_signal_str(); const char * str = str_aux.c_str(); // get a C style null terminated string @@ -198,6 +199,7 @@ void Channel::set_signal(const Gnss_Signal& gnss_signal) void Channel::start_acquisition() { + std::lock_guard lk(mx); bool result = false; result = channel_fsm_->Event_start_acquisition(); if(!result) diff --git a/src/algorithms/channel/adapters/channel.h b/src/algorithms/channel/adapters/channel.h index 405385814..8740b5ad9 100644 --- a/src/algorithms/channel/adapters/channel.h +++ b/src/algorithms/channel/adapters/channel.h @@ -37,6 +37,7 @@ #include #include +#include #include #include #include "channel_interface.h" @@ -63,8 +64,7 @@ public: Channel(ConfigurationInterface *configuration, unsigned int channel, std::shared_ptr pass_through, std::shared_ptr acq, std::shared_ptr trk, std::shared_ptr nav, - std::string role, std::string implementation, - boost::shared_ptr queue); + std::string role, std::string implementation, gr::msg_queue::sptr queue); //! Virtual destructor virtual ~Channel(); @@ -79,6 +79,7 @@ public: inline std::string implementation() override { return implementation_; } inline size_t item_size() override { return 0; } + inline Gnss_Signal get_signal() const override { return gnss_signal_; } void start_acquisition() override; //!< Start the State Machine @@ -106,6 +107,7 @@ private: bool repeat_; std::shared_ptr channel_fsm_; gr::msg_queue::sptr queue_; + std::mutex mx; }; #endif /*GNSS_SDR_CHANNEL_H_*/ diff --git a/src/algorithms/channel/libs/channel_fsm.cc b/src/algorithms/channel/libs/channel_fsm.cc index 935c696ce..b6f23cbeb 100644 --- a/src/algorithms/channel/libs/channel_fsm.cc +++ b/src/algorithms/channel/libs/channel_fsm.cc @@ -139,21 +139,25 @@ bool ChannelFsm::Event_failed_tracking_standby() void ChannelFsm::set_acquisition(std::shared_ptr acquisition) { + std::lock_guard lk(mx); acq_ = acquisition; } void ChannelFsm::set_tracking(std::shared_ptr tracking) { + std::lock_guard lk(mx); trk_ = tracking; } void ChannelFsm::set_queue(gr::msg_queue::sptr queue) { + std::lock_guard lk(mx); queue_ = queue; } void ChannelFsm::set_channel(unsigned int channel) { + std::lock_guard lk(mx); channel_ = channel; } From 188df6c5b8c425fe8a76a0649e6fc29161c78ef1 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 9 Jan 2018 16:43:38 +0100 Subject: [PATCH 24/55] Clean pcps acquisition --- .../gnuradio_blocks/pcps_acquisition_cc.cc | 387 +++++++----------- .../gnuradio_blocks/pcps_acquisition_cc.h | 22 +- 2 files changed, 149 insertions(+), 260 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index a0461f9c5..df8ff2a1f 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -127,13 +127,9 @@ pcps_acquisition_cc::pcps_acquisition_cc( // For dumping samples into a file d_dump = dump; d_dump_filename = dump_filename; - d_gnss_synchro = 0; d_grid_doppler_wipeoffs = 0; - - d_done = false; d_blocking = blocking; - d_new_data_available = false; d_worker_active = false; d_data_buffer = static_cast(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment())); } @@ -160,19 +156,6 @@ pcps_acquisition_cc::~pcps_acquisition_cc() { d_dump_file.close(); } - - // Let the worker thread know that we are done and then wait to join - if( d_worker_thread.joinable() ) - { - { - std::lock_guard lk( d_mutex ); - d_done = true; - d_cond.notify_one(); - } - - d_worker_thread.join(); - } - volk_gnsssdr_free( d_data_buffer ); } @@ -233,9 +216,6 @@ void pcps_acquisition_cc::init() int doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_freq + doppler); } - - d_new_data_available = false; - d_done = false; d_worker_active = false; } @@ -253,6 +233,7 @@ void pcps_acquisition_cc::set_state(int state) d_mag = 0.0; d_input_power = 0.0; d_test_statistics = 0.0; + d_active = true; } else if (d_state == 0) {} @@ -299,7 +280,7 @@ void pcps_acquisition_cc::send_negative_acquisition() } -int pcps_acquisition_cc::general_work(int noutput_items, +int pcps_acquisition_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) { @@ -314,193 +295,157 @@ int pcps_acquisition_cc::general_work(int noutput_items, * 6. Declare positive or negative acquisition using a message port */ - switch (d_state) + gr::thread::scoped_lock lk(d_setlock); + if(!d_active || d_worker_active) + { + d_sample_counter += d_fft_size * ninput_items[0]; + consume_each(ninput_items[0]); + return 0; + } + + switch(d_state) { case 0: { - if (d_active) - { - //restart acquisition variables - d_gnss_synchro->Acq_delay_samples = 0.0; - d_gnss_synchro->Acq_doppler_hz = 0.0; - d_gnss_synchro->Acq_samplestamp_samples = 0; - d_well_count = 0; - d_mag = 0.0; - d_input_power = 0.0; - d_test_statistics = 0.0; - d_state = 1; - } - + //restart acquisition variables + d_gnss_synchro->Acq_delay_samples = 0.0; + d_gnss_synchro->Acq_doppler_hz = 0.0; + d_gnss_synchro->Acq_samplestamp_samples = 0; + d_well_count = 0; + d_mag = 0.0; + d_input_power = 0.0; + d_test_statistics = 0.0; + d_state = 1; d_sample_counter += d_fft_size * ninput_items[0]; // sample counter consume_each(ninput_items[0]); - break; } case 1: { - std::unique_lock lk( d_mutex ); - - int num_items_consumed = 1; - - if( d_worker_active ) + // Copy the data to the core and let it know that new data is available + memcpy(d_data_buffer, input_items[0], d_fft_size * sizeof(gr_complex)); + if(d_blocking) { - if( d_blocking ) - { - // Should never get here: - std::string msg = "pcps_acquisition_cc: Entered general work with worker active in blocking mode, should never happen"; - LOG(WARNING) << msg; - std::cout << msg << std::endl; - d_cond.wait( lk, [&]{ return !this->d_worker_active; } ); - } - else - { - num_items_consumed = ninput_items[0]; - d_sample_counter += d_fft_size * num_items_consumed; - } + lk.unlock(); + acquisition_core(); } else { - // Copy the data to the core and let it know that new data is available - memcpy( d_data_buffer, input_items[0], d_fft_size * sizeof( gr_complex ) ); - d_new_data_available = true; - d_cond.notify_one(); - - if( d_blocking ) - { - d_cond.wait( lk, [&]{ return !this->d_new_data_available; } ); - - } + gr::thread::thread d_worker(&pcps_acquisition_cc::acquisition_core, this); + d_worker_active = true; } - - consume_each(num_items_consumed); - + consume_each(1); break; - } // case 1, switch d_state - - } // switch d_state - - return noutput_items; + } + } + return 0; } void pcps_acquisition_cc::acquisition_core( void ) { - d_worker_active = false; - while( 1 ) + gr::thread::scoped_lock lk(d_setlock); + + unsigned long int sample_counter = d_sample_counter; // sample counter + // initialize acquisition algorithm + int doppler; + uint32_t indext = 0; + float magt = 0.0; + const gr_complex *in = d_data_buffer; //Get the input samples pointer + + int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size ); + + float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); + + d_input_power = 0.0; + d_mag = 0.0; + d_well_count++; + + DLOG(INFO) << "Channel: " << d_channel + << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + << " ,sample stamp: " << sample_counter << ", threshold: " + << d_threshold << ", doppler_max: " << d_doppler_max + << ", doppler_step: " << d_doppler_step + << ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); + + lk.unlock(); + if (d_use_CFAR_algorithm_flag) { - std::unique_lock lk( d_mutex ); - d_cond.wait( lk, [&]{ return this->d_new_data_available or this->d_done; } ); - d_worker_active = !d_done; - unsigned long int sample_counter = d_sample_counter; // sample counter - lk.unlock(); + // 1- (optional) Compute the input signal power estimation + volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); + volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size); + d_input_power /= static_cast(d_fft_size); + } + // 2- Doppler frequency search loop + for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) + { + // doppler search steps + doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; - if( d_done ) + volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size); + + // 3- Perform the FFT-based convolution (parallel time search) + // Compute the FFT of the carrier wiped--off incoming signal + d_fft_if->execute(); + + // Multiply carrier wiped--off, Fourier transformed incoming signal + // with the local FFT'd code reference using SIMD operations with VOLK library + volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); + + // compute the inverse FFT + d_ifft->execute(); + + // Search maximum + size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 ); + volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size); + volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); + magt = d_magnitude[indext]; + + if (d_use_CFAR_algorithm_flag) { - break; + // Normalize the maximum value to correct the scale factor introduced by FFTW + magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); } - - // initialize acquisition algorithm - int doppler; - uint32_t indext = 0; - float magt = 0.0; - const gr_complex *in = d_data_buffer; //Get the input samples pointer - - int effective_fft_size = ( d_bit_transition_flag ? d_fft_size/2 : d_fft_size ); - - float fft_normalization_factor = static_cast(d_fft_size) * static_cast(d_fft_size); - - d_input_power = 0.0; - d_mag = 0.0; - d_well_count++; - - DLOG(INFO) << "Channel: " << d_channel - << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN - << " ,sample stamp: " << sample_counter << ", threshold: " - << d_threshold << ", doppler_max: " << d_doppler_max - << ", doppler_step: " << d_doppler_step - << ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); - - if (d_use_CFAR_algorithm_flag == true) + // 4- record the maximum peak and the associated synchronization parameters + if (d_mag < magt) { - // 1- (optional) Compute the input signal power estimation - volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size); - volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size); - d_input_power /= static_cast(d_fft_size); - } - // 2- Doppler frequency search loop - for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++) - { - // doppler search steps - doppler = -static_cast(d_doppler_max) + d_doppler_step * doppler_index; + d_mag = magt; - volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, - d_grid_doppler_wipeoffs[doppler_index], d_fft_size); - - // 3- Perform the FFT-based convolution (parallel time search) - // Compute the FFT of the carrier wiped--off incoming signal - d_fft_if->execute(); - - // Multiply carrier wiped--off, Fourier transformed incoming signal - // with the local FFT'd code reference using SIMD operations with VOLK library - volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), - d_fft_if->get_outbuf(), d_fft_codes, d_fft_size); - - // compute the inverse FFT - d_ifft->execute(); - - // Search maximum - size_t offset = ( d_bit_transition_flag ? effective_fft_size : 0 ); - volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size); - volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size); - magt = d_magnitude[indext]; - - if (d_use_CFAR_algorithm_flag == true) + if (!d_use_CFAR_algorithm_flag) { - // Normalize the maximum value to correct the scale factor introduced by FFTW - magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor); - } - // 4- record the maximum peak and the associated synchronization parameters - if (d_mag < magt) - { - d_mag = magt; - - if (d_use_CFAR_algorithm_flag == false) - { - // Search grid noise floor approximation for this doppler line - volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size); - d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1); - } - - // In case that d_bit_transition_flag = true, we compare the potentially - // new maximum test statistics (d_mag/d_input_power) with the value in - // d_test_statistics. When the second dwell is being processed, the value - // of d_mag/d_input_power could be lower than d_test_statistics (i.e, - // the maximum test statistics in the previous dwell is greater than - // current d_mag/d_input_power). Note that d_test_statistics is not - // restarted between consecutive dwells in multidwell operation. - - if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) - { - d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); - d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); - d_gnss_synchro->Acq_samplestamp_samples = sample_counter; - - // 5- Compute the test statistics and compare to the threshold - //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; - d_test_statistics = d_mag / d_input_power; - } + // Search grid noise floor approximation for this doppler line + volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size); + d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1); } - // Record results to file if required - if (d_dump) - { - std::stringstream filename; - std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write - filename.str(""); + // In case that d_bit_transition_flag = true, we compare the potentially + // new maximum test statistics (d_mag/d_input_power) with the value in + // d_test_statistics. When the second dwell is being processed, the value + // of d_mag/d_input_power could be lower than d_test_statistics (i.e, + // the maximum test statistics in the previous dwell is greater than + // current d_mag/d_input_power). Note that d_test_statistics is not + // restarted between consecutive dwells in multidwell operation. - boost::filesystem::path p = d_dump_filename; - filename << p.parent_path().string() + if (d_test_statistics < (d_mag / d_input_power) || !d_bit_transition_flag) + { + d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); + d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); + d_gnss_synchro->Acq_samplestamp_samples = sample_counter; + + // 5- Compute the test statistics and compare to the threshold + //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; + d_test_statistics = d_mag / d_input_power; + } + } + // Record results to file if required + if (d_dump) + { + std::stringstream filename; + std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write + filename.str(""); + boost::filesystem::path p = d_dump_filename; + filename << p.parent_path().string() << boost::filesystem::path::preferred_separator << p.stem().string() << "_" << d_gnss_synchro->System @@ -509,15 +454,32 @@ void pcps_acquisition_cc::acquisition_core( void ) << doppler << p.extension().string(); - DLOG(INFO) << "Writing ACQ out to " << filename.str(); + DLOG(INFO) << "Writing ACQ out to " << filename.str(); - d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); - d_dump_file.write(reinterpret_cast(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? - d_dump_file.close(); - } + d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary); + d_dump_file.write(reinterpret_cast(d_ifft->get_outbuf()), n); //write directly |abs(x)|^2 in this Doppler bin? + d_dump_file.close(); } - - if (!d_bit_transition_flag) + } + lk.lock(); + if (!d_bit_transition_flag) + { + if (d_test_statistics > d_threshold) + { + d_state = 0; // Positive acquisition + d_active = false; + send_positive_acquisition(); + } + else if (d_well_count == d_max_dwells) + { + d_state = 0; + d_active = false; + send_negative_acquisition(); + } + } + else + { + if (d_well_count == d_max_dwells) // d_max_dwells = 2 { if (d_test_statistics > d_threshold) { @@ -525,66 +487,13 @@ void pcps_acquisition_cc::acquisition_core( void ) d_active = false; send_positive_acquisition(); } - else if (d_well_count == d_max_dwells) + else { - d_state = 0; + d_state = 0; // Negative acquisition d_active = false; send_negative_acquisition(); } } - else - { - if (d_well_count == d_max_dwells) // d_max_dwells = 2 - { - if (d_test_statistics > d_threshold) - { - d_state = 0; // Positive acquisition - d_active = false; - send_positive_acquisition(); - } - else - { - d_state = 0; // Negative acquisition - d_active = false; - send_negative_acquisition(); - } - } - } - - lk.lock(); - d_worker_active = false; - d_new_data_available = false; - lk.unlock(); - d_cond.notify_one(); } -} - - -bool pcps_acquisition_cc::start( void ) -{ d_worker_active = false; - d_done = false; - - // Start the worker thread and wait for it to acknowledge: - d_worker_thread = std::move( std::thread( &pcps_acquisition_cc::acquisition_core, this ) ); - - return gr::block::start(); } - - -bool pcps_acquisition_cc::stop( void ) -{ - // Let the worker thread know that we are done and then wait to join - if( d_worker_thread.joinable() ) - { - { - std::lock_guard lk( d_mutex ); - d_done = true; - d_cond.notify_one(); - } - - d_worker_thread.join(); - } - return gr::block::stop(); -} - diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 486468379..3408a01f4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -21,6 +21,7 @@ *
  • Luis Esteve, 2012. luis(at)epsilon-formacion.com *
  • Marc Molina, 2013. marc.molina.pena@gmail.com *
  • Cillian O'Driscoll, 2017. cillian(at)ieee.org + *
  • Antonio Ramos, 2017. antonio.ramos@cttc.es * * * ------------------------------------------------------------------------- @@ -53,9 +54,6 @@ #include #include -#include -#include -#include #include #include #include @@ -109,7 +107,6 @@ private: int d_samples_per_code; //unsigned int d_doppler_resolution; float d_threshold; - std::string d_satellite_str; unsigned int d_doppler_max; unsigned int d_doppler_step; unsigned int d_sampled_ms; @@ -137,16 +134,8 @@ private: bool d_dump; unsigned int d_channel; std::string d_dump_filename; - - std::thread d_worker_thread; - std::mutex d_mutex; - - std::condition_variable d_cond; - bool d_done; - bool d_new_data_available; bool d_worker_active; bool d_blocking; - gr_complex *d_data_buffer; public: @@ -251,15 +240,6 @@ public: gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - /*! - * Called by the flowgraph when processing is about to start. - */ - bool start( void ); - - /*! - * Called by the flowgraph when processing is done. - */ - bool stop( void ); }; #endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/ From 2749d2518bf8caddd82a5c405f79fdec5f31d384 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 10 Jan 2018 11:08:06 +0100 Subject: [PATCH 25/55] Fix sample counting --- .../gnuradio_blocks/pcps_acquisition_cc.cc | 12 ++++++------ .../gnuradio_blocks/pcps_acquisition_cc.h | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index df8ff2a1f..897532983 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -328,13 +328,14 @@ int pcps_acquisition_cc::general_work(int noutput_items __attribute__((unused)), if(d_blocking) { lk.unlock(); - acquisition_core(); + acquisition_core(d_sample_counter); } else { - gr::thread::thread d_worker(&pcps_acquisition_cc::acquisition_core, this); + gr::thread::thread d_worker(&pcps_acquisition_cc::acquisition_core, this, d_sample_counter); d_worker_active = true; } + d_sample_counter += d_fft_size; consume_each(1); break; } @@ -343,11 +344,10 @@ int pcps_acquisition_cc::general_work(int noutput_items __attribute__((unused)), } -void pcps_acquisition_cc::acquisition_core( void ) +void pcps_acquisition_cc::acquisition_core( unsigned long int samp_count ) { gr::thread::scoped_lock lk(d_setlock); - unsigned long int sample_counter = d_sample_counter; // sample counter // initialize acquisition algorithm int doppler; uint32_t indext = 0; @@ -364,7 +364,7 @@ void pcps_acquisition_cc::acquisition_core( void ) DLOG(INFO) << "Channel: " << d_channel << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN - << " ,sample stamp: " << sample_counter << ", threshold: " + << " ,sample stamp: " << samp_count << ", threshold: " << d_threshold << ", doppler_max: " << d_doppler_max << ", doppler_step: " << d_doppler_step << ", use_CFAR_algorithm_flag: " << ( d_use_CFAR_algorithm_flag ? "true" : "false" ); @@ -431,7 +431,7 @@ void pcps_acquisition_cc::acquisition_core( void ) { d_gnss_synchro->Acq_delay_samples = static_cast(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(doppler); - d_gnss_synchro->Acq_samplestamp_samples = sample_counter; + d_gnss_synchro->Acq_samplestamp_samples = samp_count; // 5- Compute the test statistics and compare to the threshold //d_test_statistics = 2 * d_fft_size * d_mag / d_input_power; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 3408a01f4..bb3db1e43 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -97,7 +97,7 @@ private: void update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq); - void acquisition_core( void ); + void acquisition_core( unsigned long int samp_count ); void send_negative_acquisition(); void send_positive_acquisition(); From c46345692416d89c0f1e7c3a9f451f0d706af626 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 10 Jan 2018 11:30:16 +0100 Subject: [PATCH 26/55] Adding pilot signal acquisition option for Galileo E1 --- .../galileo_e1_pcps_ambiguous_acquisition.cc | 15 +++++++++++++-- .../galileo_e1_pcps_ambiguous_acquisition.h | 1 + 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc index f9712824c..b0f64a25b 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -70,6 +70,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions + acquire_pilot_= configuration_->property(role + ".acquire_pilot", false); //will be true in future versions max_dwells_ = configuration_->property(role + ".max_dwells", 1); @@ -252,8 +253,18 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() std::complex * code = new std::complex[code_length_]; - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, - cboc, gnss_synchro_->PRN, fs_in_, 0, false); + if (acquire_pilot_==true) + { + //set local signal generator to Galileo E1 pilot component (1C) + char pilot_signal[3]="1C"; + galileo_e1_code_gen_complex_sampled(code, pilot_signal, + cboc, gnss_synchro_->PRN, fs_in_, 0, false); + }else + { + galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + cboc, gnss_synchro_->PRN, fs_in_, 0, false); + } + for (unsigned int i = 0; i < sampled_ms_ / 4; i++) { diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h index da8d2e742..71727cecd 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h @@ -146,6 +146,7 @@ private: unsigned int code_length_; bool bit_transition_flag_; bool use_CFAR_algorithm_flag_; + bool acquire_pilot_; unsigned int channel_; float threshold_; unsigned int doppler_max_; From 0e9113fdcb1b37fe90af46883eeddbe7070521aa Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 10 Jan 2018 12:37:13 +0100 Subject: [PATCH 27/55] Adding new advanced features to Galileo E1 tracking: Pilot tracking, Extended coherent correlation, narrow DLL/PLL bandwidth and narrow correlator spacings --- ...r_galileo_E1_extended_correlator_byte.conf | 269 ++++ .../galileo_e1_dll_pll_veml_tracking.cc | 34 +- .../galileo_e1_dll_pll_veml_tracking_cc.cc | 1212 +++++++++++------ .../galileo_e1_dll_pll_veml_tracking_cc.h | 84 +- src/core/system_parameters/Galileo_E1.h | 2 +- 5 files changed, 1200 insertions(+), 401 deletions(-) create mode 100644 conf/gnss-sdr_galileo_E1_extended_correlator_byte.conf diff --git a/conf/gnss-sdr_galileo_E1_extended_correlator_byte.conf b/conf/gnss-sdr_galileo_E1_extended_correlator_byte.conf new file mode 100644 index 000000000..fb6a08fc0 --- /dev/null +++ b/conf/gnss-sdr_galileo_E1_extended_correlator_byte.conf @@ -0,0 +1,269 @@ +; Default configuration file +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second]. +GNSS-SDR.internal_fs_sps=20000000 + + +;######### SIGNAL_SOURCE CONFIG ############ +;#implementation: Use [File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental) +SignalSource.implementation=File_Signal_Source + +;#filename: path to file with the captured GNSS signal samples to be processed +;SignalSource.filename=/home/javier/signals/L125_III1b_210s_L1_2msps.bin ; <- PUT YOUR FILE HERE + +SignalSource.filename=/media/javier/SISTEMA/signals/fraunhofer/L125_III1b_210s_L1.bin + +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +SignalSource.item_type=byte + +;#sampling_frequency: Original Signal sampling frequency in [Hz] +SignalSource.sampling_frequency=20000000 + +;#freq: RF front-end center frequency in [Hz] +SignalSource.freq=1575420000 + +;#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 + +SignalSource.dump_filename=../data/signal_source.dat + +;#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 ############ +;## It holds blocks to change data type, filter and resample input data. + +;#implementation: Use [Pass_Through] or [Signal_Conditioner] +;#[Pass_Through] disables this block and the [DataTypeAdapter], [InputFilter] and [Resampler] blocks +;#[Signal_Conditioner] enables this block. Then you have to configure [DataTypeAdapter], [InputFilter] and [Resampler] blocks +SignalConditioner.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER CONFIG ############ +;## Changes the type of input data. Please disable it in this version. +;#implementation: [Pass_Through] disables this block +DataTypeAdapter.implementation=Ibyte_To_Complex + +;######### INPUT_FILTER CONFIG ############ +;## Filter the input data. Can be combined with frequency translation for IF signals + +InputFilter.implementation=Pass_Through + + +;######### RESAMPLER CONFIG ############ +;## Resamples the input data. + +;#implementation: Use [Pass_Through] or [Direct_Resampler] +;#[Pass_Through] disables this block +;#[Direct_Resampler] enables a resampler that implements a nearest neigbourhood interpolation +;Resampler.implementation=Direct_Resampler +Resampler.implementation=Pass_Through + + +;######### CHANNELS GLOBAL CONFIG ############ +;#count: Number of available GPS satellite channels. +Channels_1C.count=0 +;#count: Number of available Galileo satellite channels. +Channels_1B.count=1 +;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver +Channels.in_acquisition=1 + +;#signal: +;#if the option is disabled by default is assigned "1C" GPS L1 C/A +Channel1.signal=1B +Channel2.signal=1B +Channel3.signal=1B +Channel4.signal=1B +Channel5.signal=1B +Channel6.signal=1B +Channel7.signal=1B +Channel8.signal=1B +Channel9.signal=1B +Channel10.signal=1B +Channel11.signal=1B +Channel12.signal=1B +Channel13.signal=1B +Channel14.signal=1B +Channel15.signal=1B + + +;######### GPS ACQUISITION CONFIG ############ + +;#dump: Enable or disable the acquisition internal data file logging [true] or [false] +Acquisition_1C.dump=false +;#filename: Log path and filename +Acquisition_1C.dump_filename=./acq_dump.dat +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +Acquisition_1C.item_type=gr_complex +;#if: Signal intermediate frequency in [Hz] +Acquisition_1C.if=0 +;#sampled_ms: Signal block duration for the acquisition signal detection [ms] +Acquisition_1C.sampled_ms=1 +;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.use_CFAR_algorithm=false; +;#threshold: Acquisition threshold +Acquisition_1C.threshold=18 +;#doppler_max: Maximum expected Doppler shift [Hz] +Acquisition_1C.doppler_max=5000 +;#doppler_max: Doppler step in the grid search [Hz] +Acquisition_1C.doppler_step=500 + + +;######### GALILEO ACQUISITION CONFIG ############ + +;#dump: Enable or disable the acquisition internal data file logging [true] or [false] +Acquisition_1B.dump=false +;#filename: Log path and filename +Acquisition_1B.dump_filename=../data/acq_dump.dat +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +Acquisition_1B.item_type=gr_complex +;#if: Signal intermediate frequency in [Hz] +Acquisition_1B.if=0 +;#sampled_ms: Signal block duration for the acquisition signal detection [ms] +Acquisition_1B.sampled_ms=4 +;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.acquire_pilot=true +Acquisition_1B.use_CFAR_algorithm=false +;#threshold: Acquisition threshold +Acquisition_1B.threshold=21 +;#doppler_max: Maximum expected Doppler shift [Hz] +Acquisition_1B.doppler_max=5000 +;#doppler_max: Doppler step in the grid search [Hz] +Acquisition_1B.doppler_step=125 +Acquisition_1B.bit_transition_flag=true + +;######### TRACKING GPS CONFIG ############ + +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +Tracking_1C.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_1C.item_type=gr_complex + +;#sampling_frequency: Signal Intermediate Frequency in [Hz] +Tracking_1C.if=0 + +;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] +Tracking_1C.dump=false + +;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. +Tracking_1C.dump_filename=../data/epl_tracking_ch_ + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1C.pll_bw_hz=30.0; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1C.dll_bw_hz=2.0; + +;#order: PLL/DLL loop filter order [2] or [3] +Tracking_1C.order=3; + +;######### TRACKING GALILEO CONFIG ############ + +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +;#item_type: Type and resolution for each of the signal samples. +Tracking_1B.item_type=gr_complex + +;#sampling_frequency: Signal Intermediate Frequency in [Hz] +Tracking_1B.if=0 + +;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] +Tracking_1B.dump=true + +;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. +Tracking_1B.dump_filename=../data/veml_tracking_ch_ + +Tracking_1B.track_pilot=true + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1B.pll_bw_hz=4.0; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1B.dll_bw_hz=0.5; + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1B.pll_bw_narrow_hz=2.0; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1B.dll_bw_narrow_hz=0.25; + +Tracking_1B.extend_correlation_symbols=4; + +;#order: PLL/DLL loop filter order [2] or [3] +Tracking_1B.order=3; + +;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo +Tracking_1B.early_late_space_chips=0.15; + +;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] +Tracking_1B.very_early_late_space_chips=0.6; + +;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo +Tracking_1B.early_late_space_narrow_chips=0.06; + +;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] +Tracking_1B.very_early_late_space_narrow_chips=0.25; + + + +;######### TELEMETRY DECODER GPS CONFIG ############ +;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false +;#decimation factor +TelemetryDecoder_1C.decimation_factor=4; + +;######### TELEMETRY DECODER GALILEO CONFIG ############ +;#implementation: Use [Galileo_E1B_Telemetry_Decoder] for Galileo E1B +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables + +;#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: +PVT.implementation=RTKLIB_PVT + +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad + +;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] +PVT.output_rate_ms=100; + +;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms. +PVT.display_rate_ms=500; + +;#dump: Enable or disable the PVT internal binary data file logging [true] or [false] +PVT.dump=false + +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 + +;#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 diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index c3a37b548..b47e26a68 100755 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -57,19 +57,31 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( std::string item_type; std::string default_item_type = "gr_complex"; float pll_bw_hz; + float pll_bw_narrow_hz; float dll_bw_hz; + float dll_bw_narrow_hz; float early_late_space_chips; float very_early_late_space_chips; + float early_late_space_narrow_chips; + float very_early_late_space_narrow_chips; item_type = configuration->property(role + ".item_type", default_item_type); int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); 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); + pll_bw_hz = configuration->property(role + ".pll_bw_hz", 5.0); + dll_bw_hz = configuration->property(role + ".dll_bw_hz", 0.5); + pll_bw_narrow_hz = configuration->property(role + ".pll_bw_narrow_hz", 2.0); + dll_bw_narrow_hz = configuration->property(role + ".dll_bw_narrow_hz", 0.25); + int extend_correlation_symbols; + extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", 1); early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.15); very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", 0.6); + early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); + very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.6); + + bool track_pilot=configuration->property(role + ".track_pilot", false); std::string default_dump_filename = "./track_ch"; dump_filename = configuration->property(role + ".dump_filename", @@ -88,8 +100,24 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( dump_filename, pll_bw_hz, dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, early_late_space_chips, - very_early_late_space_chips); + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot); +// tracking_ = galileo_e1_dll_pll_veml_make_tracking_cc( +// f_if, +// fs_in, +// vector_length, +// dump, +// dump_filename, +// pll_bw_hz, +// dll_bw_hz, +// early_late_space_chips, +// very_early_late_space_chips); } else { diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 9250de398..a21865500 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -72,21 +72,40 @@ galileo_e1_dll_pll_veml_make_tracking_cc( std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, float early_late_space_chips, - float very_early_late_space_chips) + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot) { return galileo_e1_dll_pll_veml_tracking_cc_sptr(new galileo_e1_dll_pll_veml_tracking_cc(if_freq, - fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips, very_early_late_space_chips)); + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot)); } void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items, - gr_vector_int &ninput_items_required) + gr_vector_int &ninput_items_required) { if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } } @@ -98,10 +117,16 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, float early_late_space_chips, - float very_early_late_space_chips): - gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot): + gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -121,24 +146,31 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( // Initialize tracking ========================================== // Set bandwidth of code and carrier loop filters - d_code_loop_filter.set_DLL_BW(dll_bw_hz); - d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); + d_dll_bw_hz=dll_bw_hz; + d_pll_bw_hz=pll_bw_hz; + d_dll_bw_narrow_hz=dll_bw_narrow_hz; + d_pll_bw_narrow_hz=pll_bw_narrow_hz; + + d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); // Correlator spacing d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) d_very_early_late_spc_chips = very_early_late_space_chips; // Define very-early-late offset (in chips) + d_early_late_spc_narrow_chips = early_late_space_narrow_chips; // Define narrow early-late offset (in chips) + d_very_early_late_spc_narrow_chips = very_early_late_space_narrow_chips; // Define narrow very-early-late offset (in chips) // Initialization of local code replica // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip - d_ca_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); + d_tracking_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); // correlator outputs (scalar) d_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } // map memory pointers of correlator outputs d_Very_Early = &d_correlator_outs[0]; d_Early = &d_correlator_outs[1]; @@ -155,9 +187,32 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_local_code_shift_chips[4] = d_very_early_late_spc_chips; d_correlation_length_samples = d_vector_length; - multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); + d_extend_correlation_symbols=extend_correlation_symbols; + // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal + d_track_pilot=track_pilot; + if (d_track_pilot) + { + //extended integration control + if (d_extend_correlation_symbols>1) + { + d_enable_extended_integration=true; + }else{ + d_enable_extended_integration=false; + } + //Extra correlator for the data component + d_local_code_data_shift_chips=static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); + d_local_code_data_shift_chips[0]=0.0; + correlator_data_cpu.init(2 * d_correlation_length_samples, 1); + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_Prompt_Data[0] = gr_complex(0,0); + d_data_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); + }else{ + // Disable extended integration if data component tracking is selected + d_enable_extended_integration=false; + } + //--- Initializations ------------------------------ // Initial code frequency basis of NCO d_code_freq_chips = static_cast(Galileo_E1_CODE_CHIP_RATE_HZ); @@ -171,9 +226,6 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( //d_sample_counter_seconds = 0; d_acq_sample_stamp = 0; - d_enable_tracking = false; - d_pull_in = false; - d_current_prn_length_samples = static_cast(d_vector_length); // CN0 estimation and lock detector buffers @@ -185,11 +237,8 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; systemName["E"] = std::string("Galileo"); - *d_Very_Early = gr_complex(0,0); - *d_Early = gr_complex(0,0); - *d_Prompt = gr_complex(0,0); - *d_Late = gr_complex(0,0); - *d_Very_Late = gr_complex(0,0); + + clear_tracking_vars(); d_acquisition_gnss_synchro = 0; d_channel = 0; @@ -197,106 +246,391 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_acq_carrier_doppler_hz = 0.0; d_carrier_doppler_hz = 0.0; d_acc_carrier_phase_rad = 0.0; - d_acc_code_phase_secs = 0.0; + + d_state=0;// intial state: stanby } void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() { + /* + * correct the code phase according to the delay between acq and trk + */ d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + long int acq_trk_diff_samples; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples; + acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); + // Doppler effect + // Fd=(C/(C+Vr))*F + double radial_velocity = (Galileo_E1_FREQ_HZ + d_acq_carrier_doppler_hz) / Galileo_E1_FREQ_HZ; + // new chip and prn sequence periods based on acq Doppler + double T_chip_mod_seconds; + double T_prn_mod_seconds; + double T_prn_mod_samples; + d_code_freq_chips = radial_velocity * Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); + T_chip_mod_seconds = 1/d_code_freq_chips; + T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + + d_current_prn_length_samples = round(T_prn_mod_samples); + + double T_prn_true_seconds = Galileo_E1_B_CODE_LENGTH_CHIPS / Galileo_E1_CODE_CHIP_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; + double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; + double corrected_acq_phase_samples, delay_correction_samples; + corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(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; + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + // DLL/PLL filter initialization d_carrier_loop_filter.initialize(); // initialize the carrier filter d_code_loop_filter.initialize(); // initialize the code filter - // generate local reference ALWAYS starting at chip 1 (2 samples per chip) - galileo_e1_code_gen_float_sampled(d_ca_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - 2 * Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - multicorrelator_cpu.set_local_code_and_taps(static_cast(2 * Galileo_E1_B_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); + if (d_track_pilot) + { + char pilot_signal[3]="1C"; + galileo_e1_code_gen_float_sampled(d_tracking_code, + pilot_signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + galileo_e1_code_gen_float_sampled(d_data_code, + d_acquisition_gnss_synchro->Signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + d_Prompt_Data[0]=gr_complex(0,0); //clean data correlator output + correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), + d_data_code, + d_local_code_shift_chips); + }else{ + galileo_e1_code_gen_float_sampled(d_tracking_code, + d_acquisition_gnss_synchro->Signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + } + + multicorrelator_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), d_tracking_code, d_local_code_shift_chips); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } d_carrier_lock_fail_counter = 0; - d_rem_code_phase_samples = 0.0; + d_rem_code_phase_samples = 0; d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; d_acc_carrier_phase_rad = 0.0; - d_acc_code_phase_secs = 0.0; - d_carrier_doppler_hz = d_acq_carrier_doppler_hz; - d_current_prn_length_samples = d_vector_length; + d_code_phase_samples = d_acq_code_phase_samples; std::string sys_ = &d_acquisition_gnss_synchro->System; - sys = sys_.substr(0, 1); + sys = sys_.substr(0,1); // DEBUG OUTPUT std::cout << "Tracking of Galileo E1 signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - // enable tracking - d_pull_in = true; - d_enable_tracking = true; + // enable tracking pull-in + d_state=1; LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz - << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; + + } galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() { if (d_dump_file.is_open()) + { + try { - try - { - d_dump_file.close(); - } - catch(const std::exception & ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } + d_dump_file.close(); } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } + } if(d_dump) + { + if(d_channel == 0) { - if(d_channel == 0) - { - std::cout << "Writing .mat files ..."; - } - galileo_e1_dll_pll_veml_tracking_cc::save_matfile(); - if(d_channel == 0) - { - std::cout << " done." << std::endl; - } + std::cout << "Writing .mat files ..."; } + galileo_e1_dll_pll_veml_tracking_cc::save_matfile(); + if(d_channel == 0) + { + std::cout << " done." << std::endl; + } + } try { - volk_gnsssdr_free(d_local_code_shift_chips); - volk_gnsssdr_free(d_correlator_outs); - volk_gnsssdr_free(d_ca_code); - delete[] d_Prompt_buffer; - multicorrelator_cpu.free(); + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_tracking_code); + if (d_track_pilot) + { + volk_gnsssdr_free(d_Prompt_Data); + volk_gnsssdr_free(d_data_code); + volk_gnsssdr_free(d_local_code_data_shift_chips); + correlator_data_cpu.free(); + } + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); } catch(const std::exception & ex) { - LOG(WARNING) << "Exception in destructor " << ex.what(); + LOG(WARNING) << "Exception in destructor " << ex.what(); } } - -int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +bool galileo_e1_dll_pll_veml_tracking_cc::acquire_secondary() +{ + //******* preamble correlation ******** + int corr_value=0; + for (unsigned int i = 0; i < Galileo_E1_C_SECONDARY_CODE_LENGTH; i++) + { + if (d_Prompt_buffer_deque.at(i).real() < 0) // symbols clipping + { + if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + { + corr_value++; + } + else + { + corr_value--; + } + } + else + { + if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + { + corr_value--; + } + else + { + corr_value++; + } + } + } + + if (abs(corr_value) == Galileo_E1_C_SECONDARY_CODE_LENGTH) + { + return true; + }else + { + return false; + } +} + +bool galileo_e1_dll_pll_veml_tracking_cc::cn0_and_tracking_lock_status() +{ + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; + d_cn0_estimation_counter++; + return true; + } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < 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 > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock + d_carrier_lock_fail_counter = 0; + return false; + }else{ + return true; + } + } +} +// correlation requires: +// - updated remnant carrier phase in radians (rem_carr_phase_rad) +// - updated remnant code phase in samples (d_rem_code_phase_samples) +// - d_code_freq_chips +// - d_carrier_doppler_hz +void galileo_e1_dll_pll_veml_tracking_cc::do_correlation_step(const gr_complex* input_samples) +{ + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,input_samples); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); + + // DATA CORRELATOR (if tracking tracks the pilot signal) + if (d_track_pilot) + { + correlator_data_cpu.set_input_output_vectors(d_Prompt_Data,input_samples); + correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); + } +} + +void galileo_e1_dll_pll_veml_tracking_cc::run_dll_pll(bool disable_costas_loop) +{ + // ################## PLL ########################################################## + // PLL discriminator + if (disable_costas_loop==true) + { + //Secondary code acquired. No symbols transition should be present in the signal + d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + }else{ + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + } + + // Carrier discriminator filter + d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz); + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + d_carr_error_filt_hz; + // New code Doppler frequency estimation + d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); + + // ################## DLL ########################################################## + // DLL discriminator + d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); //[chips/Ti] + // Code discriminator filter + d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); //[chips/second] + +} + +void galileo_e1_dll_pll_veml_tracking_cc::clear_tracking_vars() +{ + *d_Very_Early = gr_complex(0,0); + *d_Early = gr_complex(0,0); + *d_Prompt = gr_complex(0,0); + *d_Late = gr_complex(0,0); + *d_Very_Late= gr_complex(0,0); + d_carr_error_hz =0.0; + d_carr_error_filt_hz =0.0; + d_code_error_chips =0.0; + d_code_error_filt_chips =0.0; + d_current_symbol=0; +} + +void galileo_e1_dll_pll_veml_tracking_cc::log_data() +{ + if(d_dump) + { + // Dump results to file + float prompt_I; + float prompt_Q; + float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; + float tmp_float; + double tmp_double; + + prompt_I = static_cast(d_P_accu.real()); + prompt_Q = static_cast(d_P_accu.imag()); + + tmp_VE = std::abs(d_VE_accu); + tmp_E = std::abs(d_E_accu); + tmp_P = std::abs(d_P_accu); + tmp_L = std::abs(d_L_accu); + tmp_VL = std::abs(d_VL_accu); + + try + { + // Dump correlators output + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + //PLL commands + tmp_float = d_carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + //DLL commands + tmp_float = d_code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } +} +int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - double carr_error_hz = 0.0; - double carr_error_filt_hz = 0.0; - double code_error_chips = 0.0; - double code_error_filt_chips = 0.0; // Block input data and block output stream pointers const gr_complex* in = reinterpret_cast(input_items[0]); @@ -304,123 +638,208 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder Gnss_Synchro current_synchro_data = Gnss_Synchro(); - if (d_enable_tracking == true) + switch(d_state) + { + case 0: //standby - bypass + { + current_synchro_data.Tracking_sample_counter = d_sample_counter; + break; + } + case 1: // pull-in + { + /* + * Signal alignment (skip samples until the incoming signal is aligned with local replica) + */ + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + int samples_offset; + double 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_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.fs = d_fs_in; + *out[0] = current_synchro_data; + d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples + consume_each(samples_offset); //shift input to perform alignment with local replica + d_state=2; //next state is the symbol synchronization + return 0; + } + case 2: // wide tracking and symbol synchronization { // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; - if (d_pull_in == true) + //Current NCO and code generator parameters + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; + // perform a correlation step + do_correlation_step(in); + // save single correlation step variables + d_VE_accu=*d_Very_Early; + d_E_accu=*d_Early; + d_P_accu=*d_Prompt; + d_L_accu=*d_Late; + d_VL_accu=*d_Very_Late; + //check lock status + if (cn0_and_tracking_lock_status()==false) + { + clear_tracking_vars(); + d_state=0; //loss-of-lock detected + }else{ + + //perform DLL/PLL tracking loop computations + run_dll_pll(false); + + // ################## PLL COMMANDS ################################################# + //carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + //remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## DLL COMMANDS ################################################# + + //Code error from DLL + double code_error_filt_secs; + code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples + + // ########### Output the tracking results to Telemetry block ########## + if (d_track_pilot) { - /* - * Signal alignment (skip samples until the incoming signal is aligned with local replica) - */ - int samples_offset; - double 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_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; - current_synchro_data.fs = d_fs_in; - *out[0] = current_synchro_data; - d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples - d_pull_in = false; - consume_each(samples_offset); //shift input to perform alignment with local replica - return 1; + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + }else{ + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); } + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + //compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs,in); + //enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); - double carr_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - double code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast(d_fs_in)); - double rem_code_phase_half_chips = d_rem_code_phase_samples * (2.0*d_code_freq_chips / d_fs_in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, - carr_phase_step_rad, - rem_code_phase_half_chips, - code_phase_step_half_chips, - d_correlation_length_samples); + //std::cout<<(d_Prompt->real()>0); + if (d_enable_extended_integration) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_buffer_deque.push_back(*d_Prompt); + if (d_Prompt_buffer_deque.size()==Galileo_E1_C_SECONDARY_CODE_LENGTH) + { + if (acquire_secondary()==true) + { + d_extend_correlation_symbols_count=0; + //reset extended correlator + d_VE_accu=gr_complex(0,0); + d_E_accu=gr_complex(0,0); + d_P_accu=gr_complex(0,0); + d_L_accu=gr_complex(0,0); + d_VL_accu=gr_complex(0,0); + d_Prompt_buffer_deque.clear(); + d_current_symbol=0; + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_very_early_late_spc_narrow_chips; + d_local_code_shift_chips[1] = - d_early_late_spc_narrow_chips; + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; + + + LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); + std::cout<< "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)<(d_extend_correlation_symbols) * Galileo_E1_CODE_PERIOD; + d_carrier_loop_filter.set_pdi(new_correlation_time_s); + d_code_loop_filter.set_pdi(new_correlation_time_s); + + d_state=3; // next state is the extended correlator integrator + } + + d_Prompt_buffer_deque.pop_front(); + } + } + } + break; + } + case 3: // coherent integration (correlation time extension) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + //Current NCO and code generator parameters + d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; + // perform a correlation step + do_correlation_step(in); + //correct the integration sign using the current symbol of the secondary code + if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') + { + d_VE_accu+=*d_Very_Early; + d_E_accu+=*d_Early; + d_P_accu+=*d_Prompt; + d_L_accu+=*d_Late; + d_VL_accu+=*d_Very_Late; + }else{ + d_VE_accu-=*d_Very_Early; + d_E_accu-=*d_Early; + d_P_accu-=*d_Prompt; + d_L_accu-=*d_Late; + d_VL_accu-=*d_Very_Late; + } + d_current_symbol++; + //secondary code roll-up + d_current_symbol=d_current_symbol%Galileo_E1_C_SECONDARY_CODE_LENGTH; + + // PLL/DLL not enabled, we are in the middle of a coherent integration + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation // ################## PLL ########################################################## - // PLL discriminator - carr_error_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GALILEO_TWO_PI; - // Carrier discriminator filter - carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; - // New code Doppler frequency estimation - d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); //carrier phase accumulator for (K) Doppler estimation- d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); //remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - // ################## DLL ########################################################## - // DLL discriminator - code_error_chips = dll_nc_vemlp_normalized(*d_Very_Early, *d_Early, *d_Late, *d_Very_Late); //[chips/Ti] - // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] - //Code phase accumulator - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - double T_chip_seconds; - double T_prn_seconds; - double T_prn_samples; - double K_blk_samples; // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1.0 / d_code_freq_chips; - T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples; d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt; - d_cn0_estimation_counter++; - } - else - { - d_cn0_estimation_counter = 0; - - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); - - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); - - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < 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 > MAXIMUM_LOCK_FAIL_COUNTER) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock - d_carrier_lock_fail_counter = 0; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - // ########### Output the tracking results to Telemetry block ########## - - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); current_synchro_data.Tracking_sample_counter = d_sample_counter; current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; //compute remnant code phase samples AFTER the Tracking timestamp @@ -431,94 +850,113 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri current_synchro_data.Flag_valid_symbol_output = true; current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count>=(d_extend_correlation_symbols-1)) + { + d_extend_correlation_symbols_count=0; + d_state=4; + } + break; + } + case 4: // narrow tracking + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // perform a correlation step + do_correlation_step(in); + + //correct the integration using the current symbol + if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') + { + d_VE_accu+=*d_Very_Early; + d_E_accu+=*d_Early; + d_P_accu+=*d_Prompt; + d_L_accu+=*d_Late; + d_VL_accu+=*d_Very_Late; + }else{ + d_VE_accu-=*d_Very_Early; + d_E_accu-=*d_Early; + d_P_accu-=*d_Prompt; + d_L_accu-=*d_Late; + d_VL_accu-=*d_Very_Late; + } + d_current_symbol++; + //secondary code roll-up + d_current_symbol=d_current_symbol%Galileo_E1_C_SECONDARY_CODE_LENGTH; + + //check lock status + if (cn0_and_tracking_lock_status()==false) + { + clear_tracking_vars(); + d_state=0; //loss-of-lock detected + }else{ + run_dll_pll(true);//Costas loop disabled, use four quadrant atan + + // ################## PLL ########################################################## + //carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + //remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## DLL ########################################################## + + //Code phase accumulator + double code_error_filt_secs; + code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples + + // ########### Output the tracking results to Telemetry block ########## + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + //compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; + //enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + //reset extended correlator + d_VE_accu=gr_complex(0,0); + d_E_accu=gr_complex(0,0); + d_P_accu=gr_complex(0,0); + d_L_accu=gr_complex(0,0); + d_VL_accu=gr_complex(0,0); + d_state=3; //new coherent integration (correlation time extension) cycle + } } - else - { - *d_Early = gr_complex(0,0); - *d_Prompt = gr_complex(0,0); - *d_Late = gr_complex(0,0); - // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - current_synchro_data.Tracking_sample_counter = d_sample_counter; } + //assign the GNURadio block output data - current_synchro_data.System = {'E'}; - std::string str_aux = "1B"; - const char * str = str_aux.c_str(); // get a C style null terminated string - std::memcpy(static_cast(current_synchro_data.Signal), str, 3); +// current_synchro_data.System = {'E'}; +// std::string str_aux = "1B"; +// const char * str = str_aux.c_str(); // get a C style null terminated string +// std::memcpy(static_cast(current_synchro_data.Signal), str, 3); current_synchro_data.fs = d_fs_in; *out[0] = current_synchro_data; - if(d_dump) - { - // Dump results to file - float prompt_I; - float prompt_Q; - float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; - float tmp_float; - double tmp_double; - prompt_I = (*d_Prompt).real(); - prompt_Q = (*d_Prompt).imag(); - tmp_VE = std::abs(*d_Very_Early); - tmp_E = std::abs(*d_Early); - tmp_P = std::abs(*d_Prompt); - tmp_L = std::abs(*d_Late); - tmp_VL = std::abs(*d_Very_Late); - - try - { - // Dump correlators output - d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - tmp_float = d_acc_carrier_phase_rad; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // carrier and code frequency - tmp_float = d_carrier_doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - //PLL commands - tmp_float = carr_error_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = carr_error_filt_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - //DLL commands - tmp_float = code_error_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = code_error_filt_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // CN0 and carrier lock test - tmp_float = d_CN0_SNV_dB_Hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carrier_lock_test; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // PRN - unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } - } consume_each(d_current_prn_length_samples); // this is required for gr_block derivates 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 + if (current_synchro_data.Flag_valid_symbol_output) + { + return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + }else{ + return 0; + } } @@ -534,25 +972,25 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try { - dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); } catch(const std::ifstream::failure &e) { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; } // count number of epochs and rewind long int num_epoch = 0; if (dump_file.is_open()) - { - size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); - dump_file.seekg(0, std::ios::beg); - } + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } else - { - return 1; - } + { + return 1; + } float * abs_VE = new float [num_epoch]; float * abs_E = new float [num_epoch]; float * abs_P = new float [num_epoch]; @@ -576,58 +1014,58 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() try { - if (dump_file.is_open()) - { - for(long int i = 0; i < num_epoch; i++) - { - dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); - dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); - } - } - dump_file.close(); + if (dump_file.is_open()) + { + for(long int i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); + dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); + } + } + dump_file.close(); } catch (const std::ifstream::failure &e) { - std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; - return 1; + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 1; } // WRITE MAT FILE @@ -638,88 +1076,88 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if(reinterpret_cast(matfp) != NULL) - { - size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + { + size_t dims[2] = {1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - } + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } Mat_Close(matfp); delete[] abs_VE; delete[] abs_E; @@ -751,23 +1189,23 @@ void galileo_e1_dll_pll_veml_tracking_cc::set_channel(unsigned int channel) LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) + { + if (d_dump_file.is_open() == false) { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_filename.append(boost::lexical_cast(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); - LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } - } + try + { + d_dump_filename.append(boost::lexical_cast(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); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } } + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h index 2d1aef220..f02c51b8e 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h @@ -53,8 +53,14 @@ galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, float early_late_space_chips, - float very_early_late_space_chips); + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot); /*! * \brief This class implements a code DLL + carrier PLL VEML (Very Early @@ -88,8 +94,14 @@ private: std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, float early_late_space_chips, - float very_early_late_space_chips); + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot); galileo_e1_dll_pll_veml_tracking_cc(long if_freq, long fs_in, unsigned @@ -98,12 +110,25 @@ private: std::string dump_filename, float pll_bw_hz, float dll_bw_hz, + float pll_bw_narrow_hz, + float dll_bw_narrow_hz, float early_late_space_chips, - float very_early_late_space_chips); + float very_early_late_space_chips, + float early_late_space_narrow_chips, + float very_early_late_space_narrow_chips, + int extend_correlation_symbols, + bool track_pilot); + bool cn0_and_tracking_lock_status(); + void do_correlation_step(const gr_complex* input_samples); + void run_dll_pll(bool disable_costas_loop); void update_local_code(); - void update_local_carrier(); + bool acquire_secondary(); + + void clear_tracking_vars(); + + void log_data(); // tracking configuration vars unsigned int d_vector_length; @@ -114,16 +139,29 @@ private: long d_if_freq; long d_fs_in; + //tracking state machine + int d_state; + //Integration period in samples int d_correlation_length_samples; int d_n_correlator_taps; double d_early_late_spc_chips; double d_very_early_late_spc_chips; - float* d_ca_code; + double d_early_late_spc_narrow_chips; + double d_very_early_late_spc_narrow_chips; + + float* d_tracking_code; + float* d_data_code; float* d_local_code_shift_chips; gr_complex* d_correlator_outs; cpu_multicorrelator_real_codes multicorrelator_cpu; + //todo: currently the multicorrelator does not support adding extra correlator + //with different local code, thus we need extra multicorrelator instance. + //Implement this functionality inside multicorrelator class + //as an enhancement to increase the performance + float* d_local_code_data_shift_chips; + cpu_multicorrelator_real_codes correlator_data_cpu; //for data channel gr_complex *d_Very_Early; gr_complex *d_Early; @@ -131,6 +169,22 @@ private: gr_complex *d_Late; gr_complex *d_Very_Late; + int d_extend_correlation_symbols; + int d_extend_correlation_symbols_count; + bool d_enable_extended_integration; + int d_current_symbol; + + gr_complex d_VE_accu; + gr_complex d_E_accu; + gr_complex d_P_accu; + gr_complex d_L_accu; + gr_complex d_VL_accu; + + bool d_track_pilot; + gr_complex *d_Prompt_Data; + + double d_code_phase_step_chips; + double d_carrier_phase_step_rad; // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; double d_rem_carr_phase_rad; @@ -143,11 +197,24 @@ private: double d_acq_code_phase_samples; double d_acq_carrier_doppler_hz; + // tracking parameters + float d_dll_bw_hz; + float d_pll_bw_hz; + float d_dll_bw_narrow_hz; + float d_pll_bw_narrow_hz; // tracking vars + double d_carr_error_hz; + double d_carr_error_filt_hz; + double d_code_error_chips; + double d_code_error_filt_chips; + + double d_K_blk_samples; + double d_code_freq_chips; double d_carrier_doppler_hz; double d_acc_carrier_phase_rad; - double d_acc_code_phase_secs; + double d_rem_code_phase_chips; + double d_code_phase_samples; //PRN period in samples int d_current_prn_length_samples; @@ -158,16 +225,13 @@ private: // CN0 estimation and lock detector int d_cn0_estimation_counter; + std::deque d_Prompt_buffer_deque; gr_complex* d_Prompt_buffer; double d_carrier_lock_test; double d_CN0_SNV_dB_Hz; double d_carrier_lock_threshold; int d_carrier_lock_fail_counter; - // control vars - bool d_enable_tracking; - bool d_pull_in; - // file dump std::string d_dump_filename; std::ofstream d_dump_file; diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index a95afd1ca..be5704a6e 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -57,7 +57,7 @@ const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-car const double Galileo_E1_SUB_CARRIER_B_RATE_HZ = 6.138e6; //!< Galileo E1 sub-carrier 'b' rate [Hz] const double Galileo_E1_B_CODE_LENGTH_CHIPS = 4092.0; //!< Galileo E1-B code length [chips] const double Galileo_E1_B_SYMBOL_RATE_BPS = 250.0; //!< Galileo E1-B symbol rate [bits/second] -const double Galileo_E1_C_SECONDARY_CODE_LENGTH = 25.0; //!< Galileo E1-C secondary code length [chips] +const int Galileo_E1_C_SECONDARY_CODE_LENGTH = 25; //!< Galileo E1-C secondary code length [chips] const int Galileo_E1_NUMBER_OF_CODES = 50; const double GALILEO_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) From 25127a0b273ac96f7381fe2256724cc88cccbf41 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Wed, 10 Jan 2018 16:24:09 +0100 Subject: [PATCH 28/55] Minor changes --- .../tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc | 2 ++ src/core/receiver/gnss_flowgraph.cc | 1 + 2 files changed, 3 insertions(+) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index ec5004c56..8911db3db 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -185,6 +185,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() { + gr::thread::scoped_lock lk(d_setlock); /* * correct the code phase according to the delay between acq and trk */ @@ -521,6 +522,7 @@ 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 __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { + gr::thread::scoped_lock lk(d_setlock); // process vars double carr_error_hz = 0.0; double carr_error_filt_hz = 0.0; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index cf1226102..a59d4e5d1 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -382,6 +382,7 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_state_[i] = 1; channels_.at(i)->set_signal(search_next_signal(channels_.at(i)->get_signal().get_signal_str(), true)); acq_channels_count_++; + DLOG(INFO) << "Channel "<< i << " Starting acquisition " << channels_.at(i)->get_signal().get_satellite() << ", Signal " << channels_.at(i)->get_signal().get_signal_str(); channels_.at(i)->start_acquisition(); } DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; From 81179a9f38d5f9a6f26b412bd15b473b61ff6af3 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 10 Jan 2018 16:26:05 +0100 Subject: [PATCH 29/55] Removing gnuradio output buffer request on observables and disabling the gnss_synchro output for all trackings that are not tracking any satellite. This is a bug fix to avoid gnuradio flowgraph deadlocks when different GNSS integration times are used in satellite channels --- .../gnuradio_blocks/hybrid_observables_cc.cc | 28 ++++++++++++++----- .../gnuradio_blocks/hybrid_observables_cc.h | 3 +- .../galileo_e1_dll_pll_veml_tracking_cc.cc | 2 +- .../galileo_e1_tcp_connector_tracking_cc.cc | 7 ++++- .../galileo_e5a_dll_pll_tracking_cc.cc | 8 +++++- .../gps_l1_ca_dll_pll_c_aid_tracking_cc.cc | 7 ++++- ...ps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc | 7 ++++- .../gps_l1_ca_dll_pll_c_aid_tracking_sc.cc | 7 ++++- .../gps_l1_ca_dll_pll_tracking_cc.cc | 8 +++++- .../gps_l1_ca_dll_pll_tracking_gpu_cc.cc | 7 ++++- .../gps_l1_ca_tcp_connector_tracking_cc.cc | 7 ++++- .../gps_l2_m_dll_pll_tracking_cc.cc | 7 ++++- .../gps_l5i_dll_pll_tracking_cc.cc | 7 ++++- 13 files changed, 85 insertions(+), 20 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index d63570996..bb548e25d 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -66,11 +66,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, { d_gnss_synchro_history_queue.push_back(std::deque()); } - // todo: this is a gnuradio scheduler hack. - // Migrate the queues to gnuradio set_history to see if the scheduler can handle - // the multiple output flow - d_max_noutputs = 100; - this->set_min_noutput_items(100); // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) @@ -328,8 +323,15 @@ bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) return (a.TOW_at_current_symbol_s) < (b); } +void hybrid_observables_cc::forecast (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) +{ + for(unsigned int i = 0; i < d_nchannels; i++) + { + ninput_items_required[i] = 0; //set the required available samples in each call + } +} -int hybrid_observables_cc::general_work (int noutput_items __attribute__((unused)), +int hybrid_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) @@ -351,15 +353,27 @@ int hybrid_observables_cc::general_work (int noutput_items __attribute__((unused * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel * Record all synchronization data into queues */ + bool zero_samples=true; for (unsigned int i = 0; i < d_nchannels; i++) { n_consume[i] = ninput_items[i];// full throttle for (int j = 0; j < n_consume[i]; j++) { d_gnss_synchro_history_queue[i].push_back(in[i][j]); + zero_samples=false; } } + //check if there are new channel data available + //This is required because the combination of several GNSS tracking signals + //leads to a multirrate inputs that can not warantee that every channel will have new data + //and forecast method is set to zero samples for each channel to avoid blockings + if (zero_samples==true) + { + usleep(500); // run this task at up to 2 kHz rate + return 0; // No new samples in this call, thus, return. + } + bool channel_history_ok; do { @@ -561,7 +575,7 @@ int hybrid_observables_cc::general_work (int noutput_items __attribute__((unused } } } - } while(channel_history_ok == true && d_max_noutputs > n_outputs); + } while(channel_history_ok == true && noutput_items > n_outputs); // Multi-rate consume! for (unsigned int i = 0; i < d_nchannels; i++) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index 4d7e67338..494f84e75 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -55,7 +55,7 @@ public: ~hybrid_observables_cc (); int general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - + void forecast (int noutput_items, gr_vector_int &ninput_items_required); private: friend hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); @@ -66,7 +66,6 @@ private: double T_rx_s; double T_rx_step_s; - int d_max_noutputs; bool d_dump; unsigned int d_nchannels; unsigned int history_deep; diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index a21865500..cd6b9764c 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -953,7 +953,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri if (current_synchro_data.Flag_valid_symbol_output) { - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + return 1; }else{ return 0; } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index 6204215a5..995941285 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -513,7 +513,12 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attr consume_each(d_current_prn_length_samples); // this is needed in gr::block derivates 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 + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc index f36dc1139..fd4ab4864 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc @@ -733,7 +733,13 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute d_secondary_delay = (d_secondary_delay + 1) % Galileo_E5a_Q_SECONDARY_CODE_LENGTH; d_sample_counter += d_current_prn_length_samples; //count for the processed samples consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + + if (current_synchro_data.Flag_valid_symbol_output) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc index 4bab79242..5264b141a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -875,7 +875,12 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_correlation_length_samples; //count for the processed samples - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc index 0b39b1ebe..464ef6493 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc @@ -694,7 +694,12 @@ int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::general_work( //consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_correlation_length_samples; //count for the processed samples - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc index 8973bb6a4..2d7aa4102 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -879,7 +879,12 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_correlation_length_samples; //count for the processed samples - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index ec5004c56..b90860ada 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -736,7 +736,13 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates 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 + + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index cbd835b86..b79479d68 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -536,7 +536,12 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items __attribu consume_each(d_correlation_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_correlation_length_samples; //count for the processed samples - return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index 9bb40eefa..219aaff76 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -549,7 +549,12 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attri d_sample_counter_seconds = d_sample_counter_seconds + ( static_cast(d_current_prn_length_samples) / static_cast(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 + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc index dfa1b6990..24224ede0 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc @@ -729,7 +729,12 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__( } consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates 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 + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc index 3fd0a151c..100f8c62f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc @@ -729,7 +729,12 @@ int gps_l5i_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(( } consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates 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 + if (d_enable_tracking) + { + return 1; + }else{ + return 0; + } } From 184bd0d1de98482dc15cbe4ebaa16077365f4181 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Wed, 10 Jan 2018 18:13:46 +0100 Subject: [PATCH 30/55] A smarter way of handling the multirrate input of observables block --- .../gnuradio_blocks/hybrid_observables_cc.cc | 688 +++++++++--------- 1 file changed, 345 insertions(+), 343 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index bb548e25d..f9fb4a936 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -37,6 +37,8 @@ #include #include #include +#include +#include #include #include #include "Galileo_E1.h" @@ -52,8 +54,8 @@ hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels, bo hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) : - gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) + gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) { // initialize internal vars d_dump = dump; @@ -63,49 +65,49 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, T_rx_s = 0.0; T_rx_step_s = 1e-3; // todo: move to gnss-sdr config for (unsigned int i = 0; i < d_nchannels; i++) - { - d_gnss_synchro_history_queue.push_back(std::deque()); - } + { + d_gnss_synchro_history_queue.push_back(std::deque()); + } // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) + { + if (d_dump_file.is_open() == false) { - 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); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - } - } + 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); + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure & e) + { + LOG(WARNING) << "Exception opening observables dump file " << e.what(); + } } + } } hybrid_observables_cc::~hybrid_observables_cc() { if (d_dump_file.is_open() == true) + { + try { - try - { - d_dump_file.close(); - } - catch(const std::exception & ex) - { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); - } + d_dump_file.close(); } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + } + } if(d_dump == true) - { - std::cout << "Writing observables .mat files ..."; - hybrid_observables_cc::save_matfile(); - std::cout << " done." << std::endl; - } + { + std::cout << "Writing observables .mat files ..."; + hybrid_observables_cc::save_matfile(); + std::cout << " done." << std::endl; + } } @@ -119,25 +121,25 @@ int hybrid_observables_cc::save_matfile() dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try { - dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); } catch(const std::ifstream::failure &e) { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; } // count number of epochs and rewind long int num_epoch = 0; if (dump_file.is_open()) - { - size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); - dump_file.seekg(0, std::ios::beg); - } + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } else - { - return 1; - } + { + return 1; + } double ** RX_time = new double * [d_nchannels]; double ** TOW_at_current_symbol_s = new double * [d_nchannels]; double ** Carrier_Doppler_hz = new double * [d_nchannels]; @@ -147,58 +149,58 @@ int hybrid_observables_cc::save_matfile() double ** Flag_valid_pseudorange = new double * [d_nchannels]; for(unsigned int i = 0; i < d_nchannels; i++) - { - RX_time[i] = new double [num_epoch]; - TOW_at_current_symbol_s[i] = new double[num_epoch]; - Carrier_Doppler_hz[i] = new double[num_epoch]; - Carrier_phase_cycles[i] = new double[num_epoch]; - Pseudorange_m[i] = new double[num_epoch]; - PRN[i] = new double[num_epoch]; - Flag_valid_pseudorange[i] = new double[num_epoch]; - } + { + RX_time[i] = new double [num_epoch]; + TOW_at_current_symbol_s[i] = new double[num_epoch]; + Carrier_Doppler_hz[i] = new double[num_epoch]; + Carrier_phase_cycles[i] = new double[num_epoch]; + Pseudorange_m[i] = new double[num_epoch]; + PRN[i] = new double[num_epoch]; + Flag_valid_pseudorange[i] = new double[num_epoch]; + } try { - if (dump_file.is_open()) + if (dump_file.is_open()) + { + for(long int i = 0; i < num_epoch; i++) + { + for(unsigned int chan = 0; chan < d_nchannels; chan++) { - for(long int i = 0; i < num_epoch; i++) - { - for(unsigned int chan = 0; chan < d_nchannels; chan++) - { - dump_file.read(reinterpret_cast(&RX_time[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&TOW_at_current_symbol_s[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Carrier_Doppler_hz[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Carrier_phase_cycles[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Pseudorange_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&PRN[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Flag_valid_pseudorange[chan][i]), sizeof(double)); - } - } + dump_file.read(reinterpret_cast(&RX_time[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_s[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Carrier_Doppler_hz[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Carrier_phase_cycles[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Pseudorange_m[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&PRN[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Flag_valid_pseudorange[chan][i]), sizeof(double)); } - dump_file.close(); + } + } + dump_file.close(); } catch (const std::ifstream::failure &e) { - std::cerr << "Problem reading dump file:" << e.what() << std::endl; - for(unsigned int i = 0; i < d_nchannels; i++) - { - delete[] RX_time[i]; - delete[] TOW_at_current_symbol_s[i]; - delete[] Carrier_Doppler_hz[i]; - delete[] Carrier_phase_cycles[i]; - delete[] Pseudorange_m[i]; - delete[] PRN[i]; - delete[] Flag_valid_pseudorange[i]; - } - delete[] RX_time; - delete[] TOW_at_current_symbol_s; - delete[] Carrier_Doppler_hz; - delete[] Carrier_phase_cycles; - delete[] Pseudorange_m; - delete[] PRN; - delete[] Flag_valid_pseudorange; + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + for(unsigned int i = 0; i < d_nchannels; i++) + { + delete[] RX_time[i]; + delete[] TOW_at_current_symbol_s[i]; + delete[] Carrier_Doppler_hz[i]; + delete[] Carrier_phase_cycles[i]; + delete[] Pseudorange_m[i]; + delete[] PRN[i]; + delete[] Flag_valid_pseudorange[i]; + } + delete[] RX_time; + delete[] TOW_at_current_symbol_s; + delete[] Carrier_Doppler_hz; + delete[] Carrier_phase_cycles; + delete[] Pseudorange_m; + delete[] PRN; + delete[] Flag_valid_pseudorange; - return 1; + return 1; } double * RX_time_aux = new double [d_nchannels * num_epoch]; @@ -210,19 +212,19 @@ int hybrid_observables_cc::save_matfile() double * Flag_valid_pseudorange_aux = new double[d_nchannels * num_epoch]; unsigned int k = 0; for(long int j = 0; j < num_epoch; j++ ) + { + for(unsigned int i = 0; i < d_nchannels; i++ ) { - for(unsigned int i = 0; i < d_nchannels; i++ ) - { - RX_time_aux[k] = RX_time[i][j]; - TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j]; - Carrier_Doppler_hz_aux[k] = Carrier_Doppler_hz[i][j]; - Carrier_phase_cycles_aux[k] = Carrier_phase_cycles[i][j]; - Pseudorange_m_aux[k] = Pseudorange_m[i][j]; - PRN_aux[k] = PRN[i][j]; - Flag_valid_pseudorange_aux[k] = Flag_valid_pseudorange[i][j]; - k++; - } + RX_time_aux[k] = RX_time[i][j]; + TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j]; + Carrier_Doppler_hz_aux[k] = Carrier_Doppler_hz[i][j]; + Carrier_phase_cycles_aux[k] = Carrier_phase_cycles[i][j]; + Pseudorange_m_aux[k] = Pseudorange_m[i][j]; + PRN_aux[k] = PRN[i][j]; + Flag_valid_pseudorange_aux[k] = Flag_valid_pseudorange[i][j]; + k++; } + } // WRITE MAT FILE mat_t *matfp; @@ -232,49 +234,49 @@ int hybrid_observables_cc::save_matfile() filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if(reinterpret_cast(matfp) != NULL) - { - size_t dims[2] = {static_cast(d_nchannels), static_cast(num_epoch)}; - matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + { + size_t dims[2] = {static_cast(d_nchannels), static_cast(num_epoch)}; + matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("TOW_at_current_symbol_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, TOW_at_current_symbol_s_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("TOW_at_current_symbol_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, TOW_at_current_symbol_s_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Carrier_Doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_Doppler_hz_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Carrier_Doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_Doppler_hz_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Carrier_phase_cycles", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_phase_cycles_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Carrier_phase_cycles", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_phase_cycles_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Pseudorange_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Pseudorange_m_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Pseudorange_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Pseudorange_m_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, PRN_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("PRN", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, PRN_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Flag_valid_pseudorange", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Flag_valid_pseudorange_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - } + matvar = Mat_VarCreate("Flag_valid_pseudorange", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Flag_valid_pseudorange_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } Mat_Close(matfp); for(unsigned int i = 0; i < d_nchannels; i++) - { - delete[] RX_time[i]; - delete[] TOW_at_current_symbol_s[i]; - delete[] Carrier_Doppler_hz[i]; - delete[] Carrier_phase_cycles[i]; - delete[] Pseudorange_m[i]; - delete[] PRN[i]; - delete[] Flag_valid_pseudorange[i]; + { + delete[] RX_time[i]; + delete[] TOW_at_current_symbol_s[i]; + delete[] Carrier_Doppler_hz[i]; + delete[] Carrier_phase_cycles[i]; + delete[] Pseudorange_m[i]; + delete[] PRN[i]; + delete[] Flag_valid_pseudorange[i]; - } + } delete[] RX_time; delete[] TOW_at_current_symbol_s; delete[] Carrier_Doppler_hz; @@ -325,16 +327,28 @@ bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) void hybrid_observables_cc::forecast (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) { + + bool zero_samples=true; for(unsigned int i = 0; i < d_nchannels; i++) + { + int items=detail()->input(i)->items_available(); + if (items>0) zero_samples=false; + ninput_items_required[i] = items; //set the required available samples in each call + } + + if (zero_samples==true) + { + for(unsigned int i = 0; i < d_nchannels; i++) { - ninput_items_required[i] = 0; //set the required available samples in each call + ninput_items_required[i] = 1; //set the required available samples in each call } + } } int hybrid_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) + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) { const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // Get the output buffer pointer @@ -345,243 +359,231 @@ int hybrid_observables_cc::general_work (int noutput_items , Gnss_Synchro current_gnss_synchro[d_nchannels]; Gnss_Synchro aux = Gnss_Synchro(); for(unsigned int i = 0; i < d_nchannels; i++) - { - current_gnss_synchro[i] = aux; - } + { + current_gnss_synchro[i] = aux; + } /* * 1. Read the GNSS SYNCHRO objects from available channels. * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel * Record all synchronization data into queues */ - bool zero_samples=true; for (unsigned int i = 0; i < d_nchannels; i++) + { + n_consume[i] = ninput_items[i];// full throttle + for (int j = 0; j < n_consume[i]; j++) { - n_consume[i] = ninput_items[i];// full throttle - for (int j = 0; j < n_consume[i]; j++) - { - d_gnss_synchro_history_queue[i].push_back(in[i][j]); - zero_samples=false; - } - } - - //check if there are new channel data available - //This is required because the combination of several GNSS tracking signals - //leads to a multirrate inputs that can not warantee that every channel will have new data - //and forecast method is set to zero samples for each channel to avoid blockings - if (zero_samples==true) - { - usleep(500); // run this task at up to 2 kHz rate - return 0; // No new samples in this call, thus, return. + d_gnss_synchro_history_queue[i].push_back(in[i][j]); } + } bool channel_history_ok; do + { + channel_history_ok = true; + for (unsigned int i = 0; i < d_nchannels; i++) { - channel_history_ok = true; + if (d_gnss_synchro_history_queue[i].size() < history_deep) + { + channel_history_ok = false; + } + } + if (channel_history_ok == true) + { + std::map::const_iterator gnss_synchro_map_iter; + std::deque::const_iterator gnss_synchro_deque_iter; + + // 1. If the RX time is not set, set the Rx time + if (T_rx_s == 0) + { + // 0. Read a gnss_synchro snapshot from the queue and store it in a map + std::map gnss_synchro_map; + for (unsigned int i = 0; i < d_nchannels; i++) + { + gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].front().Channel_ID, + d_gnss_synchro_history_queue[i].front())); + } + gnss_synchro_map_iter = min_element(gnss_synchro_map.cbegin(), + gnss_synchro_map.cend(), + Hybrid_pairCompare_gnss_synchro_sample_counter); + T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / static_cast(gnss_synchro_map_iter->second.fs); + T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms + T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate + } + + // 2. Realign RX time in all valid channels + std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx + std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate + // shift channels history to match the reference TOW for (unsigned int i = 0; i < d_nchannels; i++) + { + gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), + d_gnss_synchro_history_queue[i].cend(), + T_rx_s, + Hybrid_valueCompare_gnss_synchro_receiver_time); + if (gnss_synchro_deque_iter != d_gnss_synchro_history_queue[i].cend()) { - if (d_gnss_synchro_history_queue[i].size() < history_deep) + if (gnss_synchro_deque_iter->Flag_valid_word == true) + { + double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); + double delta_T_rx_s = T_rx_channel - T_rx_s; + + // check that T_rx difference is less than a threshold (the correlation interval) + if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) { - channel_history_ok = false; - } - } - if (channel_history_ok == true) - { - std::map::const_iterator gnss_synchro_map_iter; - std::deque::const_iterator gnss_synchro_deque_iter; - - // 1. If the RX time is not set, set the Rx time - if (T_rx_s == 0) - { - // 0. Read a gnss_synchro snapshot from the queue and store it in a map - std::map gnss_synchro_map; - for (unsigned int i = 0; i < d_nchannels; i++) + // record the word structure in a map for pseudorange computation + // save the previous observable + int distance = std::distance(d_gnss_synchro_history_queue[i].cbegin(), gnss_synchro_deque_iter); + if (distance > 0) + { + if (d_gnss_synchro_history_queue[i].at(distance - 1).Flag_valid_word) { - gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].front().Channel_ID, - d_gnss_synchro_history_queue[i].front())); - } - gnss_synchro_map_iter = min_element(gnss_synchro_map.cbegin(), - gnss_synchro_map.cend(), - Hybrid_pairCompare_gnss_synchro_sample_counter); - T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / static_cast(gnss_synchro_map_iter->second.fs); - T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms - T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate - } - - // 2. Realign RX time in all valid channels - std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx - std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate - // shift channels history to match the reference TOW - for (unsigned int i = 0; i < d_nchannels; i++) - { - gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), - d_gnss_synchro_history_queue[i].cend(), - T_rx_s, - Hybrid_valueCompare_gnss_synchro_receiver_time); - if (gnss_synchro_deque_iter != d_gnss_synchro_history_queue[i].cend()) - { - if (gnss_synchro_deque_iter->Flag_valid_word == true) - { - double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s = T_rx_channel - T_rx_s; - - // check that T_rx difference is less than a threshold (the correlation interval) - if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) - { - // record the word structure in a map for pseudorange computation - // save the previous observable - int distance = std::distance(d_gnss_synchro_history_queue[i].cbegin(), gnss_synchro_deque_iter); - if (distance > 0) - { - if (d_gnss_synchro_history_queue[i].at(distance - 1).Flag_valid_word) - { - double T_rx_channel_prev = static_cast(d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; - if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) - { - realigned_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); - adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - adjacent_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); - } - } - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - } - - } - } - } - } - - if(!realigned_gnss_synchro_map.empty()) - { - /* - * 2.1 Use CURRENT set of measurements and find the nearest satellite - * common RX time algorithm - */ - // what is the most recent symbol TOW in the current set? -> this will be the reference symbol - gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.cbegin(), - realigned_gnss_synchro_map.cend(), - Hybrid_pairCompare_gnss_synchro_d_TOW); - double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - - // compute interpolated TOW value at T_rx_s - int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; - Gnss_Synchro adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); - double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; - - double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; - - double selected_T_rx_s = T_rx_s; - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + - (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); - - // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double channel_T_rx_s; - double channel_fs_hz; - double channel_TOW_s; - for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) - { - channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; - // compute interpolated observation values - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - // TOW at the selected receiver time T_rx_s - int element_key = gnss_synchro_map_iter->second.Channel_ID; - adj_obs = adjacent_gnss_synchro_map.at(element_key); - - double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; - - double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); - - // Doppler and Accumulated carrier phase - double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); - double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); - - // compute the pseudorange (no rx time offset correction) - traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; - // convert to meters - pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] - // update the pseudorange object - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; - // Save the estimated RX time (no RX clock offset correction yet!) - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; - - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; - } - - if(d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try + double T_rx_channel_prev = static_cast(d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); + double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; + if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) - { - tmp_double = current_gnss_synchro[i].RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Pseudorange_m; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].PRN; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } + realigned_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance - 1))); + adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); } - catch (const std::ifstream::failure& e) + else { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + adjacent_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance - 1))); } } + } + else + { + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + } - for (unsigned int i = 0; i < d_nchannels; i++) - { - out[i][n_outputs] = current_gnss_synchro[i]; - } - - n_outputs++; - } - - // Move RX time - T_rx_s = T_rx_s + T_rx_step_s; - // pop old elements from queue - for (unsigned int i = 0; i < d_nchannels; i++) - { - while (static_cast(d_gnss_synchro_history_queue[i].front().Tracking_sample_counter) / static_cast(d_gnss_synchro_history_queue[i].front().fs) < (T_rx_s - past_history_s)) - { - d_gnss_synchro_history_queue[i].pop_front(); - } } + } } - } while(channel_history_ok == true && noutput_items > n_outputs); + } + + if(!realigned_gnss_synchro_map.empty()) + { + /* + * 2.1 Use CURRENT set of measurements and find the nearest satellite + * common RX time algorithm + */ + // what is the most recent symbol TOW in the current set? -> this will be the reference symbol + gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.cbegin(), + realigned_gnss_synchro_map.cend(), + Hybrid_pairCompare_gnss_synchro_d_TOW); + double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); + + // compute interpolated TOW value at T_rx_s + int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; + Gnss_Synchro adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); + double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; + + double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; + + double selected_T_rx_s = T_rx_s; + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + + (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); + + // Now compute RX time differences due to the PRN alignment in the correlators + double traveltime_ms; + double pseudorange_m; + double channel_T_rx_s; + double channel_fs_hz; + double channel_TOW_s; + for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) + { + channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); + channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; + // compute interpolated observation values + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + // TOW at the selected receiver time T_rx_s + int element_key = gnss_synchro_map_iter->second.Channel_ID; + adj_obs = adjacent_gnss_synchro_map.at(element_key); + + double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; + + double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); + + // Doppler and Accumulated carrier phase + double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); + double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); + + // compute the pseudorange (no rx time offset correction) + traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; + // convert to meters + pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] + // update the pseudorange object + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; + // Save the estimated RX time (no RX clock offset correction yet!) + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; + + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; + } + + 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 = current_gnss_synchro[i].RX_time; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Pseudorange_m; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].PRN; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + + for (unsigned int i = 0; i < d_nchannels; i++) + { + out[i][n_outputs] = current_gnss_synchro[i]; + } + + n_outputs++; + } + + // Move RX time + T_rx_s = T_rx_s + T_rx_step_s; + // pop old elements from queue + for (unsigned int i = 0; i < d_nchannels; i++) + { + while (static_cast(d_gnss_synchro_history_queue[i].front().Tracking_sample_counter) / static_cast(d_gnss_synchro_history_queue[i].front().fs) < (T_rx_s - past_history_s)) + { + d_gnss_synchro_history_queue[i].pop_front(); + } + } + } + } while(channel_history_ok == true && noutput_items > n_outputs); // Multi-rate consume! for (unsigned int i = 0; i < d_nchannels; i++) - { - consume(i, n_consume[i]); // which input, how many items - } + { + consume(i, n_consume[i]); // which input, how many items + } return n_outputs; } From 29b8643def49d7d15cd6a632bfd2353745c4bb44 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 12 Jan 2018 13:15:20 +0100 Subject: [PATCH 31/55] Added Spirent GSS6450 file signal source --- .../signal_source/adapters/CMakeLists.txt | 1 + .../spir_gss6450_file_signal_source.cc | 261 ++++++++++++++++++ .../spir_gss6450_file_signal_source.h | 130 +++++++++ .../gnuradio_blocks/CMakeLists.txt | 1 + .../unpack_spir_gss6450_samples.cc | 75 +++++ .../unpack_spir_gss6450_samples.h | 62 +++++ 6 files changed, 530 insertions(+) create mode 100644 src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc create mode 100644 src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h create mode 100644 src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc create mode 100644 src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h diff --git a/src/algorithms/signal_source/adapters/CMakeLists.txt b/src/algorithms/signal_source/adapters/CMakeLists.txt index bac9aee3d..3d786bb46 100644 --- a/src/algorithms/signal_source/adapters/CMakeLists.txt +++ b/src/algorithms/signal_source/adapters/CMakeLists.txt @@ -139,6 +139,7 @@ set(SIGNAL_SOURCE_ADAPTER_SOURCES file_signal_source.cc gen_signal_source.cc nsr_file_signal_source.cc spir_file_signal_source.cc + spir_gss6450_file_signal_source.cc rtl_tcp_signal_source.cc ${OPT_DRIVER_SOURCES} ) diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc new file mode 100644 index 000000000..6e577810a --- /dev/null +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc @@ -0,0 +1,261 @@ +/*! + * \file spir_file_signal_source.cc + * \brief Implementation of a class that reads signals samples from a SPIR file + * and adapts it to a SignalSourceInterface. + * \author Fran Fabra, 2014 fabra(at)ice.csic.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is not 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 . + * + * ------------------------------------------------------------------------- + */ + +#include "spir_gss6450_file_signal_source.h" +#include +#include +#include +#include +#include +#include +#include +#include "configuration_interface.h" + + +using google::LogMessage; + +DEFINE_string(spir_gss6450_signal_source, "-", + "If defined, path to the file containing the Spirent GSS6450 signal samples (overrides the configuration file)"); + + +SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(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_filename = "../data/my_capture.dat"; + std::string default_dump_filename = "../data/my_capture_dump.dat"; + item_type_ = "int"; + + samples_ = configuration->property(role + ".samples", 0); + sampling_frequency_ = configuration->property(role + ".sampling_frequency", 0); + filename_ = configuration->property(role + ".filename", default_filename); + + // override value with commandline flag, if present + if (FLAGS_spir_gss6450_signal_source.compare("-") != 0) filename_= FLAGS_spir_gss6450_signal_source; + + repeat_ = configuration->property(role + ".repeat", false); + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); + enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false); + adc_bits_ = configuration->property(role + ".adc_bits", 4); + n_channels_ = configuration->property(role + ".RF_channels", 1); + sel_ch_ = configuration->property(role + ".sel_ch", 1); + item_size_ = sizeof(int); + long bytes_seek = 65536; + double sample_size_byte = static_cast(adc_bits_) / 4.0; + int samples_per_item = 16 / adc_bits_; + + if(sel_ch_ > n_channels_) { LOG(WARNING) << "Invalid RF channel selection"; } + try + { + file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_); + file_source_->seek(bytes_seek, SEEK_SET); + unpack_ii_ = gr::blocks::packed_to_unpacked_ii::make(2 * adc_bits_, gr::GR_MSB_FIRST); + unpack_spir_ = make_unpack_spir_gss6450_samples(n_channels_, sel_ch_, item_size_); + } + catch (const std::exception &e) + { + std::cerr + << "The receiver was configured to work with a file signal source " + << std::endl + << "but the specified file is unreachable by GNSS-SDR." + << std::endl + << "Please modify your configuration file" + << std::endl + << "and point SignalSource.filename to a valid raw data file. Then:" + << std::endl + << "$ gnss-sdr --config_file=/path/to/my_GNSS_SDR_configuration.conf" + << std::endl + << "Examples of configuration files available at:" + << std::endl + << GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/" + << std::endl; + + LOG(WARNING) << "file_signal_source: Unable to open the samples file " + << filename_.c_str() << ", exiting the program."; + throw(e); + } + + DLOG(INFO) << "file_source(" << file_source_->unique_id() << ")"; + + if(samples_ == 0) // read all file + { + /*! + * BUG workaround: The GNU Radio file source does not stop the receiver after reaching the End of File. + * A possible solution is to compute the file length 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(); + LOG(INFO) << "Total samples in the file= " << floor(static_cast(size) / static_cast(item_size())); + } + else + { + std::cout << "file_signal_source: Unable to open the samples file " << filename_.c_str() << std::endl; + LOG(ERROR) << "file_signal_source: Unable to open the samples file " << filename_.c_str(); + } + std::streamsize ss = std::cout.precision(); + std::cout << std::setprecision(16); + std::cout << "Processing file " << filename_ << ", which contains " << size << " [bytes]" << std::endl; + std::cout.precision (ss); + + if(size > 0) + { + samples_ = floor(static_cast(size - bytes_seek) / (sample_size_byte * static_cast(n_channels_))); + samples_ = samples_- ceil(0.002 * static_cast(sampling_frequency_)); //process all the samples available in the file excluding the last 2 ms + } + } + + CHECK(samples_ > 0) << "File does not contain enough samples to process."; + double signal_duration_s; + signal_duration_s = static_cast(samples_) * ( 1 /static_cast(sampling_frequency_)); + LOG(INFO) << "Total number 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]" << std::endl; + + valve_ = gnss_sdr_make_valve(sizeof(gr_complex), samples_, queue_); + DLOG(INFO) << "valve(" << valve_->unique_id() << ")"; + + if (dump_) + { + sink_ = gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str()); + DLOG(INFO) << "file_sink(" << sink_->unique_id() << ")"; + } + + if (enable_throttle_control_) + { + throttle_ = gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_); + } + DLOG(INFO) << "File source filename " << filename_; + DLOG(INFO) << "Samples " << samples_; + DLOG(INFO) << "Sampling frequency " << sampling_frequency_; + DLOG(INFO) << "Item type " << item_type_; + DLOG(INFO) << "Item size " << item_size_; + DLOG(INFO) << "Repeat " << repeat_; + DLOG(INFO) << "Dump " << dump_; + DLOG(INFO) << "Dump filename " << dump_filename_; +} + + + + +SpirGSS6450FileSignalSource::~SpirGSS6450FileSignalSource() +{} + + + + +void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block) +{ + if (samples_ > 0) + { + if (enable_throttle_control_) + { + top_block->connect(file_source_, 0, unpack_ii_, 0); + top_block->connect(unpack_ii_, 0, unpack_spir_, 0); + top_block->connect(unpack_spir_, 0, throttle_, 0); + top_block->connect(throttle_, 0, valve_, 0); + } + else + { + top_block->connect(file_source_, 0, unpack_ii_, 0); + top_block->connect(unpack_ii_, 0, unpack_spir_, 0); + top_block->connect(unpack_spir_, 0, valve_, 0); + } + if(dump_) + { + top_block->connect(valve_, 0, sink_, 0); + DLOG(INFO) << "connected valve to file sink"; + } + } + else + { + LOG(WARNING) << "0 samples to read"; + } +} + + + + + + +void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block) +{ + if (samples_ > 0) + { + if (enable_throttle_control_) + { + top_block->disconnect(file_source_, 0, unpack_ii_, 0); + top_block->disconnect(unpack_ii_, 0, unpack_spir_, 0); + top_block->disconnect(unpack_spir_, 0, throttle_, 0); + top_block->disconnect(throttle_, 0, valve_, 0); + if (dump_) + { + top_block->disconnect(valve_, 0, sink_, 0); + } + } + else + { + top_block->disconnect(file_source_, 0, unpack_ii_, 0); + top_block->disconnect(unpack_ii_, 0, unpack_spir_, 0); + top_block->disconnect(unpack_spir_, 0, valve_, 0); + if (dump_) + { + top_block->disconnect(valve_, 0, sink_, 0); + } + } + } + else + { + LOG(WARNING) << "Nothing to disconnect"; + } +} + + +gr::basic_block_sptr SpirGSS6450FileSignalSource::get_left_block() +{ + LOG(WARNING) << "Left block of a signal source should not be retrieved"; + return gr::blocks::file_source::sptr(); +} + + +gr::basic_block_sptr SpirGSS6450FileSignalSource::get_right_block() +{ + if(samples_ > 0) { return valve_; } + else + { + if(enable_throttle_control_) { return throttle_; } + else { return unpack_spir_; } + } +} diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h new file mode 100644 index 000000000..346ebdc79 --- /dev/null +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h @@ -0,0 +1,130 @@ +/*! + * \file spir_file_signal_source.h + * \brief Implementation of a class that reads signals samples from a SPIR file + * and adapts it to a SignalSourceInterface. + * \author Fran Fabra, 2014 fabra(at)ice.csic.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is not 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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_SPIR_GSS6450_FILE_SIGNAL_SOURCE_H_ +#define GNSS_SDR_SPIR_GSS6450_FILE_SIGNAL_SOURCE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include "gnss_block_interface.h" +#include "gnss_sdr_valve.h" +#include "unpack_spir_gss6450_samples.h" + + +class ConfigurationInterface; + +/*! + * \brief Class that reads signals samples from a file + * and adapts it to a SignalSourceInterface + */ +class SpirGSS6450FileSignalSource: public GNSSBlockInterface +{ +public: + SpirGSS6450FileSignalSource(ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams, gr::msg_queue::sptr queue); + + virtual ~SpirGSS6450FileSignalSource(); + inline std::string role() override + { + return role_; + } + + inline std::string implementation() override + { + return "Spir_GSS6450_File_Signal_Source"; + } + + inline size_t item_size() override + { + return item_size_; + } + + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; + + inline std::string filename() const + { + return filename_; + } + + inline std::string item_type() const + { + return item_type_; + } + + inline bool repeat() const + { + return repeat_; + } + + inline long sampling_frequency() const + { + return sampling_frequency_; + } + + inline long samples() const + { + return samples_; + } + +private: + unsigned long long samples_; + long sampling_frequency_; + std::string filename_; + bool repeat_; + bool dump_; + std::string dump_filename_; + std::string role_; + std::string item_type_; + unsigned int in_streams_; + unsigned int out_streams_; + unsigned int adc_bits_; + unsigned int n_channels_; + unsigned int sel_ch_; + gr::blocks::file_source::sptr file_source_; + gr::blocks::packed_to_unpacked_ii::sptr unpack_ii_; + unpack_spir_gss6450_samples_sptr unpack_spir_; + boost::shared_ptr valve_; + gr::blocks::file_sink::sptr sink_; + gr::blocks::throttle::sptr throttle_; + gr::msg_queue::sptr queue_; + size_t item_size_; + bool enable_throttle_control_; +}; + +#endif /*GNSS_SDR_SPIR_GSS6450_FILE_SIGNAL_SOURCE_H_*/ diff --git a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt index 4159321f3..67b9f3162 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt @@ -23,6 +23,7 @@ set(SIGNAL_SOURCE_GR_BLOCKS_SOURCES unpack_intspir_1bit_samples.cc rtl_tcp_signal_source_c.cc unpack_2bit_samples.cc + unpack_spir_gss6450_samples.cc ) include_directories( diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc new file mode 100644 index 000000000..8d7d4e3fc --- /dev/null +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc @@ -0,0 +1,75 @@ +/*! + * \file unpack_intspir_1bit_samples.cc + * + * \brief Unpacks SPIR int samples to NSR 1 bit samples + * \author Fran Fabra fabra (at) ice.csic.es + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is not 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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "unpack_spir_gss6450_samples.h" +#include + + + +unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, size_t item_size) +{ + return unpack_spir_gss6450_samples_sptr(new unpack_spir_gss6450_samples(n_chann, sel_ch, item_size)); +} + + +unpack_spir_gss6450_samples::unpack_spir_gss6450_samples( + unsigned int n_chann, unsigned int sel_ch, size_t item_size) : gr::block("unpack_spir_gss6450_samples", + gr::io_signature::make(1, 1, sizeof(int)), + gr::io_signature::make(1, 1, sizeof(gr_complex))) +{ + d_channels = n_chann; + d_sel_ch = sel_ch; + item_size_ = item_size; + ch_processing = 1; +} + +void unpack_spir_gss6450_samples::forecast(int noutput_items, gr_vector_int &ninput_items_required) +{ + ninput_items_required[0] = d_channels * noutput_items; +} + + +unpack_spir_gss6450_samples::~unpack_spir_gss6450_samples() +{} + + +int unpack_spir_gss6450_samples::general_work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + const int *in = reinterpret_cast(input_items[0]); + gr_complex *out = reinterpret_cast(output_items[0]); + + + + + return noutput_items; +} diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h new file mode 100644 index 000000000..6e83c88e2 --- /dev/null +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h @@ -0,0 +1,62 @@ +/*! + * \file unpack_intspir_1bit_samples.h + * + * \brief Unpacks SPIR int samples to NSR 1 bit samples + * \author Fran Fabra fabra (at) ice.csic.es + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is not 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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_UNPACK_SPIR_GSS6450_SAMPLES_H +#define GNSS_SDR_UNPACK_SPIR_GSS6450_SAMPLES_H + +#include + +class unpack_spir_gss6450_samples; + +typedef boost::shared_ptr unpack_spir_gss6450_samples_sptr; + +unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, size_t item_size); + + +class unpack_spir_gss6450_samples: public gr::block +{ +public: + int general_work (int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + void forecast(int noutput_items, gr_vector_int &ninput_items_required); + ~unpack_spir_gss6450_samples(); + +private: + unsigned int d_channels; + unsigned int d_sel_ch; + unsigned int ch_processing; + size_t item_size_; + friend unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples_sptr(unsigned int n_chann, unsigned int sel_ch, size_t item_size); + unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, size_t item_size); + +}; + +#endif From 282c3d865998ae49e076aea79505175c80d13555 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Tue, 16 Jan 2018 17:20:55 +0100 Subject: [PATCH 32/55] Implement Spirent GSS6450 file signal source --- .../spir_gss6450_file_signal_source.cc | 7 +- .../unpack_spir_gss6450_samples.cc | 111 +++++++++++++++--- .../unpack_spir_gss6450_samples.h | 18 ++- src/core/receiver/gnss_block_factory.cc | 16 +++ 4 files changed, 128 insertions(+), 24 deletions(-) diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc index 6e577810a..ede0abb72 100644 --- a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc @@ -78,8 +78,8 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* { file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_); file_source_->seek(bytes_seek, SEEK_SET); - unpack_ii_ = gr::blocks::packed_to_unpacked_ii::make(2 * adc_bits_, gr::GR_MSB_FIRST); - unpack_spir_ = make_unpack_spir_gss6450_samples(n_channels_, sel_ch_, item_size_); + unpack_ii_ = gr::blocks::packed_to_unpacked_ii::make(adc_bits_, gr::GR_MSB_FIRST); + unpack_spir_ = make_unpack_spir_gss6450_samples(n_channels_, sel_ch_, samples_per_item, item_size_); } catch (const std::exception &e) { @@ -139,8 +139,7 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* } CHECK(samples_ > 0) << "File does not contain enough samples to process."; - double signal_duration_s; - signal_duration_s = static_cast(samples_) * ( 1 /static_cast(sampling_frequency_)); + double signal_duration_s = static_cast(samples_) * ( 1 /static_cast(sampling_frequency_)); LOG(INFO) << "Total number 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]" << std::endl; diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc index 8d7d4e3fc..d8ae66e39 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc @@ -34,14 +34,14 @@ -unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, size_t item_size) +unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size) { - return unpack_spir_gss6450_samples_sptr(new unpack_spir_gss6450_samples(n_chann, sel_ch, item_size)); + return unpack_spir_gss6450_samples_sptr(new unpack_spir_gss6450_samples(n_chann, sel_ch, samp_item, item_size)); } unpack_spir_gss6450_samples::unpack_spir_gss6450_samples( - unsigned int n_chann, unsigned int sel_ch, size_t item_size) : gr::block("unpack_spir_gss6450_samples", + unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size) : gr::block("unpack_spir_gss6450_samples", gr::io_signature::make(1, 1, sizeof(int)), gr::io_signature::make(1, 1, sizeof(gr_complex))) { @@ -49,27 +49,108 @@ unpack_spir_gss6450_samples::unpack_spir_gss6450_samples( d_sel_ch = sel_ch; item_size_ = item_size; ch_processing = 1; -} - -void unpack_spir_gss6450_samples::forecast(int noutput_items, gr_vector_int &ninput_items_required) -{ - ninput_items_required[0] = d_channels * noutput_items; + d_samp_item = samp_item; + samp_frame = 0; + adc_bits = 16 / d_samp_item; + i_ = true; + new_sample = false; + i_data = 0; + q_data = 0; } unpack_spir_gss6450_samples::~unpack_spir_gss6450_samples() {} +void unpack_spir_gss6450_samples::process_sample(gr_complex* out) +{ + gr_complex result = gr_complex(0.5, 0.5); + compute_two_complement(i_data); + compute_two_complement(q_data); + result += gr_complex(static_cast(i_data), static_cast(q_data)); + *out = result; +} int unpack_spir_gss6450_samples::general_work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - const int *in = reinterpret_cast(input_items[0]); - gr_complex *out = reinterpret_cast(output_items[0]); - - - - - return noutput_items; + const int* in = reinterpret_cast(input_items[0]); + gr_complex* out = reinterpret_cast(output_items[0]); + int samples_produced = 0; + for(int i = 0; i < noutput_items; i++) + { + if(ch_processing == d_sel_ch) + { + if(i_) + { + i_data = in[i]; + swap_data(i_data); + i_ = false; + } + else + { + q_data = in[i]; + swap_data(q_data); + i_ = true; + process_sample(out); + out++; + samples_produced++; + new_sample = true; + } + } + else + { + if(i_) { i_ = false;} + else + { + i_ = true; + new_sample = true; + } + } + if(new_sample) + { + new_sample = false; + samp_frame++; + if(samp_frame == d_samp_item) + { + samp_frame = 0; + ch_processing++; + if(ch_processing > d_channels) { ch_processing = 1; } + } + } + } + consume_each(noutput_items); + return samples_produced; +} + +void unpack_spir_gss6450_samples::swap_data(int& data) +{ + int result = 0; + int aux = data; + int mask = 1; + for (int i = 0; i < adc_bits; i++) + { + result = result << 1; + result += (aux & mask); + aux = aux >> 1; + } + data = result; +} + +void unpack_spir_gss6450_samples::compute_two_complement(int& data) +{ + int result = 0; + int mask = 1; + for(int i = 0; i < (adc_bits - 1); i++) + { + result = result << 1; + result += (data >> i) & mask; + } + if((data >> (adc_bits - 1)) == 1) + { + if(adc_bits == 2) { result -= 2; } + else { result -= 8; } + } + data = result; } diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h index 6e83c88e2..1077fcc73 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h @@ -37,7 +37,7 @@ class unpack_spir_gss6450_samples; typedef boost::shared_ptr unpack_spir_gss6450_samples_sptr; -unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, size_t item_size); +unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size); class unpack_spir_gss6450_samples: public gr::block @@ -46,17 +46,25 @@ public: int general_work (int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - void forecast(int noutput_items, gr_vector_int &ninput_items_required); + friend unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples_sptr(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size); + unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size); ~unpack_spir_gss6450_samples(); private: unsigned int d_channels; unsigned int d_sel_ch; unsigned int ch_processing; + int d_samp_item; + int samp_frame; + int adc_bits; size_t item_size_; - friend unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples_sptr(unsigned int n_chann, unsigned int sel_ch, size_t item_size); - unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, size_t item_size); - + void process_sample(gr_complex* out); + void swap_data(int& data); + void compute_two_complement(int& data); + bool i_; + bool new_sample; + int i_data; + int q_data; }; #endif diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index bdf34dc22..322577e62 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -48,6 +48,7 @@ #include "nsr_file_signal_source.h" #include "two_bit_cpx_file_signal_source.h" #include "spir_file_signal_source.h" +#include "spir_gss6450_file_signal_source.h" #include "rtl_tcp_signal_source.h" #include "two_bit_packed_file_signal_source.h" #include "channel.h" @@ -864,6 +865,21 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams, queue)); block = std::move(block_); + } + catch (const std::exception &e) + { + std::cout << "GNSS-SDR program ended." << std::endl; + exit(1); + } + } + else if (implementation.compare("Spir_GSS6450_File_Signal_Source") == 0) + { + try + { + std::unique_ptr block_(new SpirGSS6450FileSignalSource(configuration.get(), role, in_streams, + out_streams, queue)); + block = std::move(block_); + } catch (const std::exception &e) { From 6af85275182c9a31062c35f0889d324f19776d49 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Tue, 16 Jan 2018 23:10:04 +0100 Subject: [PATCH 33/55] Add block identifier to Galileo satellites launched on Dec. 12, 2017 --- src/core/system_parameters/gnss_satellite.cc | 34 +++++++++++++------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 2e2e18f00..a146b3451 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -492,34 +492,34 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int switch ( PRN_ ) { case 1: - block_ = std::string("FOC-FM10"); // Galileo Full Operational Capability (FOC) satellite FM10 / GSAT-0210, launched on May 24, 2016 + block_ = std::string("FOC-FM10"); // Galileo Full Operational Capability (FOC) satellite FM10 / GSAT-0210, launched on May 24, 2016. break; case 2: - block_ = std::string("FOC-FM11"); // Galileo Full Operational Capability (FOC) satellite FM11 / GSAT-0211, launched on May 24, 2016 + block_ = std::string("FOC-FM11"); // Galileo Full Operational Capability (FOC) satellite FM11 / GSAT-0211, launched on May 24, 2016. break; case 3: - block_ = std::string("FOC-FM12"); // Galileo Full Operational Capability (FOC) satellite FM12 / GSAT-0212, launched on November 17, 2016 + block_ = std::string("FOC-FM12"); // Galileo Full Operational Capability (FOC) satellite FM12 / GSAT-0212, launched on November 17, 2016. break; case 4: - block_ = std::string("FOC-FM13"); // Galileo Full Operational Capability (FOC) satellite FM13 / GSAT-0213, launched on November 17, 2016 + block_ = std::string("FOC-FM13"); // Galileo Full Operational Capability (FOC) satellite FM13 / GSAT-0213, launched on November 17, 2016. break; case 5: - block_ = std::string("FOC-FM14"); // Galileo Full Operational Capability (FOC) satellite FM14 / GSAT-0214, launched on November 17, 2016 + block_ = std::string("FOC-FM14"); // Galileo Full Operational Capability (FOC) satellite FM14 / GSAT-0214, launched on November 17, 2016. break; case 7: - block_ = std::string("FOC-FM7"); // Galileo Full Operational Capability (FOC) satellite FM7 / GSAT-0207, launched on November 17, 2016 + block_ = std::string("FOC-FM7"); // Galileo Full Operational Capability (FOC) satellite FM7 / GSAT-0207, launched on November 17, 2016. break; case 8: - block_ = std::string("FOC-FM8"); // Galileo Full Operational Capability (FOC) satellite FM8 / GSAT0208, launched on December 17, 2015 + block_ = std::string("FOC-FM8"); // Galileo Full Operational Capability (FOC) satellite FM8 / GSAT0208, launched on December 17, 2015. break; case 9: - block_ = std::string("FOC-FM9"); // Galileo Full Operational Capability (FOC) satellite FM9 / GSAT0209, launched on December 17, 2015 + block_ = std::string("FOC-FM9"); // Galileo Full Operational Capability (FOC) satellite FM9 / GSAT0209, launched on December 17, 2015. break; case 11 : - block_ = std::string("IOV-PFM"); // PFM, the ProtoFlight Model / GSAT0101, launched from French Guiana at 10:30 GMT on October 21, 2011 + block_ = std::string("IOV-PFM"); // PFM, the ProtoFlight Model / GSAT0101, launched from French Guiana at 10:30 GMT on October 21, 2011. break; case 12 : - block_ = std::string("IOV-FM2**"); // Galileo In-Orbit Validation (IOV) satellite FM2 (Flight Model 2) also known as GSAT0102, from French Guiana at 10:30 GMT on October 21, 2011 + block_ = std::string("IOV-FM2**"); // Galileo In-Orbit Validation (IOV) satellite FM2 (Flight Model 2) also known as GSAT0102, from French Guiana at 10:30 GMT on October 21, 2011. break; case 14 : block_ = std::string("FOC-FM2*"); // Galileo Full Operational Capability (FOC) satellite FM2 / GSAT0202, launched into incorrect orbit on August 22, 2014. Moved to usable orbit in March, 2015. @@ -528,23 +528,35 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int block_ = std::string("FOC-FM1*"); // Galileo Full Operational Capability (FOC) satellite FM1 / GSAT0201, launched into incorrect orbit on August 22, 2014. Moved to usable orbit in December, 2014. break; case 19 : - block_ = std::string("IOV-FM3"); // Galileo In-Orbit Validation (IOV) satellite FM3 (Flight Model 3) / GSAT0103, launched on October 12, 2012 + block_ = std::string("IOV-FM3"); // Galileo In-Orbit Validation (IOV) satellite FM3 (Flight Model 3) / GSAT0103, launched on October 12, 2012. break; case 20 : block_ = std::string("IOV-FM4**"); // Galileo In-Orbit Validation (IOV) satellite FM4 (Flight Model 4) / GSAT0104, launched on October 12, 2012. Partially unavailable: Payload power problem beginning May 27, 2014 led to permanent loss of E5 and E6 transmissions, E1 transmission restored. break; + case 21 : + block_ = std::string("FOC-FM15"); // Galileo Full Operational Capability (FOC) satellite FM15 / GSAT0215, launched on Dec. 12, 2017. + break; case 22 : block_ = std::string("FOC-FM4"); // Galileo Full Operational Capability (FOC) satellite FM4 / GSAT0204, launched on March 27, 2015. break; case 24 : block_ = std::string("FOC-FM5"); // Galileo Full Operational Capability (FOC) satellite FM5 / GSAT0205, launched on Sept. 11, 2015. break; + case 25 : + block_ = std::string("FOC-FM16"); // Galileo Full Operational Capability (FOC) satellite FM16 / GSAT0216, launched on Dec. 12, 2017. + break; case 26 : block_ = std::string("FOC-FM3"); // Galileo Full Operational Capability (FOC) satellite FM3 / GSAT0203, launched on March 27, 2015. break; + case 27 : + block_ = std::string("FOC-FM17"); // Galileo Full Operational Capability (FOC) satellite FM17 / GSAT0217, launched on Dec. 12, 2017. + break; case 30 : block_ = std::string("FOC-FM6"); // Galileo Full Operational Capability (FOC) satellite FM6 / GSAT0206, launched on Sept. 11, 2015. break; + case 31 : + block_ = std::string("FOC-FM18"); // Galileo Full Operational Capability (FOC) satellite FM18 / GSAT0218, launched on Dec. 12, 2017. + break; default: block_ = std::string("Unknown(Simulated)"); } From 27baa7e45db5c4b74077c57d06f3c87295e875da Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 17 Jan 2018 08:42:50 +0100 Subject: [PATCH 34/55] Apply coding style --- .../galileo_e1_dll_pll_veml_tracking.cc | 19 +- .../galileo_e1_dll_pll_veml_tracking_cc.cc | 863 +++++++++--------- .../gps_l5i_dll_pll_tracking_cc.cc | 12 +- 3 files changed, 457 insertions(+), 437 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc index b47e26a68..3289b3ecf 100755 --- a/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_dll_pll_veml_tracking.cc @@ -81,7 +81,7 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", 0.15); very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", 0.6); - bool track_pilot=configuration->property(role + ".track_pilot", false); + bool track_pilot = configuration->property(role + ".track_pilot", false); std::string default_dump_filename = "./track_ch"; dump_filename = configuration->property(role + ".dump_filename", @@ -108,16 +108,6 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( very_early_late_space_narrow_chips, extend_correlation_symbols, track_pilot); -// tracking_ = galileo_e1_dll_pll_veml_make_tracking_cc( -// f_if, -// fs_in, -// vector_length, -// dump, -// dump_filename, -// pll_bw_hz, -// dll_bw_hz, -// early_late_space_chips, -// very_early_late_space_chips); } else { @@ -130,14 +120,17 @@ GalileoE1DllPllVemlTracking::GalileoE1DllPllVemlTracking( DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; } + GalileoE1DllPllVemlTracking::~GalileoE1DllPllVemlTracking() {} + void GalileoE1DllPllVemlTracking::start_tracking() { tracking_->start_tracking(); } + /* * Set tracking channel unique ID */ @@ -153,23 +146,27 @@ void GalileoE1DllPllVemlTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) tracking_->set_gnss_synchro(p_gnss_synchro); } + void GalileoE1DllPllVemlTracking::connect(gr::top_block_sptr top_block) { if(top_block) { /* top_block is not null */}; //nothing to connect, now the tracking uses gr_sync_decimator } + void GalileoE1DllPllVemlTracking::disconnect(gr::top_block_sptr top_block) { if(top_block) { /* top_block is not null */}; //nothing to disconnect, now the tracking uses gr_sync_decimator } + gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_left_block() { return tracking_; } + gr::basic_block_sptr GalileoE1DllPllVemlTracking::get_right_block() { return tracking_; diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index cd6b9764c..0fd20887b 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -82,30 +82,30 @@ galileo_e1_dll_pll_veml_make_tracking_cc( bool track_pilot) { return galileo_e1_dll_pll_veml_tracking_cc_sptr(new galileo_e1_dll_pll_veml_tracking_cc(if_freq, - fs_in, - vector_length, - dump, - dump_filename, - pll_bw_hz, - dll_bw_hz, - pll_bw_narrow_hz, - dll_bw_narrow_hz, - early_late_space_chips, - very_early_late_space_chips, - early_late_space_narrow_chips, - very_early_late_space_narrow_chips, - extend_correlation_symbols, - track_pilot)); + fs_in, + vector_length, + dump, + dump_filename, + pll_bw_hz, + dll_bw_hz, + pll_bw_narrow_hz, + dll_bw_narrow_hz, + early_late_space_chips, + very_early_late_space_chips, + early_late_space_narrow_chips, + very_early_late_space_narrow_chips, + extend_correlation_symbols, + track_pilot)); } void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items, - gr_vector_int &ninput_items_required) + gr_vector_int &ninput_items_required) { if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } } @@ -125,8 +125,8 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( float very_early_late_space_narrow_chips, int extend_correlation_symbols, bool track_pilot): - gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -146,10 +146,10 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( // Initialize tracking ========================================== // Set bandwidth of code and carrier loop filters - d_dll_bw_hz=dll_bw_hz; - d_pll_bw_hz=pll_bw_hz; - d_dll_bw_narrow_hz=dll_bw_narrow_hz; - d_pll_bw_narrow_hz=pll_bw_narrow_hz; + d_dll_bw_hz = dll_bw_hz; + d_pll_bw_hz = pll_bw_hz; + d_dll_bw_narrow_hz = dll_bw_narrow_hz; + d_pll_bw_narrow_hz = pll_bw_narrow_hz; d_code_loop_filter.set_DLL_BW(d_dll_bw_hz); d_carrier_loop_filter.set_PLL_BW(d_pll_bw_hz); @@ -168,9 +168,9 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_n_correlator_taps = 5; // Very-Early, Early, Prompt, Late, Very-Late d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment())); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } // map memory pointers of correlator outputs d_Very_Early = &d_correlator_outs[0]; d_Early = &d_correlator_outs[1]; @@ -189,29 +189,33 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_correlation_length_samples = d_vector_length; multicorrelator_cpu.init(2 * d_correlation_length_samples, d_n_correlator_taps); - d_extend_correlation_symbols=extend_correlation_symbols; + d_extend_correlation_symbols = extend_correlation_symbols; // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal - d_track_pilot=track_pilot; + d_track_pilot = track_pilot; if (d_track_pilot) - { - //extended integration control - if (d_extend_correlation_symbols>1) { - d_enable_extended_integration=true; - }else{ - d_enable_extended_integration=false; + //extended integration control + if (d_extend_correlation_symbols > 1) + { + d_enable_extended_integration = true; + } + else + { + d_enable_extended_integration = false; + } + //Extra correlator for the data component + d_local_code_data_shift_chips = static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); + d_local_code_data_shift_chips[0] = 0.0; + correlator_data_cpu.init(2 * d_correlation_length_samples, 1); + d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); + d_Prompt_Data[0] = gr_complex(0,0); + d_data_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); + } + else + { + // Disable extended integration if data component tracking is selected + d_enable_extended_integration = false; } - //Extra correlator for the data component - d_local_code_data_shift_chips=static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); - d_local_code_data_shift_chips[0]=0.0; - correlator_data_cpu.init(2 * d_correlation_length_samples, 1); - d_Prompt_Data = static_cast(volk_gnsssdr_malloc(sizeof(gr_complex), volk_gnsssdr_get_alignment())); - d_Prompt_Data[0] = gr_complex(0,0); - d_data_code = static_cast(volk_gnsssdr_malloc((2 * Galileo_E1_B_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment())); - }else{ - // Disable extended integration if data component tracking is selected - d_enable_extended_integration=false; - } //--- Initializations ------------------------------ // Initial code frequency basis of NCO @@ -247,7 +251,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_carrier_doppler_hz = 0.0; d_acc_carrier_phase_rad = 0.0; - d_state=0;// intial state: stanby + d_state = 0;// intial state: stanby } @@ -287,9 +291,9 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() double corrected_acq_phase_samples, delay_correction_samples; corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(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; - } + { + 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; @@ -301,40 +305,41 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() d_carrier_loop_filter.initialize(); // initialize the carrier filter d_code_loop_filter.initialize(); // initialize the code filter - if (d_track_pilot) - { - char pilot_signal[3]="1C"; - galileo_e1_code_gen_float_sampled(d_tracking_code, - pilot_signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - galileo_e1_code_gen_float_sampled(d_data_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - d_Prompt_Data[0]=gr_complex(0,0); //clean data correlator output - correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), - d_data_code, - d_local_code_shift_chips); - }else{ - galileo_e1_code_gen_float_sampled(d_tracking_code, - d_acquisition_gnss_synchro->Signal, - false, - d_acquisition_gnss_synchro->PRN, - Galileo_E1_CODE_CHIP_RATE_HZ, - 0); - } + { + char pilot_signal[3] = "1C"; + galileo_e1_code_gen_float_sampled(d_tracking_code, + pilot_signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + galileo_e1_code_gen_float_sampled(d_data_code, + d_acquisition_gnss_synchro->Signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + d_Prompt_Data[0] = gr_complex(0,0); //clean data correlator output + correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), + d_data_code, + d_local_code_shift_chips); + } + else + { + galileo_e1_code_gen_float_sampled(d_tracking_code, + d_acquisition_gnss_synchro->Signal, + false, + d_acquisition_gnss_synchro->PRN, + Galileo_E1_CODE_CHIP_RATE_HZ, + 0); + } multicorrelator_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), d_tracking_code, d_local_code_shift_chips); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } d_carrier_lock_fail_counter = 0; d_rem_code_phase_samples = 0; @@ -352,139 +357,144 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; // enable tracking pull-in - d_state=1; + d_state = 1; LOG(INFO) << "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; - - + << " Code Phase correction [samples]=" << delay_correction_samples + << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; } galileo_e1_dll_pll_veml_tracking_cc::~galileo_e1_dll_pll_veml_tracking_cc() { if (d_dump_file.is_open()) - { - try { - d_dump_file.close(); + try + { + d_dump_file.close(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor " << ex.what(); + } } - catch(const std::exception & ex) - { - LOG(WARNING) << "Exception in destructor " << ex.what(); - } - } if(d_dump) - { - if(d_channel == 0) { - std::cout << "Writing .mat files ..."; + if(d_channel == 0) + { + std::cout << "Writing .mat files ..."; + } + galileo_e1_dll_pll_veml_tracking_cc::save_matfile(); + if(d_channel == 0) + { + std::cout << " done." << std::endl; + } } - galileo_e1_dll_pll_veml_tracking_cc::save_matfile(); - if(d_channel == 0) - { - std::cout << " done." << std::endl; - } - } try { - volk_gnsssdr_free(d_local_code_shift_chips); - volk_gnsssdr_free(d_correlator_outs); - volk_gnsssdr_free(d_tracking_code); - if (d_track_pilot) - { - volk_gnsssdr_free(d_Prompt_Data); - volk_gnsssdr_free(d_data_code); - volk_gnsssdr_free(d_local_code_data_shift_chips); - correlator_data_cpu.free(); - } - delete[] d_Prompt_buffer; - multicorrelator_cpu.free(); + volk_gnsssdr_free(d_local_code_shift_chips); + volk_gnsssdr_free(d_correlator_outs); + volk_gnsssdr_free(d_tracking_code); + if (d_track_pilot) + { + volk_gnsssdr_free(d_Prompt_Data); + volk_gnsssdr_free(d_data_code); + volk_gnsssdr_free(d_local_code_data_shift_chips); + correlator_data_cpu.free(); + } + delete[] d_Prompt_buffer; + multicorrelator_cpu.free(); } catch(const std::exception & ex) { - LOG(WARNING) << "Exception in destructor " << ex.what(); + LOG(WARNING) << "Exception in destructor " << ex.what(); } } + bool galileo_e1_dll_pll_veml_tracking_cc::acquire_secondary() { //******* preamble correlation ******** - int corr_value=0; + int corr_value = 0; for (unsigned int i = 0; i < Galileo_E1_C_SECONDARY_CODE_LENGTH; i++) - { - if (d_Prompt_buffer_deque.at(i).real() < 0) // symbols clipping { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') - { - corr_value++; - } + if (d_Prompt_buffer_deque.at(i).real() < 0) // symbols clipping + { + if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + { + corr_value++; + } + else + { + corr_value--; + } + } else - { - corr_value--; - } + { + if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') + { + corr_value--; + } + else + { + corr_value++; + } + } } - else - { - if (Galileo_E1_C_SECONDARY_CODE.at(i) == '0') - { - corr_value--; - } - else - { - corr_value++; - } - } - } if (abs(corr_value) == Galileo_E1_C_SECONDARY_CODE_LENGTH) - { - return true; - }else - { - return false; - } + { + return true; + } + else + { + return false; + } } + bool galileo_e1_dll_pll_veml_tracking_cc::cn0_and_tracking_lock_status() { // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; - d_cn0_estimation_counter++; - return true; - } - else - { - d_cn0_estimation_counter = 0; - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < 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 > MAXIMUM_LOCK_FAIL_COUNTER) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock - d_carrier_lock_fail_counter = 0; - return false; - }else{ + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; + d_cn0_estimation_counter++; return true; } - } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < 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 > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + return false; + } + else + { + return true; + } + } } + + // correlation requires: // - updated remnant carrier phase in radians (rem_carr_phase_rad) // - updated remnant code phase in samples (d_rem_code_phase_samples) @@ -504,29 +514,32 @@ void galileo_e1_dll_pll_veml_tracking_cc::do_correlation_step(const gr_complex* // DATA CORRELATOR (if tracking tracks the pilot signal) if (d_track_pilot) - { - correlator_data_cpu.set_input_output_vectors(d_Prompt_Data,input_samples); - correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( - d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_correlation_length_samples); - } + { + correlator_data_cpu.set_input_output_vectors(d_Prompt_Data,input_samples); + correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_correlation_length_samples); + } } + void galileo_e1_dll_pll_veml_tracking_cc::run_dll_pll(bool disable_costas_loop) { // ################## PLL ########################################################## // PLL discriminator - if (disable_costas_loop==true) - { - //Secondary code acquired. No symbols transition should be present in the signal - d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; - }else{ - // Costas loop discriminator, insensitive to 180 deg phase transitions - d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; - } + if (disable_costas_loop == true) + { + // Secondary code acquired. No symbols transition should be present in the signal + d_carr_error_hz = pll_four_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + } + else + { + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / GALILEO_TWO_PI; + } // Carrier discriminator filter d_carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(d_carr_error_hz); @@ -537,12 +550,12 @@ void galileo_e1_dll_pll_veml_tracking_cc::run_dll_pll(bool disable_costas_loop) // ################## DLL ########################################################## // DLL discriminator - d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); //[chips/Ti] + d_code_error_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] // Code discriminator filter - d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); //[chips/second] - + d_code_error_filt_chips = d_code_loop_filter.get_code_nco(d_code_error_chips); // [chips/second] } + void galileo_e1_dll_pll_veml_tracking_cc::clear_tracking_vars() { *d_Very_Early = gr_complex(0,0); @@ -550,13 +563,14 @@ void galileo_e1_dll_pll_veml_tracking_cc::clear_tracking_vars() *d_Prompt = gr_complex(0,0); *d_Late = gr_complex(0,0); *d_Very_Late= gr_complex(0,0); - d_carr_error_hz =0.0; - d_carr_error_filt_hz =0.0; - d_code_error_chips =0.0; - d_code_error_filt_chips =0.0; - d_current_symbol=0; + d_carr_error_hz = 0.0; + d_carr_error_filt_hz = 0.0; + d_code_error_chips = 0.0; + d_code_error_filt_chips = 0.0; + d_current_symbol = 0; } + void galileo_e1_dll_pll_veml_tracking_cc::log_data() { if(d_dump) @@ -598,12 +612,12 @@ void galileo_e1_dll_pll_veml_tracking_cc::log_data() d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_code_freq_chips; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - //PLL commands + // PLL commands tmp_float = d_carr_error_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_carr_error_filt_hz; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - //DLL commands + // DLL commands tmp_float = d_code_error_chips; d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); tmp_float = d_code_error_filt_chips; @@ -628,10 +642,11 @@ void galileo_e1_dll_pll_veml_tracking_cc::log_data() } } } -int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ + +int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ // Block input data and block output stream pointers const gr_complex* in = reinterpret_cast(input_items[0]); Gnss_Synchro **out = reinterpret_cast(&output_items[0]); @@ -640,12 +655,12 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri switch(d_state) { - case 0: //standby - bypass + case 0: // standby - bypass { current_synchro_data.Tracking_sample_counter = d_sample_counter; break; } - case 1: // pull-in + case 1: // pull-in { /* * Signal alignment (skip samples until the incoming signal is aligned with local replica) @@ -661,12 +676,12 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri current_synchro_data.Tracking_sample_counter = d_sample_counter; current_synchro_data.fs = d_fs_in; *out[0] = current_synchro_data; - d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples - consume_each(samples_offset); //shift input to perform alignment with local replica - d_state=2; //next state is the symbol synchronization + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples + consume_each(samples_offset); // shift input to perform alignment with local replica + d_state = 2; // next state is the symbol synchronization return 0; } - case 2: // wide tracking and symbol synchronization + case 2: // wide tracking and symbol synchronization { // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; @@ -677,154 +692,157 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri // perform a correlation step do_correlation_step(in); // save single correlation step variables - d_VE_accu=*d_Very_Early; - d_E_accu=*d_Early; - d_P_accu=*d_Prompt; - d_L_accu=*d_Late; - d_VL_accu=*d_Very_Late; + d_VE_accu = *d_Very_Early; + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + d_VL_accu = *d_Very_Late; //check lock status - if (cn0_and_tracking_lock_status()==false) - { - clear_tracking_vars(); - d_state=0; //loss-of-lock detected - }else{ - - //perform DLL/PLL tracking loop computations - run_dll_pll(false); - - // ################## PLL COMMANDS ################################################# - //carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - //remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - - // ################## DLL COMMANDS ################################################# - - //Code error from DLL - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - - // ########### Output the tracking results to Telemetry block ########## - if (d_track_pilot) + if (cn0_and_tracking_lock_status() == false) { - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - }else{ - current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected } - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - //compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - - //enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - - //std::cout<<(d_Prompt->real()>0); - if (d_enable_extended_integration) + else { - // ####### SECONDARY CODE LOCK ##### - d_Prompt_buffer_deque.push_back(*d_Prompt); - if (d_Prompt_buffer_deque.size()==Galileo_E1_C_SECONDARY_CODE_LENGTH) - { - if (acquire_secondary()==true) + // perform DLL/PLL tracking loop computations + run_dll_pll(false); + + // ################## PLL COMMANDS ################################################# + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + + // ################## DLL COMMANDS ################################################# + // Code error from DLL + double code_error_filt_secs; + code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; // [seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete samples + + // ########### Output the tracking results to Telemetry block ########## + if (d_track_pilot) { - d_extend_correlation_symbols_count=0; - //reset extended correlator - d_VE_accu=gr_complex(0,0); - d_E_accu=gr_complex(0,0); - d_P_accu=gr_complex(0,0); - d_L_accu=gr_complex(0,0); - d_VL_accu=gr_complex(0,0); - d_Prompt_buffer_deque.clear(); - d_current_symbol=0; - d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); - d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); - - // Set TAPs delay values [chips] - d_local_code_shift_chips[0] = - d_very_early_late_spc_narrow_chips; - d_local_code_shift_chips[1] = - d_early_late_spc_narrow_chips; - d_local_code_shift_chips[2] = 0.0; - d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; - d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; - - - LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); - std::cout<< "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " - << d_channel - << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)<(d_extend_correlation_symbols) * Galileo_E1_CODE_PERIOD; - d_carrier_loop_filter.set_pdi(new_correlation_time_s); - d_code_loop_filter.set_pdi(new_correlation_time_s); - - d_state=3; // next state is the extended correlator integrator + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); } + else + { + current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); + } + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - d_Prompt_buffer_deque.pop_front(); - } + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + + //std::cout<<(d_Prompt->real()>0); + if (d_enable_extended_integration) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_buffer_deque.push_back(*d_Prompt); + if (d_Prompt_buffer_deque.size() == Galileo_E1_C_SECONDARY_CODE_LENGTH) + { + if (acquire_secondary() == true) + { + d_extend_correlation_symbols_count = 0; + //reset extended correlator + d_VE_accu = gr_complex(0,0); + d_E_accu = gr_complex(0,0); + d_P_accu = gr_complex(0,0); + d_L_accu = gr_complex(0,0); + d_VL_accu = gr_complex(0,0); + d_Prompt_buffer_deque.clear(); + d_current_symbol = 0; + d_code_loop_filter.set_DLL_BW(d_dll_bw_narrow_hz); + d_carrier_loop_filter.set_PLL_BW(d_pll_bw_narrow_hz); + + // Set TAPs delay values [chips] + d_local_code_shift_chips[0] = - d_very_early_late_spc_narrow_chips; + d_local_code_shift_chips[1] = - d_early_late_spc_narrow_chips; + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_early_late_spc_narrow_chips; + d_local_code_shift_chips[4] = d_very_early_late_spc_narrow_chips; + + LOG(INFO) << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << d_extend_correlation_symbols << " [symbols] extended correlator for CH " + << d_channel + << " : Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + //std::cout << " pll_bw = " << d_pll_bw_hz << " [Hz], pll_narrow_bw = " << d_pll_bw_narrow_hz << " [Hz]" << std::endl; + //std::cout << " dll_bw = " << d_dll_bw_hz << " [Hz], dll_narrow_bw = " << d_dll_bw_narrow_hz << " [Hz]" << std::endl; + + // UPDATE INTEGRATION TIME + double new_correlation_time_s = static_cast(d_extend_correlation_symbols) * Galileo_E1_CODE_PERIOD; + d_carrier_loop_filter.set_pdi(new_correlation_time_s); + d_code_loop_filter.set_pdi(new_correlation_time_s); + + d_state = 3; // next state is the extended correlator integrator + } + + d_Prompt_buffer_deque.pop_front(); + } + } } - } break; } - case 3: // coherent integration (correlation time extension) + case 3: // coherent integration (correlation time extension) { // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; - //Current NCO and code generator parameters + // Current NCO and code generator parameters d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; // perform a correlation step do_correlation_step(in); - //correct the integration sign using the current symbol of the secondary code + // correct the integration sign using the current symbol of the secondary code if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu+=*d_Very_Early; - d_E_accu+=*d_Early; - d_P_accu+=*d_Prompt; - d_L_accu+=*d_Late; - d_VL_accu+=*d_Very_Late; - }else{ - d_VE_accu-=*d_Very_Early; - d_E_accu-=*d_Early; - d_P_accu-=*d_Prompt; - d_L_accu-=*d_Late; - d_VL_accu-=*d_Very_Late; - } + { + d_VE_accu += *d_Very_Early; + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + d_VL_accu += *d_Very_Late; + } + else + { + d_VE_accu -= *d_Very_Early; + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + d_VL_accu -= *d_Very_Late; + } d_current_symbol++; - //secondary code roll-up - d_current_symbol=d_current_symbol%Galileo_E1_C_SECONDARY_CODE_LENGTH; + // secondary code roll-up + d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; // PLL/DLL not enabled, we are in the middle of a coherent integration // keep alignment parameters for the next input buffer // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation // ################## PLL ########################################################## - //carrier phase accumulator for (K) Doppler estimation- + // carrier phase accumulator for (K) Doppler estimation- d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - //remnant carrier phase to prevent overflow in the code NCO + // remnant carrier phase to prevent overflow in the code NCO d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); @@ -842,7 +860,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); current_synchro_data.Tracking_sample_counter = d_sample_counter; current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - //compute remnant code phase samples AFTER the Tracking timestamp + // compute remnant code phase samples AFTER the Tracking timestamp d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; @@ -851,112 +869,117 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; d_extend_correlation_symbols_count++; - if (d_extend_correlation_symbols_count>=(d_extend_correlation_symbols-1)) - { - d_extend_correlation_symbols_count=0; - d_state=4; - } + if (d_extend_correlation_symbols_count >= (d_extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } break; } - case 4: // narrow tracking + case 4: // narrow tracking { // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; // perform a correlation step do_correlation_step(in); - //correct the integration using the current symbol + // correct the integration using the current symbol if (Galileo_E1_C_SECONDARY_CODE.at(d_current_symbol) == '0') - { - d_VE_accu+=*d_Very_Early; - d_E_accu+=*d_Early; - d_P_accu+=*d_Prompt; - d_L_accu+=*d_Late; - d_VL_accu+=*d_Very_Late; - }else{ - d_VE_accu-=*d_Very_Early; - d_E_accu-=*d_Early; - d_P_accu-=*d_Prompt; - d_L_accu-=*d_Late; - d_VL_accu-=*d_Very_Late; - } + { + d_VE_accu += *d_Very_Early; + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + d_VL_accu += *d_Very_Late; + } + else + { + d_VE_accu -= *d_Very_Early; + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + d_VL_accu -= *d_Very_Late; + } d_current_symbol++; - //secondary code roll-up - d_current_symbol=d_current_symbol%Galileo_E1_C_SECONDARY_CODE_LENGTH; + // secondary code roll-up + d_current_symbol = d_current_symbol % Galileo_E1_C_SECONDARY_CODE_LENGTH; - //check lock status - if (cn0_and_tracking_lock_status()==false) - { - clear_tracking_vars(); - d_state=0; //loss-of-lock detected - }else{ - run_dll_pll(true);//Costas loop disabled, use four quadrant atan + // check lock status + if (cn0_and_tracking_lock_status() == false) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + run_dll_pll(true); // Costas loop disabled, use four quadrant atan - // ################## PLL ########################################################## - //carrier phase accumulator for (K) Doppler estimation- - d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - //remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); - d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); + // ################## PLL ########################################################## + // carrier phase accumulator for (K) Doppler estimation- + d_acc_carrier_phase_rad -= GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GALILEO_TWO_PI * d_carrier_doppler_hz * static_cast(d_current_prn_length_samples) / static_cast(d_fs_in); + d_rem_carr_phase_rad = std::fmod(d_rem_carr_phase_rad, GALILEO_TWO_PI); - // ################## DLL ########################################################## + // ################## DLL ########################################################## + // Code phase accumulator + double code_error_filt_secs; + code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - //Code phase accumulator - double code_error_filt_secs; - code_error_filt_secs = (Galileo_E1_CODE_PERIOD * d_code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_chip_seconds = 1.0 / d_code_freq_chips; + double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples - - // ########### Output the tracking results to Telemetry block ########## - current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); - current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); - current_synchro_data.Tracking_sample_counter = d_sample_counter; - current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; - //compute remnant code phase samples AFTER the Tracking timestamp - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; - current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; - //enable write dump file this cycle (valid DLL/PLL cycle) - log_data(); - //reset extended correlator - d_VE_accu=gr_complex(0,0); - d_E_accu=gr_complex(0,0); - d_P_accu=gr_complex(0,0); - d_L_accu=gr_complex(0,0); - d_VL_accu=gr_complex(0,0); - d_state=3; //new coherent integration (correlation time extension) cycle - } + // ########### Output the tracking results to Telemetry block ########## + current_synchro_data.Prompt_I = static_cast((*d_Prompt_Data).real()); + current_synchro_data.Prompt_Q = static_cast((*d_Prompt_Data).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + // compute remnant code phase samples AFTER the Tracking timestamp + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // reset extended correlator + d_VE_accu = gr_complex(0,0); + d_E_accu = gr_complex(0,0); + d_P_accu = gr_complex(0,0); + d_L_accu = gr_complex(0,0); + d_VL_accu = gr_complex(0,0); + d_state = 3; //new coherent integration (correlation time extension) cycle + } } } //assign the GNURadio block output data -// current_synchro_data.System = {'E'}; -// std::string str_aux = "1B"; -// const char * str = str_aux.c_str(); // get a C style null terminated string -// std::memcpy(static_cast(current_synchro_data.Signal), str, 3); + // current_synchro_data.System = {'E'}; + // std::string str_aux = "1B"; + // const char * str = str_aux.c_str(); // get a C style null terminated string + // std::memcpy(static_cast(current_synchro_data.Signal), str, 3); current_synchro_data.fs = d_fs_in; *out[0] = current_synchro_data; consume_each(d_current_prn_length_samples); // this is required for gr_block derivates - d_sample_counter += d_current_prn_length_samples; //count for the processed samples + d_sample_counter += d_current_prn_length_samples; // count for the processed samples if (current_synchro_data.Flag_valid_symbol_output) - { - return 1; - }else{ - return 0; - } + { + return 1; + } + else + { + return 0; + } } @@ -982,15 +1005,15 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() // count number of epochs and rewind long int num_epoch = 0; if (dump_file.is_open()) - { - size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); - dump_file.seekg(0, std::ios::beg); - } + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } else - { - return 1; - } + { + return 1; + } float * abs_VE = new float [num_epoch]; float * abs_E = new float [num_epoch]; float * abs_P = new float [num_epoch]; @@ -1209,8 +1232,6 @@ void galileo_e1_dll_pll_veml_tracking_cc::set_channel(unsigned int channel) } - - void galileo_e1_dll_pll_veml_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { d_acquisition_gnss_synchro = p_gnss_synchro; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc index 100f8c62f..ba174dbea 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l5i_dll_pll_tracking_cc.cc @@ -730,11 +730,13 @@ int gps_l5i_dll_pll_tracking_cc::general_work (int noutput_items __attribute__(( consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_current_prn_length_samples; // count for the processed samples if (d_enable_tracking) - { - return 1; - }else{ - return 0; - } + { + return 1; + } + else + { + return 0; + } } From 1b5a3b6fa4ad16704f7eb1542af746667515547f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 17 Jan 2018 17:55:46 +0100 Subject: [PATCH 35/55] Apply coding style --- .../galileo_e1_pcps_ambiguous_acquisition.cc | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc index b0f64a25b..5ac10bcd8 100644 --- a/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc +++ b/src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc @@ -70,7 +70,7 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions - acquire_pilot_= configuration_->property(role + ".acquire_pilot", false); //will be true in future versions + acquire_pilot_ = configuration_->property(role + ".acquire_pilot", false); //will be true in future versions max_dwells_ = configuration_->property(role + ".max_dwells", 1); @@ -96,7 +96,6 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition( bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, dump_filename_); DLOG(INFO) << "acquisition(" << acquisition_sc_->unique_id() << ")"; - } else { @@ -253,17 +252,18 @@ void GalileoE1PcpsAmbiguousAcquisition::set_local_code() std::complex * code = new std::complex[code_length_]; - if (acquire_pilot_==true) - { - //set local signal generator to Galileo E1 pilot component (1C) - char pilot_signal[3]="1C"; - galileo_e1_code_gen_complex_sampled(code, pilot_signal, - cboc, gnss_synchro_->PRN, fs_in_, 0, false); - }else - { - galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, - cboc, gnss_synchro_->PRN, fs_in_, 0, false); - } + if (acquire_pilot_ == true) + { + //set local signal generator to Galileo E1 pilot component (1C) + char pilot_signal[3] = "1C"; + galileo_e1_code_gen_complex_sampled(code, pilot_signal, + cboc, gnss_synchro_->PRN, fs_in_, 0, false); + } + else + { + galileo_e1_code_gen_complex_sampled(code, gnss_synchro_->Signal, + cboc, gnss_synchro_->PRN, fs_in_, 0, false); + } for (unsigned int i = 0; i < sampled_ms_ / 4; i++) From 7e97d00a4f91dba3a28df2415a4e1ebc60be4a0c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 17 Jan 2018 19:02:52 +0100 Subject: [PATCH 36/55] Apply project's coding style --- .../gnuradio_blocks/hybrid_observables_cc.cc | 676 +++++++++--------- 1 file changed, 339 insertions(+), 337 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index f9fb4a936..40b18599d 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -65,49 +65,49 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, T_rx_s = 0.0; T_rx_step_s = 1e-3; // todo: move to gnss-sdr config for (unsigned int i = 0; i < d_nchannels; i++) - { - d_gnss_synchro_history_queue.push_back(std::deque()); - } + { + d_gnss_synchro_history_queue.push_back(std::deque()); + } // ############# 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); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - } + 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); + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure & e) + { + LOG(WARNING) << "Exception opening observables dump file " << e.what(); + } + } } - } } hybrid_observables_cc::~hybrid_observables_cc() { if (d_dump_file.is_open() == true) - { - try { - d_dump_file.close(); + try + { + d_dump_file.close(); + } + catch(const std::exception & ex) + { + LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); + } } - catch(const std::exception & ex) - { - LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); - } - } if(d_dump == true) - { - std::cout << "Writing observables .mat files ..."; - hybrid_observables_cc::save_matfile(); - std::cout << " done." << std::endl; - } + { + std::cout << "Writing observables .mat files ..."; + hybrid_observables_cc::save_matfile(); + std::cout << " done." << std::endl; + } } @@ -121,25 +121,25 @@ int hybrid_observables_cc::save_matfile() dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try { - dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); } catch(const std::ifstream::failure &e) { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; } // count number of epochs and rewind long int num_epoch = 0; if (dump_file.is_open()) - { - size = dump_file.tellg(); - num_epoch = static_cast(size) / static_cast(epoch_size_bytes); - dump_file.seekg(0, std::ios::beg); - } + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } else - { - return 1; - } + { + return 1; + } double ** RX_time = new double * [d_nchannels]; double ** TOW_at_current_symbol_s = new double * [d_nchannels]; double ** Carrier_Doppler_hz = new double * [d_nchannels]; @@ -149,58 +149,58 @@ int hybrid_observables_cc::save_matfile() double ** Flag_valid_pseudorange = new double * [d_nchannels]; for(unsigned int i = 0; i < d_nchannels; i++) - { - RX_time[i] = new double [num_epoch]; - TOW_at_current_symbol_s[i] = new double[num_epoch]; - Carrier_Doppler_hz[i] = new double[num_epoch]; - Carrier_phase_cycles[i] = new double[num_epoch]; - Pseudorange_m[i] = new double[num_epoch]; - PRN[i] = new double[num_epoch]; - Flag_valid_pseudorange[i] = new double[num_epoch]; - } + { + RX_time[i] = new double [num_epoch]; + TOW_at_current_symbol_s[i] = new double[num_epoch]; + Carrier_Doppler_hz[i] = new double[num_epoch]; + Carrier_phase_cycles[i] = new double[num_epoch]; + Pseudorange_m[i] = new double[num_epoch]; + PRN[i] = new double[num_epoch]; + Flag_valid_pseudorange[i] = new double[num_epoch]; + } try { - if (dump_file.is_open()) - { - for(long int i = 0; i < num_epoch; i++) - { - for(unsigned int chan = 0; chan < d_nchannels; chan++) + if (dump_file.is_open()) { - dump_file.read(reinterpret_cast(&RX_time[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&TOW_at_current_symbol_s[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Carrier_Doppler_hz[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Carrier_phase_cycles[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Pseudorange_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&PRN[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&Flag_valid_pseudorange[chan][i]), sizeof(double)); + for(long int i = 0; i < num_epoch; i++) + { + for(unsigned int chan = 0; chan < d_nchannels; chan++) + { + dump_file.read(reinterpret_cast(&RX_time[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_s[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Carrier_Doppler_hz[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Carrier_phase_cycles[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Pseudorange_m[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&PRN[chan][i]), sizeof(double)); + dump_file.read(reinterpret_cast(&Flag_valid_pseudorange[chan][i]), sizeof(double)); + } + } } - } - } - dump_file.close(); + dump_file.close(); } catch (const std::ifstream::failure &e) { - std::cerr << "Problem reading dump file:" << e.what() << std::endl; - for(unsigned int i = 0; i < d_nchannels; i++) - { - delete[] RX_time[i]; - delete[] TOW_at_current_symbol_s[i]; - delete[] Carrier_Doppler_hz[i]; - delete[] Carrier_phase_cycles[i]; - delete[] Pseudorange_m[i]; - delete[] PRN[i]; - delete[] Flag_valid_pseudorange[i]; - } - delete[] RX_time; - delete[] TOW_at_current_symbol_s; - delete[] Carrier_Doppler_hz; - delete[] Carrier_phase_cycles; - delete[] Pseudorange_m; - delete[] PRN; - delete[] Flag_valid_pseudorange; + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + for(unsigned int i = 0; i < d_nchannels; i++) + { + delete[] RX_time[i]; + delete[] TOW_at_current_symbol_s[i]; + delete[] Carrier_Doppler_hz[i]; + delete[] Carrier_phase_cycles[i]; + delete[] Pseudorange_m[i]; + delete[] PRN[i]; + delete[] Flag_valid_pseudorange[i]; + } + delete[] RX_time; + delete[] TOW_at_current_symbol_s; + delete[] Carrier_Doppler_hz; + delete[] Carrier_phase_cycles; + delete[] Pseudorange_m; + delete[] PRN; + delete[] Flag_valid_pseudorange; - return 1; + return 1; } double * RX_time_aux = new double [d_nchannels * num_epoch]; @@ -212,19 +212,19 @@ int hybrid_observables_cc::save_matfile() double * Flag_valid_pseudorange_aux = new double[d_nchannels * num_epoch]; unsigned int k = 0; for(long int j = 0; j < num_epoch; j++ ) - { - for(unsigned int i = 0; i < d_nchannels; i++ ) { - RX_time_aux[k] = RX_time[i][j]; - TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j]; - Carrier_Doppler_hz_aux[k] = Carrier_Doppler_hz[i][j]; - Carrier_phase_cycles_aux[k] = Carrier_phase_cycles[i][j]; - Pseudorange_m_aux[k] = Pseudorange_m[i][j]; - PRN_aux[k] = PRN[i][j]; - Flag_valid_pseudorange_aux[k] = Flag_valid_pseudorange[i][j]; - k++; + for(unsigned int i = 0; i < d_nchannels; i++ ) + { + RX_time_aux[k] = RX_time[i][j]; + TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j]; + Carrier_Doppler_hz_aux[k] = Carrier_Doppler_hz[i][j]; + Carrier_phase_cycles_aux[k] = Carrier_phase_cycles[i][j]; + Pseudorange_m_aux[k] = Pseudorange_m[i][j]; + PRN_aux[k] = PRN[i][j]; + Flag_valid_pseudorange_aux[k] = Flag_valid_pseudorange[i][j]; + k++; + } } - } // WRITE MAT FILE mat_t *matfp; @@ -234,49 +234,49 @@ int hybrid_observables_cc::save_matfile() filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if(reinterpret_cast(matfp) != NULL) - { - size_t dims[2] = {static_cast(d_nchannels), static_cast(num_epoch)}; - matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + { + size_t dims[2] = {static_cast(d_nchannels), static_cast(num_epoch)}; + matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("TOW_at_current_symbol_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, TOW_at_current_symbol_s_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("TOW_at_current_symbol_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, TOW_at_current_symbol_s_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Carrier_Doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_Doppler_hz_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Carrier_Doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_Doppler_hz_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Carrier_phase_cycles", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_phase_cycles_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Carrier_phase_cycles", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Carrier_phase_cycles_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Pseudorange_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Pseudorange_m_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Pseudorange_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Pseudorange_m_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, PRN_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("PRN", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, PRN_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Flag_valid_pseudorange", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Flag_valid_pseudorange_aux, MAT_F_DONT_COPY_DATA); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - } + matvar = Mat_VarCreate("Flag_valid_pseudorange", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, Flag_valid_pseudorange_aux, MAT_F_DONT_COPY_DATA); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } Mat_Close(matfp); for(unsigned int i = 0; i < d_nchannels; i++) - { - delete[] RX_time[i]; - delete[] TOW_at_current_symbol_s[i]; - delete[] Carrier_Doppler_hz[i]; - delete[] Carrier_phase_cycles[i]; - delete[] Pseudorange_m[i]; - delete[] PRN[i]; - delete[] Flag_valid_pseudorange[i]; + { + delete[] RX_time[i]; + delete[] TOW_at_current_symbol_s[i]; + delete[] Carrier_Doppler_hz[i]; + delete[] Carrier_phase_cycles[i]; + delete[] Pseudorange_m[i]; + delete[] PRN[i]; + delete[] Flag_valid_pseudorange[i]; - } + } delete[] RX_time; delete[] TOW_at_current_symbol_s; delete[] Carrier_Doppler_hz; @@ -325,30 +325,32 @@ bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) return (a.TOW_at_current_symbol_s) < (b); } + void hybrid_observables_cc::forecast (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) { - bool zero_samples=true; + bool zero_samples = true; for(unsigned int i = 0; i < d_nchannels; i++) - { - int items=detail()->input(i)->items_available(); - if (items>0) zero_samples=false; - ninput_items_required[i] = items; //set the required available samples in each call - } - - if (zero_samples==true) - { - for(unsigned int i = 0; i < d_nchannels; i++) { - ninput_items_required[i] = 1; //set the required available samples in each call + int items=detail()->input(i)->items_available(); + if (items>0) zero_samples = false; + ninput_items_required[i] = items; // set the required available samples in each call + } + + if (zero_samples == true) + { + for(unsigned int i = 0; i < d_nchannels; i++) + { + ninput_items_required[i] = 1; // set the required available samples in each call + } } - } } + int hybrid_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) + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) { const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer Gnss_Synchro **out = reinterpret_cast(&output_items[0]); // Get the output buffer pointer @@ -359,231 +361,231 @@ int hybrid_observables_cc::general_work (int noutput_items , Gnss_Synchro current_gnss_synchro[d_nchannels]; Gnss_Synchro aux = Gnss_Synchro(); for(unsigned int i = 0; i < d_nchannels; i++) - { - current_gnss_synchro[i] = aux; - } + { + current_gnss_synchro[i] = aux; + } /* * 1. Read the GNSS SYNCHRO objects from available channels. * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel * Record all synchronization data into queues */ for (unsigned int i = 0; i < d_nchannels; i++) - { - n_consume[i] = ninput_items[i];// full throttle - for (int j = 0; j < n_consume[i]; j++) { - d_gnss_synchro_history_queue[i].push_back(in[i][j]); + n_consume[i] = ninput_items[i]; // full throttle + for (int j = 0; j < n_consume[i]; j++) + { + d_gnss_synchro_history_queue[i].push_back(in[i][j]); + } } - } bool channel_history_ok; do - { - channel_history_ok = true; - for (unsigned int i = 0; i < d_nchannels; i++) { - if (d_gnss_synchro_history_queue[i].size() < history_deep) - { - channel_history_ok = false; - } - } - if (channel_history_ok == true) - { - std::map::const_iterator gnss_synchro_map_iter; - std::deque::const_iterator gnss_synchro_deque_iter; - - // 1. If the RX time is not set, set the Rx time - if (T_rx_s == 0) - { - // 0. Read a gnss_synchro snapshot from the queue and store it in a map - std::map gnss_synchro_map; - for (unsigned int i = 0; i < d_nchannels; i++) - { - gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].front().Channel_ID, - d_gnss_synchro_history_queue[i].front())); - } - gnss_synchro_map_iter = min_element(gnss_synchro_map.cbegin(), - gnss_synchro_map.cend(), - Hybrid_pairCompare_gnss_synchro_sample_counter); - T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / static_cast(gnss_synchro_map_iter->second.fs); - T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms - T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate - } - - // 2. Realign RX time in all valid channels - std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx - std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate - // shift channels history to match the reference TOW + channel_history_ok = true; for (unsigned int i = 0; i < d_nchannels; i++) - { - gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), - d_gnss_synchro_history_queue[i].cend(), - T_rx_s, - Hybrid_valueCompare_gnss_synchro_receiver_time); - if (gnss_synchro_deque_iter != d_gnss_synchro_history_queue[i].cend()) { - if (gnss_synchro_deque_iter->Flag_valid_word == true) - { - double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s = T_rx_channel - T_rx_s; - - // check that T_rx difference is less than a threshold (the correlation interval) - if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) + if (d_gnss_synchro_history_queue[i].size() < history_deep) { - // record the word structure in a map for pseudorange computation - // save the previous observable - int distance = std::distance(d_gnss_synchro_history_queue[i].cbegin(), gnss_synchro_deque_iter); - if (distance > 0) - { - if (d_gnss_synchro_history_queue[i].at(distance - 1).Flag_valid_word) + channel_history_ok = false; + } + } + if (channel_history_ok == true) + { + std::map::const_iterator gnss_synchro_map_iter; + std::deque::const_iterator gnss_synchro_deque_iter; + + // 1. If the RX time is not set, set the Rx time + if (T_rx_s == 0) + { + // 0. Read a gnss_synchro snapshot from the queue and store it in a map + std::map gnss_synchro_map; + for (unsigned int i = 0; i < d_nchannels; i++) { - double T_rx_channel_prev = static_cast(d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); - double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; - if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) + gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].front().Channel_ID, + d_gnss_synchro_history_queue[i].front())); + } + gnss_synchro_map_iter = min_element(gnss_synchro_map.cbegin(), + gnss_synchro_map.cend(), + Hybrid_pairCompare_gnss_synchro_sample_counter); + T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / static_cast(gnss_synchro_map_iter->second.fs); + T_rx_s = floor(T_rx_s * 1000.0) / 1000.0; // truncate to ms + T_rx_s += past_history_s; // increase T_rx to have a minimum past history to interpolate + } + + // 2. Realign RX time in all valid channels + std::map realigned_gnss_synchro_map; // container for the aligned set of observables for the selected T_rx + std::map adjacent_gnss_synchro_map; // container for the previous observable values to interpolate + // shift channels history to match the reference TOW + for (unsigned int i = 0; i < d_nchannels; i++) + { + gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].cbegin(), + d_gnss_synchro_history_queue[i].cend(), + T_rx_s, + Hybrid_valueCompare_gnss_synchro_receiver_time); + if (gnss_synchro_deque_iter != d_gnss_synchro_history_queue[i].cend()) + { + if (gnss_synchro_deque_iter->Flag_valid_word == true) + { + double T_rx_channel = static_cast(gnss_synchro_deque_iter->Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); + double delta_T_rx_s = T_rx_channel - T_rx_s; + + // check that T_rx difference is less than a threshold (the correlation interval) + if (delta_T_rx_s * 1000.0 < static_cast(gnss_synchro_deque_iter->correlation_length_ms)) + { + // record the word structure in a map for pseudorange computation + // save the previous observable + int distance = std::distance(d_gnss_synchro_history_queue[i].cbegin(), gnss_synchro_deque_iter); + if (distance > 0) + { + if (d_gnss_synchro_history_queue[i].at(distance - 1).Flag_valid_word) + { + double T_rx_channel_prev = static_cast(d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter) / static_cast(gnss_synchro_deque_iter->fs); + double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; + if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) + { + realigned_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance - 1))); + adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + } + else + { + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + adjacent_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance - 1))); + } + } + } + else + { + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + } + + } + } + } + } + + if(!realigned_gnss_synchro_map.empty()) + { + /* + * 2.1 Use CURRENT set of measurements and find the nearest satellite + * common RX time algorithm + */ + // what is the most recent symbol TOW in the current set? -> this will be the reference symbol + gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.cbegin(), + realigned_gnss_synchro_map.cend(), + Hybrid_pairCompare_gnss_synchro_d_TOW); + double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); + + // compute interpolated TOW value at T_rx_s + int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; + Gnss_Synchro adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); + double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; + + double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; + + double selected_T_rx_s = T_rx_s; + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + + (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); + + // Now compute RX time differences due to the PRN alignment in the correlators + double traveltime_ms; + double pseudorange_m; + double channel_T_rx_s; + double channel_fs_hz; + double channel_TOW_s; + for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) + { + channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); + channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; + // compute interpolated observation values + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + // TOW at the selected receiver time T_rx_s + int element_key = gnss_synchro_map_iter->second.Channel_ID; + adj_obs = adjacent_gnss_synchro_map.at(element_key); + + double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; + + double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); + + // Doppler and Accumulated carrier phase + double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); + double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); + + // compute the pseudorange (no rx time offset correction) + traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; + // convert to meters + pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] + // update the pseudorange object + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; + // Save the estimated RX time (no RX clock offset correction yet!) + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; + + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; + } + + if(d_dump == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try { - realigned_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); - adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + double tmp_double; + for (unsigned int i = 0; i < d_nchannels; i++) + { + tmp_double = current_gnss_synchro[i].RX_time; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].Pseudorange_m; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = current_gnss_synchro[i].PRN; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + } } - else + catch (const std::ifstream::failure& e) { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - adjacent_gnss_synchro_map.insert(std::pair(d_gnss_synchro_history_queue[i].at(distance - 1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance - 1))); + LOG(WARNING) << "Exception writing observables dump file " << e.what(); } } - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - } + for (unsigned int i = 0; i < d_nchannels; i++) + { + out[i][n_outputs] = current_gnss_synchro[i]; + } + + n_outputs++; } - } - } - } - if(!realigned_gnss_synchro_map.empty()) - { - /* - * 2.1 Use CURRENT set of measurements and find the nearest satellite - * common RX time algorithm - */ - // what is the most recent symbol TOW in the current set? -> this will be the reference symbol - gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.cbegin(), - realigned_gnss_synchro_map.cend(), - Hybrid_pairCompare_gnss_synchro_d_TOW); - double ref_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - - // compute interpolated TOW value at T_rx_s - int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; - Gnss_Synchro adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); - double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; - - double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - double d_ref_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / ref_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / ref_fs_hz; - - double selected_T_rx_s = T_rx_s; - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + - (selected_T_rx_s - ref_adj_T_rx_s) * (d_TOW_reference - adj_obs.TOW_at_current_symbol_s) / (d_ref_T_rx_s - ref_adj_T_rx_s); - - // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double channel_T_rx_s; - double channel_fs_hz; - double channel_TOW_s; - for(gnss_synchro_map_iter = realigned_gnss_synchro_map.cbegin(); gnss_synchro_map_iter != realigned_gnss_synchro_map.cend(); gnss_synchro_map_iter++) - { - channel_fs_hz = static_cast(gnss_synchro_map_iter->second.fs); - channel_TOW_s = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; - channel_T_rx_s = static_cast(gnss_synchro_map_iter->second.Tracking_sample_counter) / channel_fs_hz + gnss_synchro_map_iter->second.Code_phase_samples / channel_fs_hz; - // compute interpolated observation values - // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) - // TOW at the selected receiver time T_rx_s - int element_key = gnss_synchro_map_iter->second.Channel_ID; - adj_obs = adjacent_gnss_synchro_map.at(element_key); - - double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; - - double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s + (selected_T_rx_s - adj_T_rx_s) * (channel_TOW_s - adj_obs.TOW_at_current_symbol_s) / (channel_T_rx_s - adj_T_rx_s); - - // Doppler and Accumulated carrier phase - double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_phase_rads - adj_obs.Carrier_phase_rads) / (channel_T_rx_s - adj_T_rx_s); - double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz + (selected_T_rx_s - adj_T_rx_s) * (gnss_synchro_map_iter->second.Carrier_Doppler_hz - adj_obs.Carrier_Doppler_hz) / (channel_T_rx_s - adj_T_rx_s); - - // compute the pseudorange (no rx time offset correction) - traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + GPS_STARTOFFSET_ms; - // convert to meters - pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] - // update the pseudorange object - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; - // Save the estimated RX time (no RX clock offset correction yet!) - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; - - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; - current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; - } - - if(d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) + // Move RX time + T_rx_s = T_rx_s + T_rx_step_s; + // pop old elements from queue + for (unsigned int i = 0; i < d_nchannels; i++) { - tmp_double = current_gnss_synchro[i].RX_time; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].TOW_at_current_symbol_s; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_phase_rads / GPS_TWO_PI; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].Pseudorange_m; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = current_gnss_synchro[i].PRN; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(current_gnss_synchro[i].Flag_valid_pseudorange); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + while (static_cast(d_gnss_synchro_history_queue[i].front().Tracking_sample_counter) / static_cast(d_gnss_synchro_history_queue[i].front().fs) < (T_rx_s - past_history_s)) + { + d_gnss_synchro_history_queue[i].pop_front(); + } } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } } - - for (unsigned int i = 0; i < d_nchannels; i++) - { - out[i][n_outputs] = current_gnss_synchro[i]; - } - - n_outputs++; - } - - // Move RX time - T_rx_s = T_rx_s + T_rx_step_s; - // pop old elements from queue - for (unsigned int i = 0; i < d_nchannels; i++) - { - while (static_cast(d_gnss_synchro_history_queue[i].front().Tracking_sample_counter) / static_cast(d_gnss_synchro_history_queue[i].front().fs) < (T_rx_s - past_history_s)) - { - d_gnss_synchro_history_queue[i].pop_front(); - } - } - } - } while(channel_history_ok == true && noutput_items > n_outputs); + } while(channel_history_ok == true && noutput_items > n_outputs); // Multi-rate consume! for (unsigned int i = 0; i < d_nchannels; i++) - { - consume(i, n_consume[i]); // which input, how many items - } + { + consume(i, n_consume[i]); // which input, how many items + } return n_outputs; } From 59c153846c6fd10e730d8db43eddbc1859fd231e Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 17 Jan 2018 19:06:39 +0100 Subject: [PATCH 37/55] Remove blank line --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 40b18599d..0afa10b52 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -328,7 +328,6 @@ bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) void hybrid_observables_cc::forecast (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required) { - bool zero_samples = true; for(unsigned int i = 0; i < d_nchannels; i++) { From bc922404438afc6cc7a9c36012266bf8e4830589 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 17 Jan 2018 19:11:42 +0100 Subject: [PATCH 38/55] Replace C-style cast by C++ casts --- .../observables/gnuradio_blocks/hybrid_observables_cc.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 0afa10b52..efd29e092 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -310,7 +310,7 @@ bool Hybrid_valueCompare_gnss_synchro_sample_counter(const Gnss_Synchro& a, unsi bool Hybrid_valueCompare_gnss_synchro_receiver_time(const Gnss_Synchro& a, double b) { - return (((double)a.Tracking_sample_counter+a.Code_phase_samples)/(double)a.fs) < (b); + return ((static_cast(a.Tracking_sample_counter) + static_cast(a.Code_phase_samples)) / static_cast(a.fs) ) < (b); } @@ -331,8 +331,8 @@ void hybrid_observables_cc::forecast (int noutput_items __attribute__((unused)), bool zero_samples = true; for(unsigned int i = 0; i < d_nchannels; i++) { - int items=detail()->input(i)->items_available(); - if (items>0) zero_samples = false; + int items = detail()->input(i)->items_available(); + if (items > 0) zero_samples = false; ninput_items_required[i] = items; // set the required available samples in each call } From dc717db61e5e4ab8048c1d8cb57b9501bd70d6ad Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 18 Jan 2018 15:27:38 +0100 Subject: [PATCH 39/55] Minor fixes --- .../galileo_e1_dll_pll_veml_tracking_cc.cc | 406 +++++++++--------- 1 file changed, 203 insertions(+), 203 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 0fd20887b..a818da13d 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -194,7 +194,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_track_pilot = track_pilot; if (d_track_pilot) { - //extended integration control + // extended integration control if (d_extend_correlation_symbols > 1) { d_enable_extended_integration = true; @@ -203,7 +203,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( { d_enable_extended_integration = false; } - //Extra correlator for the data component + // Extra correlator for the data component d_local_code_data_shift_chips = static_cast(volk_gnsssdr_malloc(sizeof(float), volk_gnsssdr_get_alignment())); d_local_code_data_shift_chips[0] = 0.0; correlator_data_cpu.init(2 * d_correlation_length_samples, 1); @@ -251,7 +251,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_carrier_doppler_hz = 0.0; d_acc_carrier_phase_rad = 0.0; - d_state = 0;// intial state: stanby + d_state = 0; // initial state: standby } @@ -320,7 +320,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking() d_acquisition_gnss_synchro->PRN, Galileo_E1_CODE_CHIP_RATE_HZ, 0); - d_Prompt_Data[0] = gr_complex(0,0); //clean data correlator output + d_Prompt_Data[0] = gr_complex(0,0); // clean data correlator output correlator_data_cpu.set_local_code_and_taps(static_cast(Galileo_E1_B_CODE_LENGTH_CHIPS), d_data_code, d_local_code_shift_chips); @@ -574,73 +574,73 @@ void galileo_e1_dll_pll_veml_tracking_cc::clear_tracking_vars() void galileo_e1_dll_pll_veml_tracking_cc::log_data() { if(d_dump) - { - // Dump results to file - float prompt_I; - float prompt_Q; - float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; - float tmp_float; - double tmp_double; - - prompt_I = static_cast(d_P_accu.real()); - prompt_Q = static_cast(d_P_accu.imag()); - - tmp_VE = std::abs(d_VE_accu); - tmp_E = std::abs(d_E_accu); - tmp_P = std::abs(d_P_accu); - tmp_L = std::abs(d_L_accu); - tmp_VL = std::abs(d_VL_accu); - - try { - // Dump correlators output - d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); - // PROMPT I and Q (to analyze navigation symbols) - d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); - d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); - // PRN start sample stamp - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - tmp_float = d_acc_carrier_phase_rad; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // carrier and code frequency - tmp_float = d_carrier_doppler_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_freq_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // PLL commands - tmp_float = d_carr_error_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carr_error_filt_hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // DLL commands - tmp_float = d_code_error_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_code_error_filt_chips; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // CN0 and carrier lock test - tmp_float = d_CN0_SNV_dB_Hz; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_float = d_carrier_lock_test; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - // AUX vars (for debug purposes) - tmp_float = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // PRN - unsigned int prn_ = d_acquisition_gnss_synchro->PRN; - d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + // Dump results to file + float prompt_I; + float prompt_Q; + float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL; + float tmp_float; + double tmp_double; + + prompt_I = static_cast(d_P_accu.real()); + prompt_Q = static_cast(d_P_accu.imag()); + + tmp_VE = std::abs(d_VE_accu); + tmp_E = std::abs(d_E_accu); + tmp_P = std::abs(d_P_accu); + tmp_L = std::abs(d_L_accu); + tmp_VL = std::abs(d_VL_accu); + + try + { + // Dump correlators output + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // PLL commands + tmp_float = d_carr_error_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carr_error_filt_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = d_code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + unsigned int prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&prn_), sizeof(unsigned int)); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } - } } @@ -685,7 +685,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri { // Fill the acquisition data current_synchro_data = *d_acquisition_gnss_synchro; - //Current NCO and code generator parameters + // Current NCO and code generator parameters d_carrier_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); d_rem_code_phase_chips = d_rem_code_phase_samples * d_code_freq_chips / d_fs_in; @@ -697,7 +697,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri d_P_accu = *d_Prompt; d_L_accu = *d_Late; d_VL_accu = *d_Very_Late; - //check lock status + // check lock status if (cn0_and_tracking_lock_status() == false) { clear_tracking_vars(); @@ -727,7 +727,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri double T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(K_blk_samples); // round to a discrete samples + d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples // ########### Output the tracking results to Telemetry block ########## if (d_track_pilot) @@ -763,7 +763,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri if (acquire_secondary() == true) { d_extend_correlation_symbols_count = 0; - //reset extended correlator + // reset extended correlator d_VE_accu = gr_complex(0,0); d_E_accu = gr_complex(0,0); d_P_accu = gr_complex(0,0); @@ -995,12 +995,12 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); try { - dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); } catch(const std::ifstream::failure &e) { - std::cerr << "Problem opening dump file:" << e.what() << std::endl; - return 1; + std::cerr << "Problem opening dump file:" << e.what() << std::endl; + return 1; } // count number of epochs and rewind long int num_epoch = 0; @@ -1037,58 +1037,58 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() try { - if (dump_file.is_open()) - { - for(long int i = 0; i < num_epoch; i++) - { - dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); - dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); - dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); - } - } - dump_file.close(); + if (dump_file.is_open()) + { + for(long int i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&abs_VE[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(unsigned long int)); + dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&PRN[i]), sizeof(unsigned int)); + } + } + dump_file.close(); } catch (const std::ifstream::failure &e) { - std::cerr << "Problem reading dump file:" << e.what() << std::endl; - delete[] abs_VE; - delete[] abs_E; - delete[] abs_P; - delete[] abs_L; - delete[] abs_VL; - delete[] Prompt_I; - delete[] Prompt_Q; - delete[] PRN_start_sample_count; - delete[] acc_carrier_phase_rad; - delete[] carrier_doppler_hz; - delete[] code_freq_chips; - delete[] carr_error_hz; - delete[] carr_error_filt_hz; - delete[] code_error_chips; - delete[] code_error_filt_chips; - delete[] CN0_SNV_dB_Hz; - delete[] carrier_lock_test; - delete[] aux1; - delete[] aux2; - delete[] PRN; - return 1; + std::cerr << "Problem reading dump file:" << e.what() << std::endl; + delete[] abs_VE; + delete[] abs_E; + delete[] abs_P; + delete[] abs_L; + delete[] abs_VL; + delete[] Prompt_I; + delete[] Prompt_Q; + delete[] PRN_start_sample_count; + delete[] acc_carrier_phase_rad; + delete[] carrier_doppler_hz; + delete[] code_freq_chips; + delete[] carr_error_hz; + delete[] carr_error_filt_hz; + delete[] code_error_chips; + delete[] code_error_filt_chips; + delete[] CN0_SNV_dB_Hz; + delete[] carrier_lock_test; + delete[] aux1; + delete[] aux2; + delete[] PRN; + return 1; } // WRITE MAT FILE @@ -1099,88 +1099,88 @@ int galileo_e1_dll_pll_veml_tracking_cc::save_matfile() filename.append(".mat"); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); if(reinterpret_cast(matfp) != NULL) - { - size_t dims[2] = {1, static_cast(num_epoch)}; - matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + { + size_t dims[2] = {1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); + matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); - matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - } + matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } Mat_Close(matfp); delete[] abs_VE; delete[] abs_E; @@ -1212,23 +1212,23 @@ void galileo_e1_dll_pll_veml_tracking_cc::set_channel(unsigned int channel) LOG(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(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); - LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename.append(boost::lexical_cast(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); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } } - } } From 4bbd994f6079a4ae9522ff214845a243af901c69 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 18 Jan 2018 15:49:05 +0100 Subject: [PATCH 40/55] Add a note explaining how to activate the bias-tee in a RTL-SDR Blog V3 dongle, so it can feed an active antenna (see #77) The activation is done by adding SignalSource.osmosdr_args=rtl,bias=1 to the configuration file, if using gr-osmosdr >= 0.1.4-13 For older versions of gr-osmosdr, you can use https://github.com/rtlsdrblog/rtl_biast --- conf/front-end-cal.conf | 31 ++++++++++++----------- conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf | 16 ++++++++++-- 2 files changed, 30 insertions(+), 17 deletions(-) diff --git a/conf/front-end-cal.conf b/conf/front-end-cal.conf index af9221936..8b09a2065 100644 --- a/conf/front-end-cal.conf +++ b/conf/front-end-cal.conf @@ -41,17 +41,10 @@ GNSS-SDR.SUPL_LAC=861 GNSS-SDR.SUPL_CI=40184 ;######### SIGNAL_SOURCE CONFIG ############ -;#implementation: Use [File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] or [Osmosdr_Signal_Source] SignalSource.implementation=Osmosdr_Signal_Source -SignalSource.AGC_enabled=false - -;#filename: path to file with the captured GNSS signal samples to be processed -;SignalSource.filename=/datalogger/signals/RTL-SDR/cap_-90dBm_IF15_RF40_EzCap.dat -SignalSource.filename=/datalogger/signals/Agilent/New York/2msps.dat -;SignalSource.filename=/datalogger/signals/RTL-SDR/geo/pmt4_no_amp.dat -;SignalSource.filename=/datalogger/signals/RTL-SDR/geo/pmt4_no_amp_mini.dat -;SignalSource.filename=/datalogger/signals/RTL-SDR/mozoncillo/cap_mozon_ezcap.dat +;#freq: RF front-end center frequency in [Hz] +SignalSource.freq=1575420000 ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. SignalSource.item_type=gr_complex @@ -59,18 +52,26 @@ SignalSource.item_type=gr_complex ;#sampling_frequency: Original Signal sampling frequency in [Hz] SignalSource.sampling_frequency=2000000 -;#freq: RF front-end center frequency in [Hz] -SignalSource.freq=1575420000 - ;#gain: Front-end Gain in [dB] SignalSource.gain=40 SignalSource.rf_gain=40 SignalSource.if_gain=30 +SignalSource.AGC_enabled=false -;#subdevice: UHD subdevice specification (for USRP1 use A:0 or B:0) -SignalSource.subdevice=B:0 +;# Please note that the new RTL-SDR Blog V3 dongles ship a < 1 PPM +;# temperature compensated oscillator (TCXO), which is well suited for GNSS +;# signal processing, and a 4.5 V powered bias-tee to feed an active antenna. +;# Whether the bias-tee is turned off before reception depends on which version +;# of gr-osmosdr was used when compiling GNSS-SDR. With an old version +;# (for example, v0.1.4-8), the utility rtl_biast may be used to switch the +;# bias-tee, and then call gnss-sdr. +;# See https://github.com/rtlsdrblog/rtl_biast +;# After reception the bias-tee is switched off automatically by the program. +;# With newer versions of gr-osmosdr (>= 0.1.4-13), the bias-tee can be +;# activated by uncommenting the following line: +;SignalSource.osmosdr_args=rtl,bias=1 -;#samples: Number of samples to be processed. Notice that 0 indicates the entire file. +;#samples: Number of samples to be processed. Notice that 0 means infinite samples. SignalSource.samples=0 ;#repeat: Repeat the processing file. diff --git a/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf b/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf index 8c5f9022a..2c10ea684 100644 --- a/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf @@ -27,9 +27,8 @@ GNSS-SDR.SUPL_CI=0x31b0 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=Osmosdr_Signal_Source -;SignalSource.filename=/media/DATALOGGER_/signals/RTL-SDR/geo/pmt4.dat SignalSource.item_type=gr_complex -;FOR USE GNSS-SDR WITH RTLSDR DONGLES USER MUST SET THE CALIBRATED SAMPLE RATE HERE +; FOR USE GNSS-SDR WITH RTLSDR DONGLES USER MUST SET THE CALIBRATED SAMPLE RATE HERE ; i.e. using front-end-cal as reported here:http://www.cttc.es/publication/turning-a-television-into-a-gnss-receiver/ SignalSource.sampling_frequency=2000000 SignalSource.freq=1575420000 @@ -43,6 +42,19 @@ SignalSource.dump=false SignalSource.dump_filename=../data/signal_source.dat SignalSource.enable_throttle_control=false +;# Please note that the new RTL-SDR Blog V3 dongles ship a < 1 PPM +;# temperature compensated oscillator (TCXO), which is well suited for GNSS +;# signal processing, and a 4.5 V powered bias-tee to feed an active antenna. +;# Whether the bias-tee is turned off before reception depends on which version +;# of gr-osmosdr was used when compiling GNSS-SDR. With an old version +;# (for example, v0.1.4-8), the utility rtl_biast may be used to switch the +;# bias-tee, and then call gnss-sdr. +;# See https://github.com/rtlsdrblog/rtl_biast +;# After reception the bias-tee is switched off automatically by the program. +;# With newer versions of gr-osmosdr (>= 0.1.4-13), the bias-tee can be +;# activated by uncommenting the following line: +;SignalSource.osmosdr_args=rtl,bias=1 + ;######### SIGNAL_CONDITIONER CONFIG ############ SignalConditioner.implementation=Signal_Conditioner From 4873ea2b88720f4091b1f8a92bda3f7c9e5ede7f Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 18 Jan 2018 16:57:15 +0100 Subject: [PATCH 41/55] Fix GSS6450 signal source --- .../spir_gss6450_file_signal_source.cc | 141 ++++++++++-------- .../spir_gss6450_file_signal_source.h | 19 ++- .../unpack_spir_gss6450_samples.cc | 133 +++++++---------- .../unpack_spir_gss6450_samples.h | 33 ++-- 4 files changed, 153 insertions(+), 173 deletions(-) diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc index ede0abb72..590c60c33 100644 --- a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc @@ -1,12 +1,12 @@ /*! - * \file spir_file_signal_source.cc + * \file spir_gss6450_file_signal_source.cc * \brief Implementation of a class that reads signals samples from a SPIR file * and adapts it to a SignalSourceInterface. - * \author Fran Fabra, 2014 fabra(at)ice.csic.es + * \author Antonio Ramos, 2017 antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -35,16 +35,11 @@ #include #include #include -#include #include #include "configuration_interface.h" - using google::LogMessage; -DEFINE_string(spir_gss6450_signal_source, "-", - "If defined, path to the file containing the Spirent GSS6450 signal samples (overrides the configuration file)"); - SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* configuration, std::string role, unsigned int in_streams, unsigned int out_streams, gr::msg_queue::sptr queue) : @@ -55,31 +50,35 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* item_type_ = "int"; samples_ = configuration->property(role + ".samples", 0); - sampling_frequency_ = configuration->property(role + ".sampling_frequency", 0); + sampling_frequency_ = configuration->property(role + ".sampling_frequency", 0.0); filename_ = configuration->property(role + ".filename", default_filename); - - // override value with commandline flag, if present - if (FLAGS_spir_gss6450_signal_source.compare("-") != 0) filename_= FLAGS_spir_gss6450_signal_source; - repeat_ = configuration->property(role + ".repeat", false); dump_ = configuration->property(role + ".dump", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false); adc_bits_ = configuration->property(role + ".adc_bits", 4); - n_channels_ = configuration->property(role + ".RF_channels", 1); + n_channels_ = configuration->property(role + ".total_channels", 1); sel_ch_ = configuration->property(role + ".sel_ch", 1); item_size_ = sizeof(int); - long bytes_seek = 65536; + long bytes_seek = configuration->property(role + ".bytes_to_skip", 65536); double sample_size_byte = static_cast(adc_bits_) / 4.0; - int samples_per_item = 16 / adc_bits_; if(sel_ch_ > n_channels_) { LOG(WARNING) << "Invalid RF channel selection"; } + if(n_channels_ > 1) + { + for(unsigned int i = 0; i < (n_channels_ - 1); i++) + { + null_sinks_.push_back(gr::blocks::null_sink::make(item_size_)); + } + std::cout << "NUMBER OF NULL SINKS = " << null_sinks_.size() << std::endl; + } try { - file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_); - file_source_->seek(bytes_seek, SEEK_SET); - unpack_ii_ = gr::blocks::packed_to_unpacked_ii::make(adc_bits_, gr::GR_MSB_FIRST); - unpack_spir_ = make_unpack_spir_gss6450_samples(n_channels_, sel_ch_, samples_per_item, item_size_); + file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_); + file_source_->seek(bytes_seek / item_size_, SEEK_SET); + unpack_spir_ = make_unpack_spir_gss6450_samples(adc_bits_); + deint_ = gr::blocks::deinterleave::make(item_size_); + endian_ = gr::blocks::endian_swap::make(item_size_); } catch (const std::exception &e) { @@ -103,14 +102,13 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* << filename_.c_str() << ", exiting the program."; throw(e); } - DLOG(INFO) << "file_source(" << file_source_->unique_id() << ")"; if(samples_ == 0) // read all file { /*! * BUG workaround: The GNU Radio file source does not stop the receiver after reaching the End of File. - * A possible solution is to compute the file length in samples using file size, excluding the last 100 milliseconds, and enable always the + * A possible solution is to compute the file length in samples using file size, excluding the last 2 milliseconds, and enable always the * valve block */ std::ifstream file (filename_.c_str(), std::ios::in | std::ios::binary | std::ios::ate); @@ -119,7 +117,7 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* if (file.is_open()) { size = file.tellg(); - LOG(INFO) << "Total samples in the file= " << floor(static_cast(size) / static_cast(item_size())); + LOG(INFO) << "Total samples in the file= " << floor(static_cast(size) / static_cast(item_size_)); } else { @@ -133,13 +131,13 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* if(size > 0) { - samples_ = floor(static_cast(size - bytes_seek) / (sample_size_byte * static_cast(n_channels_))); - samples_ = samples_- ceil(0.002 * static_cast(sampling_frequency_)); //process all the samples available in the file excluding the last 2 ms + samples_ = static_cast(floor(static_cast(size - bytes_seek) / (sample_size_byte * static_cast(n_channels_)))); + samples_ = samples_- static_cast(ceil(0.002 * sampling_frequency_)); //process all the samples available in the file excluding the last 2 ms } } CHECK(samples_ > 0) << "File does not contain enough samples to process."; - double signal_duration_s = static_cast(samples_) * ( 1 /static_cast(sampling_frequency_)); + double signal_duration_s = static_cast(samples_) / sampling_frequency_; LOG(INFO) << "Total number 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]" << std::endl; @@ -149,9 +147,9 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* if (dump_) { sink_ = gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str()); + //sink_test = gr::blocks::file_sink::make(sizeof(int), "/home/aramos/Escritorio/test_int.dat"); DLOG(INFO) << "file_sink(" << sink_->unique_id() << ")"; } - if (enable_throttle_control_) { throttle_ = gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_); @@ -167,36 +165,46 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* } - - SpirGSS6450FileSignalSource::~SpirGSS6450FileSignalSource() {} - - void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block) { if (samples_ > 0) { + top_block->connect(file_source_, 0, deint_, 0); + /* + top_block->connect(deint_, sel_ch_ - 1, endian_ ,0); + top_block->connect(endian_, 0, unpack_spir_, 0); + */ + top_block->connect(deint_, sel_ch_ - 1, unpack_spir_, 0); + if(n_channels_ > 1) + { + unsigned int aux = 0; + for(unsigned int i = 0; i < n_channels_; i++) + { + if(i != (sel_ch_ - 1)) + { + top_block->connect(deint_, i, null_sinks_.at(aux), 0); + aux++; + } + } + } if (enable_throttle_control_) { - top_block->connect(file_source_, 0, unpack_ii_, 0); - top_block->connect(unpack_ii_, 0, unpack_spir_, 0); top_block->connect(unpack_spir_, 0, throttle_, 0); top_block->connect(throttle_, 0, valve_, 0); } else { - top_block->connect(file_source_, 0, unpack_ii_, 0); - top_block->connect(unpack_ii_, 0, unpack_spir_, 0); top_block->connect(unpack_spir_, 0, valve_, 0); } if(dump_) - { - top_block->connect(valve_, 0, sink_, 0); - DLOG(INFO) << "connected valve to file sink"; - } + { + top_block->connect(valve_, 0, sink_, 0); + //top_block->connect(deint_, sel_ch_ - 1, sink_test, 0); + } } else { @@ -205,40 +213,43 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block) } - - - - void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block) { if (samples_ > 0) + { + top_block->disconnect(file_source_, 0, deint_, 0); + top_block->disconnect(deint_, sel_ch_ - 1, unpack_spir_, 0); + if(n_channels_ > 1) { - if (enable_throttle_control_) + unsigned int aux = 0; + for(unsigned int i = 0; i < n_channels_; i++) + { + if(i != (sel_ch_ - 1)) { - top_block->disconnect(file_source_, 0, unpack_ii_, 0); - top_block->disconnect(unpack_ii_, 0, unpack_spir_, 0); - top_block->disconnect(unpack_spir_, 0, throttle_, 0); - top_block->disconnect(throttle_, 0, valve_, 0); - if (dump_) - { - top_block->disconnect(valve_, 0, sink_, 0); - } - } - else - { - top_block->disconnect(file_source_, 0, unpack_ii_, 0); - top_block->disconnect(unpack_ii_, 0, unpack_spir_, 0); - top_block->disconnect(unpack_spir_, 0, valve_, 0); - if (dump_) - { - top_block->disconnect(valve_, 0, sink_, 0); - } + top_block->disconnect(deint_, i, null_sinks_.at(aux), 0); + aux++; } + } } + if (enable_throttle_control_) + { + top_block->disconnect(unpack_spir_, 0, throttle_, 0); + top_block->disconnect(throttle_, 0, valve_, 0); + } + else + { + top_block->disconnect(unpack_spir_, 0, valve_, 0); + } + if(dump_) + { + top_block->disconnect(valve_, 0, sink_, 0); + //top_block->disconnect(deint_, sel_ch_ - 1, sink_test, 0); + } + } else - { - LOG(WARNING) << "Nothing to disconnect"; - } + { + LOG(WARNING) << "Nothing to disconnect"; + } } diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h index 346ebdc79..e8387dbd7 100644 --- a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h @@ -1,12 +1,12 @@ /*! - * \file spir_file_signal_source.h + * \file spir_gss6450_file_signal_source.h * \brief Implementation of a class that reads signals samples from a SPIR file * and adapts it to a SignalSourceInterface. - * \author Fran Fabra, 2014 fabra(at)ice.csic.es + * \author Antonio Ramos, 2017 antonio.ramos(at)cttc.es * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -33,10 +33,14 @@ #define GNSS_SDR_SPIR_GSS6450_FILE_SIGNAL_SOURCE_H_ #include +#include #include #include #include #include +#include +#include +#include #include #include #include "gnss_block_interface.h" @@ -104,10 +108,11 @@ public: private: unsigned long long samples_; - long sampling_frequency_; + double sampling_frequency_; std::string filename_; bool repeat_; bool dump_; + bool enable_throttle_control_; std::string dump_filename_; std::string role_; std::string item_type_; @@ -117,14 +122,16 @@ private: unsigned int n_channels_; unsigned int sel_ch_; gr::blocks::file_source::sptr file_source_; - gr::blocks::packed_to_unpacked_ii::sptr unpack_ii_; + gr::blocks::deinterleave::sptr deint_; + gr::blocks::endian_swap::sptr endian_; + std::vector null_sinks_; unpack_spir_gss6450_samples_sptr unpack_spir_; boost::shared_ptr valve_; gr::blocks::file_sink::sptr sink_; + gr::blocks::file_sink::sptr sink_test; gr::blocks::throttle::sptr throttle_; gr::msg_queue::sptr queue_; size_t item_size_; - bool enable_throttle_control_; }; #endif /*GNSS_SDR_SPIR_GSS6450_FILE_SIGNAL_SOURCE_H_*/ diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc index d8ae66e39..5992c7025 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc @@ -34,123 +34,90 @@ -unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size) +unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int adc_nbit) { - return unpack_spir_gss6450_samples_sptr(new unpack_spir_gss6450_samples(n_chann, sel_ch, samp_item, item_size)); + return unpack_spir_gss6450_samples_sptr(new unpack_spir_gss6450_samples(adc_nbit)); } -unpack_spir_gss6450_samples::unpack_spir_gss6450_samples( - unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size) : gr::block("unpack_spir_gss6450_samples", +unpack_spir_gss6450_samples::unpack_spir_gss6450_samples(unsigned int adc_nbit) : gr::sync_interpolator("unpack_spir_gss6450_samples", gr::io_signature::make(1, 1, sizeof(int)), - gr::io_signature::make(1, 1, sizeof(gr_complex))) + gr::io_signature::make(1, 1, sizeof(gr_complex)), 16 / adc_nbit) { - d_channels = n_chann; - d_sel_ch = sel_ch; - item_size_ = item_size; - ch_processing = 1; - d_samp_item = samp_item; - samp_frame = 0; - adc_bits = 16 / d_samp_item; - i_ = true; - new_sample = false; + adc_bits = adc_nbit; i_data = 0; q_data = 0; + samples_per_int = 16 / adc_bits; + if(adc_bits == 2) + { + mask_data = 0x00000003; + map_ = {0, 1, -2, -1}; + } + else + { + mask_data = 0x0000000F; + map_ = {0, 1, 2, 3, 4, 5, 6, 7, -8, -7, -6, -5, -4, -3, -2, -1}; + } } unpack_spir_gss6450_samples::~unpack_spir_gss6450_samples() {} -void unpack_spir_gss6450_samples::process_sample(gr_complex* out) +void unpack_spir_gss6450_samples::process_sample(gr_complex& out) { - gr_complex result = gr_complex(0.5, 0.5); + out = gr_complex(0.5, 0.5); compute_two_complement(i_data); compute_two_complement(q_data); - result += gr_complex(static_cast(i_data), static_cast(q_data)); - *out = result; + out += gr_complex(static_cast(i_data), static_cast(q_data)); } -int unpack_spir_gss6450_samples::general_work(int noutput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items) + +int unpack_spir_gss6450_samples::work(int noutput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { const int* in = reinterpret_cast(input_items[0]); gr_complex* out = reinterpret_cast(output_items[0]); - int samples_produced = 0; + unsigned int n_sample = 0; + unsigned int in_counter = 0; for(int i = 0; i < noutput_items; i++) { - if(ch_processing == d_sel_ch) + int sample_aux = in[in_counter]; + //reverse_bits(sample_aux); + int aux_i = sample_aux; + int aux_q = sample_aux; + int i_shift = adc_bits * 2 * (samples_per_int - n_sample - 1) + adc_bits; + int q_shift = adc_bits * 2 * (samples_per_int - n_sample - 1); + i_data = (aux_i >> i_shift) & mask_data; + q_data = (aux_q >> q_shift) & mask_data; + process_sample(out[samples_per_int * in_counter + samples_per_int - n_sample - 1]); + n_sample++; + if(n_sample == samples_per_int) { - if(i_) - { - i_data = in[i]; - swap_data(i_data); - i_ = false; - } - else - { - q_data = in[i]; - swap_data(q_data); - i_ = true; - process_sample(out); - out++; - samples_produced++; - new_sample = true; - } - } - else - { - if(i_) { i_ = false;} - else - { - i_ = true; - new_sample = true; - } - } - if(new_sample) - { - new_sample = false; - samp_frame++; - if(samp_frame == d_samp_item) - { - samp_frame = 0; - ch_processing++; - if(ch_processing > d_channels) { ch_processing = 1; } - } + n_sample = 0; + in_counter++; } } - consume_each(noutput_items); - return samples_produced; + return noutput_items; } -void unpack_spir_gss6450_samples::swap_data(int& data) +void unpack_spir_gss6450_samples::reverse_bits(int& data) { - int result = 0; - int aux = data; - int mask = 1; - for (int i = 0; i < adc_bits; i++) + unsigned int v = data; // input bits to be reversed + unsigned int r = v; // r will be reversed bits of v; first get LSB of v + int s = sizeof(v) * CHAR_BIT - 1; // extra shift needed at end + + for (v >>= 1; v; v >>= 1) { - result = result << 1; - result += (aux & mask); - aux = aux >> 1; + r <<= 1; + r |= v & 1; + s--; } - data = result; + r <<= s; // shift when v's highest bits are zero + data = r; } void unpack_spir_gss6450_samples::compute_two_complement(int& data) { - int result = 0; - int mask = 1; - for(int i = 0; i < (adc_bits - 1); i++) - { - result = result << 1; - result += (data >> i) & mask; - } - if((data >> (adc_bits - 1)) == 1) - { - if(adc_bits == 2) { result -= 2; } - else { result -= 8; } - } - data = result; + data = map_[data]; } diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h index 1077fcc73..df890fc74 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h @@ -31,40 +31,35 @@ #ifndef GNSS_SDR_UNPACK_SPIR_GSS6450_SAMPLES_H #define GNSS_SDR_UNPACK_SPIR_GSS6450_SAMPLES_H -#include +#include +#include class unpack_spir_gss6450_samples; typedef boost::shared_ptr unpack_spir_gss6450_samples_sptr; -unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size); +unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples(unsigned int adc_nbit); -class unpack_spir_gss6450_samples: public gr::block +class unpack_spir_gss6450_samples: public gr::sync_interpolator { public: - int general_work (int noutput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items); - friend unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples_sptr(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size); - unpack_spir_gss6450_samples(unsigned int n_chann, unsigned int sel_ch, int samp_item, size_t item_size); + int work(int noutput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); + friend unpack_spir_gss6450_samples_sptr make_unpack_spir_gss6450_samples_sptr(unsigned int adc_nbit); + unpack_spir_gss6450_samples(unsigned int adc_nbit); ~unpack_spir_gss6450_samples(); private: - unsigned int d_channels; - unsigned int d_sel_ch; - unsigned int ch_processing; - int d_samp_item; - int samp_frame; - int adc_bits; - size_t item_size_; - void process_sample(gr_complex* out); - void swap_data(int& data); + unsigned int adc_bits; + unsigned int samples_per_int; + void process_sample(gr_complex& out); void compute_two_complement(int& data); - bool i_; - bool new_sample; + void reverse_bits(int& data); int i_data; int q_data; + int mask_data; + std::vector map_; }; #endif From f6be0943343811a7de8317e1243cfbbc7a62d1f6 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 18 Jan 2018 19:39:21 +0100 Subject: [PATCH 42/55] Initialize all class members in the constructor --- .../gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index a818da13d..ecc7df1be 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -251,6 +251,13 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( d_carrier_doppler_hz = 0.0; d_acc_carrier_phase_rad = 0.0; + d_extend_correlation_symbols_count = 0; + d_code_phase_step_chips = 0.0; + d_carrier_phase_step_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_K_blk_samples = 0.0; + d_code_phase_samples = 0.0; + d_state = 0; // initial state: standby } From 1095bd0b48406a63bb5485dea85d76c99a834c8c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 19 Jan 2018 09:46:44 +0100 Subject: [PATCH 43/55] Update satellite info --- src/core/system_parameters/gnss_satellite.cc | 160 +++++++++---------- 1 file changed, 80 insertions(+), 80 deletions(-) diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index a146b3451..0538355b2 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -252,103 +252,103 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int { switch ( PRN_ ) { - // info from http://www.navcen.uscg.gov/?Do=constellationStatus + // info from https://www.navcen.uscg.gov/?Do=constellationStatus case 1 : - block_ = std::string("IIF"); //Plane D + block_ = std::string("IIF"); // Plane D break; case 2 : - block_ = std::string("IIR"); //Plane D + block_ = std::string("IIR"); // Plane D break; case 3 : - block_ = std::string("IIF"); //Plane E + block_ = std::string("IIF"); // Plane E break; case 4 : block_ = std::string("Unknown"); break; case 5 : - block_ = std::string("IIR-M"); //Plane E + block_ = std::string("IIR-M"); // Plane E break; case 6 : - block_ = std::string("IIF"); //Plane D + block_ = std::string("IIF"); // Plane D break; case 7 : - block_ = std::string("IIR-M"); //Plane A + block_ = std::string("IIR-M"); // Plane A break; case 8 : - block_ = std::string("IIF"); //Plane C + block_ = std::string("IIF"); // Plane C break; case 9 : - block_ = std::string("IIF"); //Plane F + block_ = std::string("IIF"); // Plane F break; case 10 : - block_ = std::string("IIF"); //Plane E + block_ = std::string("IIF"); // Plane E break; case 11 : - block_ = std::string("IIR"); //Plane D + block_ = std::string("IIR"); // Plane D break; case 12 : - block_ = std::string("IIR-M"); //Plane B + block_ = std::string("IIR-M"); // Plane B break; case 13 : - block_ = std::string("IIR"); //Plane F + block_ = std::string("IIR"); // Plane F break; case 14 : - block_ = std::string("IIR"); //Plane F + block_ = std::string("IIR"); // Plane F break; case 15 : - block_ = std::string("IIR-M"); //Plane F + block_ = std::string("IIR-M"); // Plane F break; case 16 : - block_ = std::string("IIR"); //Plane B + block_ = std::string("IIR"); // Plane B break; case 17 : - block_ = std::string("IIR-M"); //Plane C + block_ = std::string("IIR-M"); // Plane C break; case 18 : - block_ = std::string("IIR"); //Plane E + block_ = std::string("IIR"); // Plane E break; case 19 : - block_ = std::string("IIR"); //Plane D + block_ = std::string("IIR"); // Plane D break; case 20 : - block_ = std::string("IIR"); //Plane B + block_ = std::string("IIR"); // Plane B break; case 21 : - block_ = std::string("IIR"); //Plane D + block_ = std::string("IIR"); // Plane D break; case 22 : - block_ = std::string("IIR"); //Plane E + block_ = std::string("IIR"); // Plane E break; case 23 : - block_ = std::string("IIR"); //Plane F + block_ = std::string("IIR"); // Plane F break; case 24 : - block_ = std::string("IIF"); //Plane A + block_ = std::string("IIF"); // Plane A break; case 25 : - block_ = std::string("IIF"); //Plane B + block_ = std::string("IIF"); // Plane B break; case 26 : - block_ = std::string("IIF"); //Plane B + block_ = std::string("IIF"); // Plane B break; case 27 : - block_ = std::string("IIF"); //Plane C + block_ = std::string("IIF"); // Plane C break; case 28 : - block_ = std::string("IIR"); //Plane B + block_ = std::string("IIR"); // Plane B break; case 29 : - block_ = std::string("IIR-M"); //Plane C + block_ = std::string("IIR-M"); // Plane C break; case 30 : - block_ = std::string("IIF"); //Plane A + block_ = std::string("IIF"); // Plane A break; case 31 : - block_ = std::string("IIR-M"); //Plane A + block_ = std::string("IIR-M"); // Plane A break; case 32 : - block_ = std::string("IIF"); //Plane F + block_ = std::string("IIF"); // Plane F break; default : block_ = std::string("Unknown"); @@ -360,103 +360,103 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int { switch ( PRN_ ) { - // info from http://www.sdcm.ru/smglo/grupglo?version=eng&site=extern + // Info from http://www.sdcm.ru/smglo/grupglo?version=eng&site=extern // See also http://www.glonass-center.ru/en/GLONASS/ case 1 : - block_ = std::string("1"); //Plane 1 + block_ = std::string("1"); // Plane 1 rf_link = 1; break; case 2 : - block_ = std::string("-4"); //Plane 1 + block_ = std::string("-4"); // Plane 1 rf_link = -4; break; case 3 : - block_ = std::string("5"); //Plane 1 + block_ = std::string("5"); // Plane 1 rf_link = 5; break; case 4 : - block_ = std::string("6"); //Plane 1 + block_ = std::string("6"); // Plane 1 rf_link = 6; break; case 5 : - block_ = std::string("1"); //Plane 1 + block_ = std::string("1"); // Plane 1 rf_link = 1; break; case 6 : - block_ = std::string("-4"); //Plane 1 + block_ = std::string("-4"); // Plane 1 rf_link = -4; break; case 7 : - block_ = std::string("5"); //Plane 1 + block_ = std::string("5"); // Plane 1 rf_link = 5; break; case 8 : - block_ = std::string("6"); //Plane 1 + block_ = std::string("6"); // Plane 1 rf_link = 6; break; case 9 : - block_ = std::string("-2"); //Plane 2 + block_ = std::string("-2"); // Plane 2 rf_link = -2; break; case 10 : - block_ = std::string("-7"); //Plane 2 + block_ = std::string("-7"); // Plane 2 rf_link = -7; break; case 11 : - block_ = std::string("0"); //Plane 2 + block_ = std::string("0"); // Plane 2 rf_link = 0; break; case 12 : - block_ = std::string("-1"); //Plane 2 + block_ = std::string("-1"); // Plane 2 rf_link = -1; break; case 13 : - block_ = std::string("-2"); //Plane 2 + block_ = std::string("-2"); // Plane 2 rf_link = -2; break; case 14 : - block_ = std::string("-7"); //Plane 2 + block_ = std::string("-7"); // Plane 2 rf_link = -7; break; case 15 : - block_ = std::string("0"); //Plane 2 + block_ = std::string("0"); // Plane 2 rf_link = 0; break; case 16 : - block_ = std::string("-1"); //Plane 2 + block_ = std::string("-1"); // Plane 2 rf_link = -1; break; case 17 : - block_ = std::string("4"); //Plane 3 + block_ = std::string("4"); // Plane 3 rf_link = 4; break; case 18 : - block_ = std::string("-3"); //Plane 3 + block_ = std::string("-3"); // Plane 3 rf_link = -3; break; case 19 : - block_ = std::string("3"); //Plane 3 + block_ = std::string("3"); // Plane 3 rf_link = 3; break; case 20 : - block_ = std::string("2"); //Plane 3 + block_ = std::string("2"); // Plane 3 rf_link = 2; break; case 21 : - block_ = std::string("4"); //Plane 3 + block_ = std::string("4"); // Plane 3 rf_link = 4; break; case 22 : - block_ = std::string("-3"); //Plane 3 + block_ = std::string("-3"); // Plane 3 rf_link = -3; break; case 23 : - block_ = std::string("3"); //Plane 3 + block_ = std::string("3"); // Plane 3 rf_link = 3; break; case 24 : - block_ = std::string("2"); //Plane 3 + block_ = std::string("2"); // Plane 3 rf_link = 2; break; default : @@ -488,74 +488,74 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int } if (system_.compare("Galileo") == 0) { - // Check http://en.wikipedia.org/wiki/List_of_Galileo_satellites + // Check http://en.wikipedia.org/wiki/List_of_Galileo_satellites and https://www.gsc-europa.eu/system-status/Constellation-Information switch ( PRN_ ) { case 1: - block_ = std::string("FOC-FM10"); // Galileo Full Operational Capability (FOC) satellite FM10 / GSAT-0210, launched on May 24, 2016. + block_ = std::string("FOC-FM10"); // Galileo Full Operational Capability (FOC) satellite FM10 / GSAT-0210, launched on May 24, 2016. break; case 2: - block_ = std::string("FOC-FM11"); // Galileo Full Operational Capability (FOC) satellite FM11 / GSAT-0211, launched on May 24, 2016. + block_ = std::string("FOC-FM11"); // Galileo Full Operational Capability (FOC) satellite FM11 / GSAT-0211, launched on May 24, 2016. break; case 3: - block_ = std::string("FOC-FM12"); // Galileo Full Operational Capability (FOC) satellite FM12 / GSAT-0212, launched on November 17, 2016. + block_ = std::string("FOC-FM12"); // Galileo Full Operational Capability (FOC) satellite FM12 / GSAT-0212, launched on November 17, 2016. break; case 4: - block_ = std::string("FOC-FM13"); // Galileo Full Operational Capability (FOC) satellite FM13 / GSAT-0213, launched on November 17, 2016. + block_ = std::string("FOC-FM13"); // Galileo Full Operational Capability (FOC) satellite FM13 / GSAT-0213, launched on November 17, 2016. break; case 5: - block_ = std::string("FOC-FM14"); // Galileo Full Operational Capability (FOC) satellite FM14 / GSAT-0214, launched on November 17, 2016. + block_ = std::string("FOC-FM14"); // Galileo Full Operational Capability (FOC) satellite FM14 / GSAT-0214, launched on November 17, 2016. break; case 7: - block_ = std::string("FOC-FM7"); // Galileo Full Operational Capability (FOC) satellite FM7 / GSAT-0207, launched on November 17, 2016. + block_ = std::string("FOC-FM7"); // Galileo Full Operational Capability (FOC) satellite FM7 / GSAT-0207, launched on November 17, 2016. break; case 8: - block_ = std::string("FOC-FM8"); // Galileo Full Operational Capability (FOC) satellite FM8 / GSAT0208, launched on December 17, 2015. + block_ = std::string("FOC-FM8"); // Galileo Full Operational Capability (FOC) satellite FM8 / GSAT0208, launched on December 17, 2015. break; case 9: - block_ = std::string("FOC-FM9"); // Galileo Full Operational Capability (FOC) satellite FM9 / GSAT0209, launched on December 17, 2015. + block_ = std::string("FOC-FM9"); // Galileo Full Operational Capability (FOC) satellite FM9 / GSAT0209, launched on December 17, 2015. break; case 11 : - block_ = std::string("IOV-PFM"); // PFM, the ProtoFlight Model / GSAT0101, launched from French Guiana at 10:30 GMT on October 21, 2011. + block_ = std::string("IOV-PFM"); // PFM, the ProtoFlight Model / GSAT0101, launched from French Guiana at 10:30 GMT on October 21, 2011. break; case 12 : - block_ = std::string("IOV-FM2**"); // Galileo In-Orbit Validation (IOV) satellite FM2 (Flight Model 2) also known as GSAT0102, from French Guiana at 10:30 GMT on October 21, 2011. + block_ = std::string("IOV-FM2"); // Galileo In-Orbit Validation (IOV) satellite FM2 (Flight Model 2) also known as GSAT0102, from French Guiana at 10:30 GMT on October 21, 2011. break; case 14 : - block_ = std::string("FOC-FM2*"); // Galileo Full Operational Capability (FOC) satellite FM2 / GSAT0202, launched into incorrect orbit on August 22, 2014. Moved to usable orbit in March, 2015. + block_ = std::string("FOC-FM2*"); // Galileo Full Operational Capability (FOC) satellite FM2 / GSAT0202, launched into incorrect orbit on August 22, 2014. Moved to usable orbit in March, 2015. UNDER TESTING. break; case 18 : - block_ = std::string("FOC-FM1*"); // Galileo Full Operational Capability (FOC) satellite FM1 / GSAT0201, launched into incorrect orbit on August 22, 2014. Moved to usable orbit in December, 2014. + block_ = std::string("FOC-FM1*"); // Galileo Full Operational Capability (FOC) satellite FM1 / GSAT0201, launched into incorrect orbit on August 22, 2014. Moved to usable orbit in December, 2014. UNDER TESTING. break; case 19 : - block_ = std::string("IOV-FM3"); // Galileo In-Orbit Validation (IOV) satellite FM3 (Flight Model 3) / GSAT0103, launched on October 12, 2012. + block_ = std::string("IOV-FM3"); // Galileo In-Orbit Validation (IOV) satellite FM3 (Flight Model 3) / GSAT0103, launched on October 12, 2012. break; case 20 : - block_ = std::string("IOV-FM4**"); // Galileo In-Orbit Validation (IOV) satellite FM4 (Flight Model 4) / GSAT0104, launched on October 12, 2012. Partially unavailable: Payload power problem beginning May 27, 2014 led to permanent loss of E5 and E6 transmissions, E1 transmission restored. + block_ = std::string("IOV-FM4**"); // Galileo In-Orbit Validation (IOV) satellite FM4 (Flight Model 4) / GSAT0104, launched on October 12, 2012. Payload power problem beginning May 27, 2014 led to permanent loss of E5 and E6 transmissions, E1 transmission restored. UNAVAILABLE FROM 2014-05-27 UNTIL FURTHER NOTICE break; case 21 : - block_ = std::string("FOC-FM15"); // Galileo Full Operational Capability (FOC) satellite FM15 / GSAT0215, launched on Dec. 12, 2017. + block_ = std::string("FOC-FM15"); // Galileo Full Operational Capability (FOC) satellite FM15 / GSAT0215, launched on Dec. 12, 2017. UNDER COMMISIONING. break; case 22 : - block_ = std::string("FOC-FM4"); // Galileo Full Operational Capability (FOC) satellite FM4 / GSAT0204, launched on March 27, 2015. + block_ = std::string("FOC-FM4**"); // Galileo Full Operational Capability (FOC) satellite FM4 / GSAT0204, launched on March 27, 2015. REMOVED FROM ACTIVE SERVICE ON 2017-12-08 UNTIL FURTHER NOTICE FOR CONSTELLATION MANAGEMENT PURPOSES. break; case 24 : - block_ = std::string("FOC-FM5"); // Galileo Full Operational Capability (FOC) satellite FM5 / GSAT0205, launched on Sept. 11, 2015. + block_ = std::string("FOC-FM5"); // Galileo Full Operational Capability (FOC) satellite FM5 / GSAT0205, launched on Sept. 11, 2015. break; case 25 : - block_ = std::string("FOC-FM16"); // Galileo Full Operational Capability (FOC) satellite FM16 / GSAT0216, launched on Dec. 12, 2017. + block_ = std::string("FOC-FM16"); // Galileo Full Operational Capability (FOC) satellite FM16 / GSAT0216, launched on Dec. 12, 2017. UNDER COMMISIONING. break; case 26 : - block_ = std::string("FOC-FM3"); // Galileo Full Operational Capability (FOC) satellite FM3 / GSAT0203, launched on March 27, 2015. + block_ = std::string("FOC-FM3"); // Galileo Full Operational Capability (FOC) satellite FM3 / GSAT0203, launched on March 27, 2015. break; case 27 : - block_ = std::string("FOC-FM17"); // Galileo Full Operational Capability (FOC) satellite FM17 / GSAT0217, launched on Dec. 12, 2017. + block_ = std::string("FOC-FM17"); // Galileo Full Operational Capability (FOC) satellite FM17 / GSAT0217, launched on Dec. 12, 2017. UNDER COMMISIONING. break; case 30 : - block_ = std::string("FOC-FM6"); // Galileo Full Operational Capability (FOC) satellite FM6 / GSAT0206, launched on Sept. 11, 2015. + block_ = std::string("FOC-FM6"); // Galileo Full Operational Capability (FOC) satellite FM6 / GSAT0206, launched on Sept. 11, 2015. break; case 31 : - block_ = std::string("FOC-FM18"); // Galileo Full Operational Capability (FOC) satellite FM18 / GSAT0218, launched on Dec. 12, 2017. + block_ = std::string("FOC-FM18"); // Galileo Full Operational Capability (FOC) satellite FM18 / GSAT0218, launched on Dec. 12, 2017. UNDER COMMISIONING. break; default: block_ = std::string("Unknown(Simulated)"); From 079f31a624881ac9130c9866eb5a7eb295d2c36f Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 19 Jan 2018 12:05:53 +0100 Subject: [PATCH 44/55] Add GPS L5 --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1b513a25e..2c101387c 100644 --- a/README.md +++ b/README.md @@ -1047,8 +1047,9 @@ Each channel must be assigned to a GNSS signal, according to the following ident |:------------------|:---------------:| | GPS L1 C/A | 1C | | GPS L2 L2C(M) | 2S | -| Galileo E1B | 1B | -| Galileo E5a (I+Q) | 5X | +| GPS L5 | L5 | +| Galileo E1b/c | 1B | +| Galileo E5a | 5X | Example: Eight GPS L1 C/A channels. From 3ca7d0d08a965861d8dbfa4a07ac41a58b20a8fa Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 19 Jan 2018 12:09:40 +0100 Subject: [PATCH 45/55] Remove unused include --- .../adapters/file_signal_source.cc | 53 ++++++------------- 1 file changed, 17 insertions(+), 36 deletions(-) diff --git a/src/algorithms/signal_source/adapters/file_signal_source.cc b/src/algorithms/signal_source/adapters/file_signal_source.cc index 42591ec79..ea2bd190f 100644 --- a/src/algorithms/signal_source/adapters/file_signal_source.cc +++ b/src/algorithms/signal_source/adapters/file_signal_source.cc @@ -37,7 +37,6 @@ #include #include #include -#include #include "gnss_sdr_valve.h" #include "configuration_interface.h" @@ -51,7 +50,7 @@ DEFINE_string(signal_source, "-", FileSignalSource::FileSignalSource(ConfigurationInterface* configuration, std::string role, unsigned int in_streams, unsigned int out_streams, boost::shared_ptr queue) : - role_(role), in_streams_(in_streams), out_streams_(out_streams), queue_(queue) + role_(role), in_streams_(in_streams), out_streams_(out_streams), queue_(queue) { std::string default_filename = "./example_capture.dat"; std::string default_item_type = "short"; @@ -71,8 +70,7 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration, dump_ = configuration->property(role + ".dump", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false); - std::string s = "InputFilter"; - //double IF = configuration->property(s + ".IF", 0.0); + double seconds_to_skip = configuration->property(role + ".seconds_to_skip", default_seconds_to_skip ); header_size = configuration->property( role + ".header_size", 0 ); long samples_to_skip = 0; @@ -116,29 +114,27 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration, file_source_ = gr::blocks::file_source::make(item_size_, filename_.c_str(), repeat_); if( seconds_to_skip > 0 ) - { - samples_to_skip = static_cast< long >( - seconds_to_skip * sampling_frequency_ ); - - if( is_complex ) { - samples_to_skip *= 2; + samples_to_skip = static_cast< long >( seconds_to_skip * sampling_frequency_ ); + + if( is_complex ) + { + samples_to_skip *= 2; + } } - } if( header_size > 0 ) - { - samples_to_skip += header_size; - } + { + samples_to_skip += header_size; + } if( samples_to_skip > 0 ) - { - LOG(INFO) << "Skipping " << samples_to_skip << " samples of the input file"; - if( not file_source_->seek( samples_to_skip, SEEK_SET ) ) { - LOG(INFO) << "Error skipping bytes!"; + LOG(INFO) << "Skipping " << samples_to_skip << " samples of the input file"; + if( not file_source_->seek( samples_to_skip, SEEK_SET ) ) + { + LOG(INFO) << "Error skipping bytes!"; + } } - } - } catch (const std::exception &e) { @@ -171,7 +167,6 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration, << std::endl << GNSSSDR_INSTALL_DIR "/share/gnss-sdr/conf/" << std::endl; - } LOG(INFO) << "file_signal_source: Unable to open the samples file " @@ -238,8 +233,8 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration, if (enable_throttle_control_) { throttle_ = gr::blocks::throttle::make(item_size_, sampling_frequency_); - } + DLOG(INFO) << "File source filename " << filename_; DLOG(INFO) << "Samples " << samples_; DLOG(INFO) << "Sampling frequency " << sampling_frequency_; @@ -251,14 +246,10 @@ FileSignalSource::FileSignalSource(ConfigurationInterface* configuration, } - - FileSignalSource::~FileSignalSource() {} - - void FileSignalSource::connect(gr::top_block_sptr top_block) { if (samples_ > 0) @@ -310,10 +301,6 @@ void FileSignalSource::connect(gr::top_block_sptr top_block) } - - - - void FileSignalSource::disconnect(gr::top_block_sptr top_block) { if (samples_ > 0) @@ -365,9 +352,6 @@ void FileSignalSource::disconnect(gr::top_block_sptr top_block) } - - - gr::basic_block_sptr FileSignalSource::get_left_block() { LOG(WARNING) << "Left block of a signal source should not be retrieved"; @@ -375,9 +359,6 @@ gr::basic_block_sptr FileSignalSource::get_left_block() } - - - gr::basic_block_sptr FileSignalSource::get_right_block() { if (samples_ > 0) From 7a2a02252a3385011c16a1455bf8e2b255ca9822 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Fri, 19 Jan 2018 13:50:33 +0100 Subject: [PATCH 46/55] Minor changes --- .../spir_gss6450_file_signal_source.cc | 56 ++++++++++++++----- .../spir_gss6450_file_signal_source.h | 4 +- 2 files changed, 45 insertions(+), 15 deletions(-) diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc index 590c60c33..81a7d33e6 100644 --- a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc @@ -54,6 +54,8 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* filename_ = configuration->property(role + ".filename", default_filename); repeat_ = configuration->property(role + ".repeat", false); dump_ = configuration->property(role + ".dump", false); + dump_test_ = configuration->property(role + ".dump_test", false); + endian_swap_ = configuration->property(role + ".endian", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false); adc_bits_ = configuration->property(role + ".adc_bits", 4); @@ -70,7 +72,7 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* { null_sinks_.push_back(gr::blocks::null_sink::make(item_size_)); } - std::cout << "NUMBER OF NULL SINKS = " << null_sinks_.size() << std::endl; + DLOG(INFO)<< "NUMBER OF NULL SINKS = " << null_sinks_.size(); } try { @@ -78,7 +80,6 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* file_source_->seek(bytes_seek / item_size_, SEEK_SET); unpack_spir_ = make_unpack_spir_gss6450_samples(adc_bits_); deint_ = gr::blocks::deinterleave::make(item_size_); - endian_ = gr::blocks::endian_swap::make(item_size_); } catch (const std::exception &e) { @@ -147,13 +148,20 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* if (dump_) { sink_ = gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str()); - //sink_test = gr::blocks::file_sink::make(sizeof(int), "/home/aramos/Escritorio/test_int.dat"); DLOG(INFO) << "file_sink(" << sink_->unique_id() << ")"; } + if (dump_test_) + { + sink_test = gr::blocks::file_sink::make(sizeof(int), (dump_filename_ + "int").c_str()); + } if (enable_throttle_control_) { throttle_ = gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_); } + if (endian_swap_) + { + endian_ = gr::blocks::endian_swap::make(item_size_); + } DLOG(INFO) << "File source filename " << filename_; DLOG(INFO) << "Samples " << samples_; DLOG(INFO) << "Sampling frequency " << sampling_frequency_; @@ -174,11 +182,15 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block) if (samples_ > 0) { top_block->connect(file_source_, 0, deint_, 0); - /* - top_block->connect(deint_, sel_ch_ - 1, endian_ ,0); - top_block->connect(endian_, 0, unpack_spir_, 0); - */ - top_block->connect(deint_, sel_ch_ - 1, unpack_spir_, 0); + if(endian_swap_) + { + top_block->connect(deint_, sel_ch_ - 1, endian_ ,0); + top_block->connect(endian_, 0, unpack_spir_, 0); + } + else + { + top_block->connect(deint_, sel_ch_ - 1, unpack_spir_, 0); + } if(n_channels_ > 1) { unsigned int aux = 0; @@ -203,7 +215,11 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block) if(dump_) { top_block->connect(valve_, 0, sink_, 0); - //top_block->connect(deint_, sel_ch_ - 1, sink_test, 0); + } + if(dump_test_) + { + if(endian_swap_) top_block->connect(endian_, 0, sink_test, 0); + else top_block->connect(deint_, sel_ch_ - 1, sink_test, 0); } } else @@ -218,7 +234,15 @@ void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block) if (samples_ > 0) { top_block->disconnect(file_source_, 0, deint_, 0); - top_block->disconnect(deint_, sel_ch_ - 1, unpack_spir_, 0); + if(endian_swap_) + { + top_block->disconnect(deint_, sel_ch_ - 1, endian_ ,0); + top_block->disconnect(endian_, 0, unpack_spir_, 0); + } + else + { + top_block->disconnect(deint_, sel_ch_ - 1, unpack_spir_, 0); + } if(n_channels_ > 1) { unsigned int aux = 0; @@ -241,10 +265,14 @@ void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block) top_block->disconnect(unpack_spir_, 0, valve_, 0); } if(dump_) - { - top_block->disconnect(valve_, 0, sink_, 0); - //top_block->disconnect(deint_, sel_ch_ - 1, sink_test, 0); - } + { + top_block->disconnect(valve_, 0, sink_, 0); + } + if(dump_test_) + { + if(endian_swap_) top_block->disconnect(endian_, 0, sink_test, 0); + else top_block->disconnect(deint_, sel_ch_ - 1, sink_test, 0); + } } else { diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h index e8387dbd7..411d04069 100644 --- a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h @@ -111,8 +111,10 @@ private: double sampling_frequency_; std::string filename_; bool repeat_; - bool dump_; + bool dump_; //Enables dumping the gr_complex sample output + bool dump_test_; //Enables dumping the raw 32-bits deinterleaved (and endian swapped if enabled) words bool enable_throttle_control_; + bool endian_swap_; std::string dump_filename_; std::string role_; std::string item_type_; From 03157464292625ad87eff3b6d5d1467d6d8c9c75 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 20 Jan 2018 04:17:50 +0100 Subject: [PATCH 47/55] Fix warning --- src/core/libs/supl/asn-supl/GeneralizedTime.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/core/libs/supl/asn-supl/GeneralizedTime.c b/src/core/libs/supl/asn-supl/GeneralizedTime.c index 9d683efe4..c3e01f99e 100644 --- a/src/core/libs/supl/asn-supl/GeneralizedTime.c +++ b/src/core/libs/supl/asn-supl/GeneralizedTime.c @@ -3,7 +3,9 @@ * Redistribution and modifications are permitted subject to BSD license. */ #define _POSIX_PTHREAD_SEMANTICS /* for Sun */ +#ifdef _REENTRANT #define _REENTRANT /* for Sun */ +#endif #include #include #include From db938bb047d2eba5a3eed4ee293f28803ebe5667 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 20 Jan 2018 11:56:17 +0100 Subject: [PATCH 48/55] Build fix due to a change in the gr-iio v0.3 API A change in the API has removed the decimation parameter from sources (see https://github.com/analogdevicesinc/gr-iio/issues/15 ) This commit fixes the build against the current master of https://github.com/analogdevicesinc/gr-iio but it breaks compatibility with older versions. Users with gr-iio already installed will need to update it. --- README.md | 2 +- conf/gnss-sdr_GPS_L1_fmcomms2_realtime.conf | 2 -- conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf | 2 -- .../signal_source/adapters/fmcomms2_signal_source.cc | 3 +-- src/algorithms/signal_source/adapters/fmcomms2_signal_source.h | 1 - .../signal_source/adapters/plutosdr_signal_source.cc | 3 +-- src/algorithms/signal_source/adapters/plutosdr_signal_source.h | 1 - 7 files changed, 3 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 2c101387c..06f884a00 100644 --- a/README.md +++ b/README.md @@ -375,7 +375,7 @@ $ sudo make install ###### Build FMCOMMS2 based SDR Hardware support (OPTIONAL): -Install the [libiio](https://github.com/analogdevicesinc/libiio.git) (>=v0.11), [libad9361](https://github.com/analogdevicesinc/libad9361-iio.git) (>=v0.1-1) libraries and [gr-iio](https://github.com/analogdevicesinc/gr-iio.git) (>v0.2) gnuradio block: +Install the [libiio](https://github.com/analogdevicesinc/libiio.git) (>=v0.11), [libad9361](https://github.com/analogdevicesinc/libad9361-iio.git) (>=v0.1-1) libraries and [gr-iio](https://github.com/analogdevicesinc/gr-iio.git) (>v0.3) gnuradio block: ~~~~~~ $ sudo apt-get install libxml2-dev bison flex diff --git a/conf/gnss-sdr_GPS_L1_fmcomms2_realtime.conf b/conf/gnss-sdr_GPS_L1_fmcomms2_realtime.conf index ae832f901..6c8d8d85c 100644 --- a/conf/gnss-sdr_GPS_L1_fmcomms2_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_fmcomms2_realtime.conf @@ -27,13 +27,11 @@ GNSS-SDR.SUPL_CI=0x31b0 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=Fmcomms2_Signal_Source -;SignalSource.filename=/media/DATALOGGER_/signals/RTL-SDR/geo/pmt4.dat SignalSource.item_type=gr_complex SignalSource.device_address=10.42.0.196 SignalSource.sampling_frequency=2000000 SignalSource.freq=1575420000 SignalSource.bandwidth=2000000 -SignalSource.decimation=0 SignalSource.rx1_enable=true SignalSource.gain_mode_rx1=manual SignalSource.rf_port_select=A_BALANCED diff --git a/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf b/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf index 97c9deded..83f808645 100644 --- a/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf @@ -27,13 +27,11 @@ GNSS-SDR.SUPL_CI=0x31b0 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=Plutosdr_Signal_Source -;SignalSource.filename=/media/DATALOGGER_/signals/RTL-SDR/geo/pmt4.dat SignalSource.item_type=gr_complex SignalSource.device_address=192.168.2.1 SignalSource.sampling_frequency=3000000 SignalSource.freq=1575420000 SignalSource.bandwidth=2600000 -SignalSource.decimation=0 SignalSource.gain_mode=manual SignalSource.gain=30 SignalSource.samples=0 diff --git a/src/algorithms/signal_source/adapters/fmcomms2_signal_source.cc b/src/algorithms/signal_source/adapters/fmcomms2_signal_source.cc index 5c354e500..52916a19e 100644 --- a/src/algorithms/signal_source/adapters/fmcomms2_signal_source.cc +++ b/src/algorithms/signal_source/adapters/fmcomms2_signal_source.cc @@ -53,7 +53,6 @@ Fmcomms2SignalSource::Fmcomms2SignalSource(ConfigurationInterface* configuration rx1_en_ = configuration->property(role + ".rx1_enable", true); rx2_en_ = configuration->property(role + ".rx2_enable", false); buffer_size_ = configuration->property(role + ".buffer_size", 0xA0000); - decimation_ = configuration->property(role + ".decimation", 1); quadrature_ = configuration->property(role + ".quadrature", true); rf_dc_ = configuration->property(role + ".rf_dc", true); bb_dc_ = configuration->property(role + ".bb_dc", true); @@ -79,7 +78,7 @@ Fmcomms2SignalSource::Fmcomms2SignalSource(ConfigurationInterface* configuration { fmcomms2_source_f32c_ = gr::iio::fmcomms2_source_f32c::make( uri_.c_str(), freq_, sample_rate_, - decimation_, bandwidth_, + bandwidth_, rx1_en_, rx2_en_, buffer_size_, quadrature_, rf_dc_, bb_dc_, gain_mode_rx1_.c_str(), rf_gain_rx1_, diff --git a/src/algorithms/signal_source/adapters/fmcomms2_signal_source.h b/src/algorithms/signal_source/adapters/fmcomms2_signal_source.h index fb62a12cb..78f3ebb58 100644 --- a/src/algorithms/signal_source/adapters/fmcomms2_signal_source.h +++ b/src/algorithms/signal_source/adapters/fmcomms2_signal_source.h @@ -83,7 +83,6 @@ private: unsigned long sample_rate_; unsigned long bandwidth_; unsigned long buffer_size_; //reception buffer - unsigned int decimation_; bool rx1_en_; bool rx2_en_; bool quadrature_; diff --git a/src/algorithms/signal_source/adapters/plutosdr_signal_source.cc b/src/algorithms/signal_source/adapters/plutosdr_signal_source.cc index 34526f6a5..4e7e37b3e 100644 --- a/src/algorithms/signal_source/adapters/plutosdr_signal_source.cc +++ b/src/algorithms/signal_source/adapters/plutosdr_signal_source.cc @@ -52,7 +52,6 @@ PlutosdrSignalSource::PlutosdrSignalSource(ConfigurationInterface* configuration sample_rate_ = configuration->property(role + ".sampling_frequency", 3000000); bandwidth_ = configuration->property(role + ".bandwidth", 2000000); buffer_size_ = configuration->property(role + ".buffer_size", 0xA0000); - decimation_ = configuration->property(role + ".decimation", 1); quadrature_ = configuration->property(role + ".quadrature", true); rf_dc_ = configuration->property(role + ".rf_dc", true); bb_dc_ = configuration->property(role + ".bb_dc", true); @@ -81,7 +80,7 @@ PlutosdrSignalSource::PlutosdrSignalSource(ConfigurationInterface* configuration std::cout << "item type: " << item_type_ << std::endl; plutosdr_source_ = gr::iio::pluto_source::make(uri_, freq_, sample_rate_, - decimation_, bandwidth_, buffer_size_, quadrature_, rf_dc_, bb_dc_, + bandwidth_, buffer_size_, quadrature_, rf_dc_, bb_dc_, gain_mode_.c_str(), rf_gain_,filter_file_.c_str(), filter_auto_); if (samples_ != 0) diff --git a/src/algorithms/signal_source/adapters/plutosdr_signal_source.h b/src/algorithms/signal_source/adapters/plutosdr_signal_source.h index 9bbdb4511..56c2334c8 100644 --- a/src/algorithms/signal_source/adapters/plutosdr_signal_source.h +++ b/src/algorithms/signal_source/adapters/plutosdr_signal_source.h @@ -83,7 +83,6 @@ private: unsigned long sample_rate_; unsigned long bandwidth_; unsigned long buffer_size_; // reception buffer - unsigned int decimation_; bool quadrature_; bool rf_dc_; bool bb_dc_; From 5acef7a9bcc8a3b76f037d3e2bd17c52f4ba3e56 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 20 Jan 2018 12:24:38 +0100 Subject: [PATCH 49/55] Remove warnings of unused variables --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 2b94ab34d..5824e8493 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -448,8 +448,6 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite bool flag_compute_pvt_output = false; bool flag_write_RTCM_1019_output = false; bool flag_write_RTCM_1045_output = false; - bool flag_write_RTCM_1077_output = false; - bool flag_write_RTCM_1097_output = false; bool flag_write_RTCM_MSM_output = false; bool flag_write_RINEX_obs_output = false; bool flag_write_RINEX_nav_output = false; @@ -539,13 +537,11 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite if ((std::fabs(current_RX_time - last_RTCM_1077_output_time) * 1000.0 >= static_cast(d_rtcm_MT1077_rate_ms)) && (d_rtcm_MT1077_rate_ms != 0) ) { - flag_write_RTCM_1077_output = true; last_RTCM_1077_output_time = current_RX_time; } if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast(d_rtcm_MT1097_rate_ms)) && (d_rtcm_MT1097_rate_ms != 0) ) { - flag_write_RTCM_1097_output = true; last_RTCM_1097_output_time = current_RX_time; } From 1b4c217d88c13871ce39c430947508ee3b6a8dfa Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 20 Jan 2018 12:29:21 +0100 Subject: [PATCH 50/55] Fix redefinition warning --- src/core/libs/supl/asn-supl/GeneralizedTime.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/core/libs/supl/asn-supl/GeneralizedTime.c b/src/core/libs/supl/asn-supl/GeneralizedTime.c index c3e01f99e..2fd63f262 100644 --- a/src/core/libs/supl/asn-supl/GeneralizedTime.c +++ b/src/core/libs/supl/asn-supl/GeneralizedTime.c @@ -3,7 +3,7 @@ * Redistribution and modifications are permitted subject to BSD license. */ #define _POSIX_PTHREAD_SEMANTICS /* for Sun */ -#ifdef _REENTRANT +#ifndef _REENTRANT #define _REENTRANT /* for Sun */ #endif #include From dd77cd10dfc23e4f92a300f7a15e7e0b229118c5 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 22 Jan 2018 12:21:28 +0100 Subject: [PATCH 51/55] Remove debug lines --- .../adapters/spir_gss6450_file_signal_source.cc | 15 --------------- .../adapters/spir_gss6450_file_signal_source.h | 2 -- .../unpack_spir_gss6450_samples.cc | 17 ----------------- .../unpack_spir_gss6450_samples.h | 1 - 4 files changed, 35 deletions(-) diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc index 81a7d33e6..f19d9d3d6 100644 --- a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.cc @@ -54,7 +54,6 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* filename_ = configuration->property(role + ".filename", default_filename); repeat_ = configuration->property(role + ".repeat", false); dump_ = configuration->property(role + ".dump", false); - dump_test_ = configuration->property(role + ".dump_test", false); endian_swap_ = configuration->property(role + ".endian", false); dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); enable_throttle_control_ = configuration->property(role + ".enable_throttle_control", false); @@ -150,10 +149,6 @@ SpirGSS6450FileSignalSource::SpirGSS6450FileSignalSource(ConfigurationInterface* sink_ = gr::blocks::file_sink::make(sizeof(gr_complex), dump_filename_.c_str()); DLOG(INFO) << "file_sink(" << sink_->unique_id() << ")"; } - if (dump_test_) - { - sink_test = gr::blocks::file_sink::make(sizeof(int), (dump_filename_ + "int").c_str()); - } if (enable_throttle_control_) { throttle_ = gr::blocks::throttle::make(sizeof(gr_complex), sampling_frequency_); @@ -216,11 +211,6 @@ void SpirGSS6450FileSignalSource::connect(gr::top_block_sptr top_block) { top_block->connect(valve_, 0, sink_, 0); } - if(dump_test_) - { - if(endian_swap_) top_block->connect(endian_, 0, sink_test, 0); - else top_block->connect(deint_, sel_ch_ - 1, sink_test, 0); - } } else { @@ -268,11 +258,6 @@ void SpirGSS6450FileSignalSource::disconnect(gr::top_block_sptr top_block) { top_block->disconnect(valve_, 0, sink_, 0); } - if(dump_test_) - { - if(endian_swap_) top_block->disconnect(endian_, 0, sink_test, 0); - else top_block->disconnect(deint_, sel_ch_ - 1, sink_test, 0); - } } else { diff --git a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h index 411d04069..caa26c8fb 100644 --- a/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h +++ b/src/algorithms/signal_source/adapters/spir_gss6450_file_signal_source.h @@ -112,7 +112,6 @@ private: std::string filename_; bool repeat_; bool dump_; //Enables dumping the gr_complex sample output - bool dump_test_; //Enables dumping the raw 32-bits deinterleaved (and endian swapped if enabled) words bool enable_throttle_control_; bool endian_swap_; std::string dump_filename_; @@ -130,7 +129,6 @@ private: unpack_spir_gss6450_samples_sptr unpack_spir_; boost::shared_ptr valve_; gr::blocks::file_sink::sptr sink_; - gr::blocks::file_sink::sptr sink_test; gr::blocks::throttle::sptr throttle_; gr::msg_queue::sptr queue_; size_t item_size_; diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc index 5992c7025..7bbb2cb75 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc @@ -83,7 +83,6 @@ int unpack_spir_gss6450_samples::work(int noutput_items, for(int i = 0; i < noutput_items; i++) { int sample_aux = in[in_counter]; - //reverse_bits(sample_aux); int aux_i = sample_aux; int aux_q = sample_aux; int i_shift = adc_bits * 2 * (samples_per_int - n_sample - 1) + adc_bits; @@ -101,22 +100,6 @@ int unpack_spir_gss6450_samples::work(int noutput_items, return noutput_items; } -void unpack_spir_gss6450_samples::reverse_bits(int& data) -{ - unsigned int v = data; // input bits to be reversed - unsigned int r = v; // r will be reversed bits of v; first get LSB of v - int s = sizeof(v) * CHAR_BIT - 1; // extra shift needed at end - - for (v >>= 1; v; v >>= 1) - { - r <<= 1; - r |= v & 1; - s--; - } - r <<= s; // shift when v's highest bits are zero - data = r; -} - void unpack_spir_gss6450_samples::compute_two_complement(int& data) { data = map_[data]; diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h index df890fc74..a592c2aee 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h @@ -55,7 +55,6 @@ private: unsigned int samples_per_int; void process_sample(gr_complex& out); void compute_two_complement(int& data); - void reverse_bits(int& data); int i_data; int q_data; int mask_data; From 0dab500fb0483bcb1e082a5b122976e96b98d8f6 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 22 Jan 2018 12:38:38 +0100 Subject: [PATCH 52/55] Update author --- .../gnuradio_blocks/unpack_spir_gss6450_samples.cc | 8 ++++---- .../gnuradio_blocks/unpack_spir_gss6450_samples.h | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc index 7bbb2cb75..6a7bf869a 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.cc @@ -1,11 +1,11 @@ /*! - * \file unpack_intspir_1bit_samples.cc + * \file unpack_spir_gss6450_samples.cc * - * \brief Unpacks SPIR int samples to NSR 1 bit samples - * \author Fran Fabra fabra (at) ice.csic.es + * \brief Unpacks SPIR int samples + * \author Antonio Ramos, antonio(at)cttc.es * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver diff --git a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h index a592c2aee..25db1a9d6 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h +++ b/src/algorithms/signal_source/gnuradio_blocks/unpack_spir_gss6450_samples.h @@ -1,11 +1,11 @@ /*! - * \file unpack_intspir_1bit_samples.h + * \file unpack_spir_gss6450_samples.h * - * \brief Unpacks SPIR int samples to NSR 1 bit samples - * \author Fran Fabra fabra (at) ice.csic.es + * \brief Unpacks SPIR int samples + * \author Antonio Ramos, antonio.ramos(at)cttc.es * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver From 84fe651f0d67216e9e051bf0b32dcfd4f94a856d Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 23 Jan 2018 17:31:42 +0100 Subject: [PATCH 53/55] Adding Labsat v2 and v3 single channel signal recorder file source --- ...galileo_E1_extended_correlator_labsat.conf | 333 ++++++++++++ .../signal_source/adapters/CMakeLists.txt | 1 + .../adapters/labsat_signal_source.cc | 122 +++++ .../adapters/labsat_signal_source.h | 94 ++++ .../gnuradio_blocks/CMakeLists.txt | 1 + .../gnuradio_blocks/labsat23_source.cc | 474 ++++++++++++++++++ .../gnuradio_blocks/labsat23_source.h | 75 +++ src/core/receiver/gnss_block_factory.cc | 16 + 8 files changed, 1116 insertions(+) create mode 100644 conf/gnss-sdr_galileo_E1_extended_correlator_labsat.conf create mode 100644 src/algorithms/signal_source/adapters/labsat_signal_source.cc create mode 100644 src/algorithms/signal_source/adapters/labsat_signal_source.h create mode 100644 src/algorithms/signal_source/gnuradio_blocks/labsat23_source.cc create mode 100644 src/algorithms/signal_source/gnuradio_blocks/labsat23_source.h diff --git a/conf/gnss-sdr_galileo_E1_extended_correlator_labsat.conf b/conf/gnss-sdr_galileo_E1_extended_correlator_labsat.conf new file mode 100644 index 000000000..094f15ebe --- /dev/null +++ b/conf/gnss-sdr_galileo_E1_extended_correlator_labsat.conf @@ -0,0 +1,333 @@ +; Default configuration file +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. +GNSS-SDR.internal_fs_hz=5456000 + +;######### SIGNAL_SOURCE CONFIG ############ +;#implementation: Use [File_Signal_Source] [Nsr_File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental) +SignalSource.implementation=Labsat_Signal_Source + +SignalSource.selected_channel=1 + +;#filename: path to file with the captured GNSS signal samples to be processed +;# Labsat sile source automatically increments the file name when the signal is splitted in several files +;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on +;# in this example, the first file complete path will be ../signals/GPS_025_0000.LS3 + +SignalSource.filename=../signals/GPS_025 ; <- PUT YOUR FILE HERE + +;#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=16368000 + +;#freq: RF front-end center frequency in [Hz] +SignalSource.freq=1575420000 + +;#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 + +SignalSource.dump_filename=../data/signal_source.dat + + +;#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 ############ +;## It holds blocks to change data type, filter and resample input data. + +;#implementation: Use [Pass_Through] or [Signal_Conditioner] +;#[Pass_Through] disables this block and the [DataTypeAdapter], [InputFilter] and [Resampler] blocks +;#[Signal_Conditioner] enables this block. Then you have to configure [DataTypeAdapter], [InputFilter] and [Resampler] blocks +SignalConditioner.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER CONFIG ############ +;## Changes the type of input data. +;#implementation: [Pass_Through] disables this block +DataTypeAdapter.implementation=Pass_Through +DataTypeAdapter.item_type=gr_complex + +;######### INPUT_FILTER CONFIG ############ +;## Filter the input data. Can be combined with frequency translation for IF signals + +;#implementation: Use [Pass_Through] or [Fir_Filter] or [Freq_Xlating_Fir_Filter] +;#[Freq_Xlating_Fir_Filter] enables FIR filter and a composite frequency translation +;# that shifts IF down to zero Hz. + +InputFilter.implementation=Freq_Xlating_Fir_Filter + +;#dump: Dump the filtered data to a file. +InputFilter.dump=false + +;#dump_filename: Log path and filename. +InputFilter.dump_filename=../data/input_filter.dat + +;#The following options are used in the filter design of Fir_Filter and Freq_Xlating_Fir_Filter implementation. +;#These options are based on parameters of gnuradio's function: gr_remez. +;#These function calculates the optimal (in the Chebyshev/minimax sense) FIR filter inpulse +;#reponse given a set of band edges, the desired reponse on those bands, +;#and the weight given to the error in those bands. + +;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version. +InputFilter.input_item_type=gr_complex + +;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version. +InputFilter.output_item_type=gr_complex + +;#taps_item_type: Type and resolution for the taps of the filter. Use only float in this version. +InputFilter.taps_item_type=float + +;#number_of_taps: Number of taps in the filter. Increasing this parameter increases the processing time +InputFilter.number_of_taps=5 + +;#number_of _bands: Number of frequency bands in the filter. +InputFilter.number_of_bands=2 + +;#bands: frequency at the band edges [ b1 e1 b2 e2 b3 e3 ...]. +;#Frequency is in the range [0, 1], with 1 being the Nyquist frequency (Fs/2) +;#The number of band_begin and band_end elements must match the number of bands + +InputFilter.band1_begin=0.0 +InputFilter.band1_end=0.45 +InputFilter.band2_begin=0.55 +InputFilter.band2_end=1.0 + +;#ampl: desired amplitude at the band edges [ a(b1) a(e1) a(b2) a(e2) ...]. +;#The number of ampl_begin and ampl_end elements must match the number of bands + +InputFilter.ampl1_begin=1.0 +InputFilter.ampl1_end=1.0 +InputFilter.ampl2_begin=0.0 +InputFilter.ampl2_end=0.0 + +;#band_error: weighting applied to each band (usually 1). +;#The number of band_error elements must match the number of bands +InputFilter.band1_error=1.0 +InputFilter.band2_error=1.0 + +;#filter_type: one of "bandpass", "hilbert" or "differentiator" +InputFilter.filter_type=bandpass + +;#grid_density: determines how accurately the filter will be constructed. +;The minimum value is 16; higher values are slower to compute the filter. +InputFilter.grid_density=16 + +;# Original sampling frequency stored in the signal file +InputFilter.sampling_frequency=16368000 + +;#The following options are used only in Freq_Xlating_Fir_Filter implementation. +;#InputFilter.IF is the intermediate frequency (in Hz) shifted down to zero Hz + +InputFilter.IF=0 + +;# Decimation factor after the frequency tranaslating block +InputFilter.decimation_factor=3 + + +;######### CHANNELS GLOBAL CONFIG ############ +;#count: Number of available GPS satellite channels. +Channels_1C.count=0 +;#count: Number of available Galileo satellite channels. +Channels_1B.count=6 +;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver +Channels.in_acquisition=1 + +;#signal: +;#if the option is disabled by default is assigned "1C" GPS L1 C/A +Channel0.signal=1B +Channel1.signal=1B +Channel2.signal=1B +Channel3.signal=1B +Channel4.signal=1B +Channel5.signal=1B +Channel6.signal=1B +Channel7.signal=1B +Channel8.signal=1B +Channel9.signal=1B +Channel10.signal=1B +Channel11.signal=1B +Channel12.signal=1B +Channel13.signal=1B +Channel14.signal=1B +Channel15.signal=1B + + +;######### GPS ACQUISITION CONFIG ############ + +;#dump: Enable or disable the acquisition internal data file logging [true] or [false] +Acquisition_1C.dump=false +;#filename: Log path and filename +Acquisition_1C.dump_filename=./acq_dump.dat +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +Acquisition_1C.item_type=gr_complex +;#if: Signal intermediate frequency in [Hz] +Acquisition_1C.if=0 +;#sampled_ms: Signal block duration for the acquisition signal detection [ms] +Acquisition_1C.sampled_ms=1 +;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.use_CFAR_algorithm=false; +;#threshold: Acquisition threshold +Acquisition_1C.threshold=22 +;#doppler_max: Maximum expected Doppler shift [Hz] +Acquisition_1C.doppler_max=5000 +;#doppler_max: Doppler step in the grid search [Hz] +Acquisition_1C.doppler_step=250 + + +;######### GALILEO ACQUISITION CONFIG ############ + +;#dump: Enable or disable the acquisition internal data file logging [true] or [false] +Acquisition_1B.dump=false +;#filename: Log path and filename +Acquisition_1B.dump_filename=../data/acq_dump.dat +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +Acquisition_1B.item_type=gr_complex +;#if: Signal intermediate frequency in [Hz] +Acquisition_1B.if=0 +;#sampled_ms: Signal block duration for the acquisition signal detection [ms] +Acquisition_1B.sampled_ms=4 +;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.acquire_pilot=true +Acquisition_1B.use_CFAR_algorithm=false +;#threshold: Acquisition threshold +Acquisition_1B.threshold=22 +;#doppler_max: Maximum expected Doppler shift [Hz] +Acquisition_1B.doppler_max=5000 +;#doppler_max: Doppler step in the grid search [Hz] +Acquisition_1B.doppler_step=125 +Acquisition_1B.bit_transition_flag=true + +;######### TRACKING GPS CONFIG ############ + +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +Tracking_1C.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_1C.item_type=gr_complex + +;#sampling_frequency: Signal Intermediate Frequency in [Hz] +Tracking_1C.if=0 + +;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] +Tracking_1C.dump=true + +;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. +Tracking_1C.dump_filename=../data/epl_tracking_ch_ + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1C.pll_bw_hz=40.0; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1C.dll_bw_hz=2.0; + +;#order: PLL/DLL loop filter order [2] or [3] +Tracking_1C.order=3; + +;######### TRACKING GALILEO CONFIG ############ + +;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +;#item_type: Type and resolution for each of the signal samples. +Tracking_1B.item_type=gr_complex + +;#sampling_frequency: Signal Intermediate Frequency in [Hz] +Tracking_1B.if=0 + +;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] +Tracking_1B.dump=true + +;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. +Tracking_1B.dump_filename=../data/veml_tracking_ch_ + +Tracking_1B.track_pilot=true + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1B.pll_bw_hz=7.5; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1B.dll_bw_hz=0.5; + +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1B.pll_bw_narrow_hz=2.5; + +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1B.dll_bw_narrow_hz=0.25; + +Tracking_1B.extend_correlation_symbols=4; + +;#order: PLL/DLL loop filter order [2] or [3] +Tracking_1B.order=3; + +;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo +Tracking_1B.early_late_space_chips=0.15; + +;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] +Tracking_1B.very_early_late_space_chips=0.6; + +;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo +Tracking_1B.early_late_space_narrow_chips=0.15; + +;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] +Tracking_1B.very_early_late_space_narrow_chips=0.30; + + + +;######### TELEMETRY DECODER GPS CONFIG ############ +;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false + +;######### TELEMETRY DECODER GALILEO CONFIG ############ +;#implementation: Use [Galileo_E1B_Telemetry_Decoder] for Galileo E1B +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables + +;#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: +PVT.implementation=RTKLIB_PVT + +PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad + +;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] +PVT.output_rate_ms=100; + +;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms. +PVT.display_rate_ms=500; + +;#dump: Enable or disable the PVT internal binary data file logging [true] or [false] +PVT.dump=false + +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 + +;#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 diff --git a/src/algorithms/signal_source/adapters/CMakeLists.txt b/src/algorithms/signal_source/adapters/CMakeLists.txt index 3d786bb46..2fb8f4be8 100644 --- a/src/algorithms/signal_source/adapters/CMakeLists.txt +++ b/src/algorithms/signal_source/adapters/CMakeLists.txt @@ -141,6 +141,7 @@ set(SIGNAL_SOURCE_ADAPTER_SOURCES file_signal_source.cc spir_file_signal_source.cc spir_gss6450_file_signal_source.cc rtl_tcp_signal_source.cc + labsat_signal_source.cc ${OPT_DRIVER_SOURCES} ) diff --git a/src/algorithms/signal_source/adapters/labsat_signal_source.cc b/src/algorithms/signal_source/adapters/labsat_signal_source.cc new file mode 100644 index 000000000..ead38e585 --- /dev/null +++ b/src/algorithms/signal_source/adapters/labsat_signal_source.cc @@ -0,0 +1,122 @@ +/*! + * \file labsat_signal_source.cc + * \brief Labsat 2 and 3 front-end signal sampler driver + * \author Javier Arribas, jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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 . + * + * ------------------------------------------------------------------------- + */ + +#include "labsat_signal_source.h" +#include +#include +#include +#include +#include "labsat23_source.h" +#include "configuration_interface.h" + + +using google::LogMessage; + +LabsatSignalSource::LabsatSignalSource(ConfigurationInterface* configuration, + std::string role, unsigned int in_stream, unsigned int out_stream, gr::msg_queue::sptr queue) : + role_(role), in_stream_(in_stream), out_stream_(out_stream), queue_(queue) +{ + std::string default_item_type = "gr_complex"; + std::string default_dump_file = "./data/source.bin"; + item_type_ = configuration->property(role + ".item_type", default_item_type); + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_file); + + int channel_selector=configuration->property(role + ".selected_channel", 1); + std::string default_filename = "./example_capture.LS3"; + + samples_ = configuration->property(role + ".samples", 0); + filename_ = configuration->property(role + ".filename", default_filename); + + if (item_type_.compare("gr_complex") == 0) + { + item_size_ = sizeof(gr_complex); + labsat23_source_ = labsat23_make_source(filename_.c_str(),channel_selector); + DLOG(INFO) << "Item size " << item_size_; + DLOG(INFO) << "labsat23_source_(" << labsat23_source_->unique_id() << ")"; + }else + { + LOG(WARNING) << item_type_ + << " unrecognized item type for LabSat source"; + item_size_ = sizeof(short); + } + if (dump_) + { + DLOG(INFO) << "Dumping output into file " << dump_filename_; + file_sink_ = gr::blocks::file_sink::make(item_size_, dump_filename_.c_str()); + } + if (dump_) + { + DLOG(INFO) << "file_sink(" << file_sink_->unique_id() << ")"; + } +} + + + +LabsatSignalSource::~LabsatSignalSource() +{} + + + +void LabsatSignalSource::connect(gr::top_block_sptr top_block) +{ + if (dump_) + { + top_block->connect(labsat23_source_, 0, file_sink_, 0); + DLOG(INFO) << "connected labsat23_source_ to file sink"; + } + else + { + DLOG(INFO) << "nothing to connect internally"; + } +} + + + +void LabsatSignalSource::disconnect(gr::top_block_sptr top_block) +{ + if (dump_) + { + top_block->disconnect(labsat23_source_, 0, file_sink_, 0); + } +} + + +gr::basic_block_sptr LabsatSignalSource::get_left_block() +{ + LOG(WARNING) << "Left block of a signal source should not be retrieved"; + return gr::block_sptr(); +} + + +gr::basic_block_sptr LabsatSignalSource::get_right_block() +{ + return labsat23_source_; +} diff --git a/src/algorithms/signal_source/adapters/labsat_signal_source.h b/src/algorithms/signal_source/adapters/labsat_signal_source.h new file mode 100644 index 000000000..6909acefc --- /dev/null +++ b/src/algorithms/signal_source/adapters/labsat_signal_source.h @@ -0,0 +1,94 @@ +/*! + * \file labsat_signal_source.h + * \brief Labsat 2 and 3 front-end signal sampler driver + * \author Javier Arribas, jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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 . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef LABSAT_SIGNAL_SOURCE_H_ +#define LABSAT_SIGNAL_SOURCE_H_ + +#include +#include +#include +#include +#include "gnss_block_interface.h" + + +class ConfigurationInterface; + +/*! + * \brief This class reads samples from a GN3S USB dongle, a RF front-end signal sampler + */ +class LabsatSignalSource: public GNSSBlockInterface +{ +public: + LabsatSignalSource(ConfigurationInterface* configuration, + std::string role, unsigned int in_stream, + unsigned int out_stream, gr::msg_queue::sptr queue); + + virtual ~LabsatSignalSource(); + + inline std::string role() override + { + return role_; + } + + /*! + * \brief Returns "LabsatSignalSource". + */ + inline std::string implementation() override + { + return "Labsat_Signal_Source"; + } + + inline size_t item_size() override + { + return item_size_; + } + + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; + +private: + std::string role_; + unsigned int in_stream_; + unsigned int out_stream_; + std::string item_type_; + size_t item_size_; + long samples_; + std::string filename_; + bool dump_; + std::string dump_filename_; + gr::block_sptr labsat23_source_; + gr::blocks::file_sink::sptr file_sink_; + boost::shared_ptr queue_; +}; + +#endif /*LABSAT_SIGNAL_SOURCE_H_*/ diff --git a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt index 67b9f3162..a9252edb0 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/signal_source/gnuradio_blocks/CMakeLists.txt @@ -24,6 +24,7 @@ set(SIGNAL_SOURCE_GR_BLOCKS_SOURCES rtl_tcp_signal_source_c.cc unpack_2bit_samples.cc unpack_spir_gss6450_samples.cc + labsat23_source.cc ) include_directories( diff --git a/src/algorithms/signal_source/gnuradio_blocks/labsat23_source.cc b/src/algorithms/signal_source/gnuradio_blocks/labsat23_source.cc new file mode 100644 index 000000000..375d246ab --- /dev/null +++ b/src/algorithms/signal_source/gnuradio_blocks/labsat23_source.cc @@ -0,0 +1,474 @@ +/*! + * \file labsat23_source.cc + * + * \brief Unpacks the Labsat 2 (ls2) and (ls3) capture files + * \author Javier Arribas jarribas (at) cttc.es + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors) + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "labsat23_source.h" +#include +#include + +labsat23_source_sptr labsat23_make_source(const char *signal_file_basename, int channel_selector) +{ + return labsat23_source_sptr(new labsat23_source(signal_file_basename, channel_selector)); +} + +std::string labsat23_source::generate_filename() +{ + std::ostringstream ss; + ss << std::setw(4) << std::setfill('0') << d_current_file_number; + return d_signal_file_basename + "_" + ss.str()+".LS3"; + +} + +labsat23_source::labsat23_source(const char *signal_file_basename, int channel_selector) : gr::block("labsat23_source", + gr::io_signature::make(0, 0, 0), + gr::io_signature::make(1, 1, sizeof(gr_complex))) +{ + if (channel_selector<1 or channel_selector>2) + { + std::cout<<"Labsat source config error: channel selection out of bounds, check gnss-sdr config file"<set_output_multiple(8); + signal_file=generate_filename(); + binary_input_file = new std::ifstream (signal_file.c_str(), std::ios::in|std::ios::binary); + + if (binary_input_file->is_open()) + { + std::cout<<"Labsat file source is reading samples from "<is_open()) + { + binary_input_file->close(); + } + delete binary_input_file; +} + +int labsat23_source::getBit(uint8_t byte, int position) +{ + return (byte >> position) & 0x01; +} + +void labsat23_source::decode_samples_one_channel(int16_t input_short, gr_complex* out, int type) +{ + std::bitset<16> bs(input_short); + switch(type) + { + case 2: + //two bits per sample, 8 samples per int16 + for (int i=0;i<8;i++) + { + out[i]=gr_complex(static_cast(bs[15-(2*i)]), + static_cast(bs[14-(2*i)])); + out[i]=out[i]*gr_complex(2,0)-gr_complex(1,1); + } + break; + case 4: + //four bits per sample, 4 samples per int16 + for (int i=0;i<4;i++) + { + out[i]=gr_complex(0.0,0.0); + //In-Phase + if (bs[15-4*i]) + { + if (bs[13-4*i]) //11 + { + out[i]+=gr_complex(-1,0); + } + else //10 + { + out[i]+=gr_complex(-2,0); + } + } + else{ + if (bs[13-4*i]) //01 + { + out[i]+=gr_complex(1,0); + } + } + + //Quadrature + if (bs[14-4*i]) + { + if (bs[12-4*i]) //11 + { + out[i]+=gr_complex(0,-1); + } + else //10 + { + out[i]+=gr_complex(0,-2); + } + } + else{ + if (bs[12-4*i]) //01 + { + out[i]+=gr_complex(0,1); + } + } + out[i]+=gr_complex(0.5,0.5); + } + break; + } +} + +int labsat23_source::general_work(int noutput_items, + __attribute__((unused)) gr_vector_int &ninput_items, + __attribute__((unused)) gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + + gr_complex *out = reinterpret_cast(output_items[0]); + + if (d_header_parsed==false) + { + + if (binary_input_file->eof()==false) + { + char memblock[1024]; + binary_input_file->read(memblock,1024); + //parse Labsat header + //check preamble + int byte_counter=0; + bool preamble_ok=true; + for (int i=0;i<8;i++) + { + if (memblock[byte_counter]!=0x00) preamble_ok=false; + //std::cout<<"H["<clear(); + binary_input_file->seekg(header_bytes, binary_input_file->beg); + return 0; + }else{ + std::cout<<"Labsat file header error: section 2 is not available."<0) + { + int16_t memblock[n_int16_to_read]; + binary_input_file->read((char*)memblock,n_int16_to_read*2); + n_int16_to_read=binary_input_file->gcount()/2; //from bytes to int16 + if (n_int16_to_read>0) + { + int output_pointer=0; + for (int i=0;iclose(); + binary_input_file->open(generate_filename().c_str(), std::ios::in|std::ios::binary); + if (binary_input_file->is_open()) + { + std::cout<<"Labsat file source is reading samples from "<0) + { + int16_t memblock[n_int16_to_read]; + binary_input_file->read((char*)memblock,n_int16_to_read*2); + n_int16_to_read=binary_input_file->gcount()/2; //from bytes to int16 + if (n_int16_to_read>0) + { + int output_pointer=0; + for (int i=0;iclose(); + binary_input_file->open(generate_filename().c_str(), std::ios::in|std::ios::binary); + if (binary_input_file->is_open()) + { + std::cout<<"Labsat file source is reading samples from "<. + * + * ------------------------------------------------------------------------- + */ + +#ifndef LABSAT23_SOURCE_H +#define LABSAT23_SOURCE_H + +#include +#include +#include +#include +#include + +class labsat23_source; + +typedef boost::shared_ptr labsat23_source_sptr; + +labsat23_source_sptr labsat23_make_source(const char *signal_file_basename, int channel_selector); + +/*! + * \brief This class implements conversion between Labsat2 and 3 format byte packet samples to gr_complex + */ +class labsat23_source: public gr::block +{ +private: + friend labsat23_source_sptr labsat23_make_source_sptr(const char *signal_file_basename, int channel_selector); + std::string generate_filename(); + void decode_samples_one_channel(int16_t input_short, gr_complex* out, int type); + int getBit(uint8_t byte, int position); + bool d_header_parsed; + uint8_t d_channel_selector; + int d_channel_selector_config; + int d_current_file_number; + uint8_t d_labsat_version; + std::string d_signal_file_basename; + std::ifstream *binary_input_file; + uint8_t d_ref_clock; + uint8_t d_bits_per_sample; + +public: + labsat23_source(const char *signal_file_basename, int channel_selector); + ~labsat23_source(); + 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 diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 322577e62..855b192a3 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -51,6 +51,7 @@ #include "spir_gss6450_file_signal_source.h" #include "rtl_tcp_signal_source.h" #include "two_bit_packed_file_signal_source.h" +#include "labsat_signal_source.h" #include "channel.h" #include "signal_conditioner.h" @@ -896,6 +897,21 @@ std::unique_ptr GNSSBlockFactory::GetBlock( block = std::move(block_); } + catch (const std::exception &e) + { + std::cout << "GNSS-SDR program ended." << std::endl; + exit(1); + } + } + else if (implementation.compare("Labsat_Signal_Source") == 0) + { + try + { + std::unique_ptr block_(new LabsatSignalSource(configuration.get(), role, in_streams, + out_streams, queue)); + block = std::move(block_); + } + catch (const std::exception &e) { std::cout << "GNSS-SDR program ended." << std::endl; From a5d65f3424537e9e432cc32768fdb4a299fc75c3 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 23 Jan 2018 17:33:06 +0100 Subject: [PATCH 54/55] Bug fix: adding exception control to std::map access in observables block --- .../gnuradio_blocks/hybrid_observables_cc.cc | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index efd29e092..66cd0fce0 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -453,6 +453,7 @@ int hybrid_observables_cc::general_work (int noutput_items , d_gnss_synchro_history_queue[i].at(distance - 1))); } } + } else { @@ -478,7 +479,8 @@ int hybrid_observables_cc::general_work (int noutput_items , // compute interpolated TOW value at T_rx_s int ref_channel_key = gnss_synchro_map_iter->second.Channel_ID; - Gnss_Synchro adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); + Gnss_Synchro adj_obs; + adj_obs = adjacent_gnss_synchro_map.at(ref_channel_key); double ref_adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / ref_fs_hz + adj_obs.Code_phase_samples / ref_fs_hz; double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; @@ -504,7 +506,12 @@ int hybrid_observables_cc::general_work (int noutput_items , // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) // TOW at the selected receiver time T_rx_s int element_key = gnss_synchro_map_iter->second.Channel_ID; - adj_obs = adjacent_gnss_synchro_map.at(element_key); + try{ + adj_obs = adjacent_gnss_synchro_map.at(element_key); + }catch(const std::exception & ex) + { + continue; + } double adj_T_rx_s = static_cast(adj_obs.Tracking_sample_counter) / channel_fs_hz + adj_obs.Code_phase_samples / channel_fs_hz; From 114121ef510b4f52cedf4519b79db204a65a9164 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 23 Jan 2018 17:36:32 +0100 Subject: [PATCH 55/55] Addign some useful matlab binary file reading functions --- src/utils/matlab/libs/read_complex_binary.m | 57 +++++++++++++++++++ .../matlab/libs/read_complex_char_binary.m | 48 ++++++++++++++++ .../matlab/libs/read_complex_short_binary.m | 48 ++++++++++++++++ 3 files changed, 153 insertions(+) create mode 100644 src/utils/matlab/libs/read_complex_binary.m create mode 100644 src/utils/matlab/libs/read_complex_char_binary.m create mode 100644 src/utils/matlab/libs/read_complex_short_binary.m diff --git a/src/utils/matlab/libs/read_complex_binary.m b/src/utils/matlab/libs/read_complex_binary.m new file mode 100644 index 000000000..a201b99bb --- /dev/null +++ b/src/utils/matlab/libs/read_complex_binary.m @@ -0,0 +1,57 @@ +% +% Copyright 2001 Free Software Foundation, Inc. +% +% This file is part of GNU Radio +% +% GNU Radio 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, or (at your option) +% any later version. +% +% GNU Radio 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 GNU Radio; see the file COPYING. If not, write to +% the Free Software Foundation, Inc., 51 Franklin Street, +% Boston, MA 02110-1301, USA. +% + +function v = read_complex_binary (filename, count, start_sample) + + %% usage: read_complex_binary (filename, [count], [start_sample]) + %% + %% open filename and return the contents as a column vector, + %% treating them as 32 bit complex numbers + %% + + m = nargchk (1,2,nargin); + if (m) + %usage (m); + end + + if (nargin < 2) + count = Inf; + start_sample=0; + end + + if (nargin < 3) + start_sample=0; + end + + f = fopen (filename, 'rb'); + if (f < 0) + v = 0; + else + if (start_sample>0) + bytes_per_sample=4; + fseek(f,start_sample*bytes_per_sample,'bof'); + end + t = fread (f, [2, count], 'float'); + fclose (f); + v = t(1,:) + t(2,:)*i; + [r, c] = size (v); + v = reshape (v, c, r); + end diff --git a/src/utils/matlab/libs/read_complex_char_binary.m b/src/utils/matlab/libs/read_complex_char_binary.m new file mode 100644 index 000000000..cafaedcc7 --- /dev/null +++ b/src/utils/matlab/libs/read_complex_char_binary.m @@ -0,0 +1,48 @@ +% +% Copyright 2001 Free Software Foundation, Inc. +% +% This file is part of GNU Radio +% +% GNU Radio 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, or (at your option) +% any later version. +% +% GNU Radio 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 GNU Radio; see the file COPYING. If not, write to +% the Free Software Foundation, Inc., 51 Franklin Street, +% Boston, MA 02110-1301, USA. +% + +function v = read_complex_char_binary (filename, count) + + %% usage: read_complex_binary (filename, [count]) + %% + %% open filename and return the contents as a column vector, + %% treating them as 32 bit complex numbers + %% + + m = nargchk (1,2,nargin); + if (m) + usage (m); + end + + if (nargin < 2) + count = Inf; + end + + f = fopen (filename, 'rb'); + if (f < 0) + v = 0; + else + t = fread (f, [2, count], 'int8'); + fclose (f); + v = t(1,:) + t(2,:)*i; + [r, c] = size (v); + v = reshape (v, c, r); + end diff --git a/src/utils/matlab/libs/read_complex_short_binary.m b/src/utils/matlab/libs/read_complex_short_binary.m new file mode 100644 index 000000000..55118528b --- /dev/null +++ b/src/utils/matlab/libs/read_complex_short_binary.m @@ -0,0 +1,48 @@ +% +% Copyright 2001 Free Software Foundation, Inc. +% +% This file is part of GNU Radio +% +% GNU Radio 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, or (at your option) +% any later version. +% +% GNU Radio 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 GNU Radio; see the file COPYING. If not, write to +% the Free Software Foundation, Inc., 51 Franklin Street, +% Boston, MA 02110-1301, USA. +% + +function v = read_complex_short_binary (filename, count) + + %% usage: read_complex_binary (filename, [count]) + %% + %% open filename and return the contents as a column vector, + %% treating them as 32 bit complex numbers + %% + + m = nargchk (1,2,nargin); + if (m) + usage (m); + end + + if (nargin < 2) + count = Inf; + end + + f = fopen (filename, 'rb'); + if (f < 0) + v = 0; + else + t = fread (f, [2, count], 'short'); + fclose (f); + v = t(1,:) + t(2,:)*i; + [r, c] = size (v); + v = reshape (v, c, r); + end