diff --git a/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.cc b/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.cc index 4eb7d53cf..60bd54858 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.cc +++ b/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.cc @@ -17,6 +17,9 @@ #include "fifo_reader.h" #include +#include + +const int FIFO_SIZE = 1472000; // Value taken from gr_complex_ip_packet_source, is it optimal? // initial construction; pass to private constructor FifoReader::sptr FifoReader::make(const std::string &file_name, const std::string &sample_type) @@ -30,7 +33,10 @@ FifoReader::FifoReader(const std::string &file_name, const std::string &sample_t gr::io_signature::make(0, 0, 0), // no input gr::io_signature::make(1, 1, sizeof(gr_complex))), // <+MIN_OUT+>, <+MAX_OUT+>, sizeof(<+OTYPE+>) file_name_(file_name), - sample_type_(sample_type) + sample_type_(sample_type), + fifo_buffer_(new char[FIFO_SIZE]), + buffer_idx_(0), + buffer_size_(0) { DLOG(INFO) << "Starting FifoReader"; } @@ -61,12 +67,14 @@ int FifoReader::work(int noutput_items, size_t items_retrieved = 0; if (sample_type_ == "ishort") { - // ishort == int16_t items_retrieved = read_interleaved(noutput_items, output_items); } + else if (sample_type_ == "ibyte") // Does this also work with cbyte? + { + items_retrieved = read_interleaved(noutput_items, output_items); + } else if (sample_type_ == "gr_complex") { - LOG(WARNING) << sample_type_ << " is not yet tested. Please consider removing this warning if tested successfully"; items_retrieved = read_gr_complex(noutput_items, output_items); } else @@ -80,31 +88,92 @@ int FifoReader::work(int noutput_items, return this->WORK_CALLED_PRODUCE; } -// read gr_complex items from fifo -// this fct has duplicate code with the templated read_interleaved fct in header -size_t FifoReader::read_gr_complex(int noutput_items, gr_vector_void_star &output_items) + +template +size_t FifoReader::read_interleaved(int noutput_items, gr_vector_void_star &output_items) { + boost::mutex::scoped_lock lock(d_mutex); // hold mutex for duration of this function size_t items_retrieved = 0; - for (int n = 0; n < noutput_items; n++) + int n; + Type real; + Type imag; + + if (buffer_idx_ >= buffer_size_) { - gr_complex sample; - fifo_.read(reinterpret_cast(&sample), sizeof(sample)); + fifo_.read(fifo_buffer_, FIFO_SIZE); + buffer_idx_ = 0; if (fifo_.good()) { - static_cast(output_items.at(0))[n] = sample; - items_retrieved++; + buffer_size_ = FIFO_SIZE; } else if (fifo_.eof()) { + // Although we got an EOF, ensure we don't lose the other samples + buffer_size_ = fifo_.gcount(); fifo_.clear(); - break; } else { fifo_error_output(); - break; + return 0; } } + + for (n = 0; n < noutput_items && buffer_idx_ < buffer_size_; n++) + { + memcpy(&real, &fifo_buffer_[buffer_idx_], sizeof(real)); + memcpy(&imag, &fifo_buffer_[buffer_idx_ + sizeof(imag)], sizeof(imag)); + static_cast(output_items.at(0))[n] = gr_complex(real, imag); + + buffer_idx_ += 2 * sizeof(Type); + } + + items_retrieved = n; + + return items_retrieved; +} + + +// read gr_complex items from fifo +// this fct has duplicate code with the templated read_interleaved fct above +size_t FifoReader::read_gr_complex(int noutput_items, gr_vector_void_star &output_items) +{ + boost::mutex::scoped_lock lock(d_mutex); // hold mutex for duration of this function + size_t items_retrieved = 0; + int n; + gr_complex sample; + + if (buffer_idx_ >= buffer_size_) + { + fifo_.read(fifo_buffer_, FIFO_SIZE); + buffer_idx_ = 0; + if (fifo_.good()) + { + buffer_size_ = FIFO_SIZE; + } + else if (fifo_.eof()) + { + // Although we got an EOF, ensure we don't lose the other samples + buffer_size_ = fifo_.gcount(); + fifo_.clear(); + } + else + { + fifo_error_output(); + return 0; + } + } + + for (n = 0; n < noutput_items && buffer_idx_ < buffer_size_; n++) + { + memcpy(&sample, &fifo_buffer_[buffer_idx_], sizeof(gr_complex)); + static_cast(output_items.at(0))[n] = sample; + + buffer_idx_ += sizeof(gr_complex); + } + + items_retrieved = n; + return items_retrieved; } @@ -112,3 +181,9 @@ void FifoReader::fifo_error_output() const { LOG(ERROR) << "unhandled FIFO event"; } + + +FifoReader::~FifoReader() +{ + delete[] fifo_buffer_; +} diff --git a/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.h b/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.h index 9e289ebf5..c90a80448 100644 --- a/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.h +++ b/src/algorithms/signal_source/gnuradio_blocks/fifo_reader.h @@ -19,6 +19,7 @@ #define GNSS_SDR_FIFO_READER_H_ #include "gnss_block_interface.h" +#include #include #include // std::ifstream #include @@ -34,8 +35,6 @@ public: using sptr = gnss_shared_ptr; static sptr make(const std::string &file_name, const std::string &sample_type); - ~FifoReader() = default; - //! initialize istream resource for FIFO bool start(); @@ -50,41 +49,16 @@ private: //! (gr handles this with public and private header pair) FifoReader(const std::string &file_name, const std::string &sample_type); + //! \brief Destructor + //! private destructor + ~FifoReader(); + size_t read_gr_complex(int noutput_items, gr_vector_void_star &output_items); + //! function to read data out of FIFO which is stored as interleaved I/Q stream. //! template argument determines sample_type template - size_t read_interleaved(int noutput_items, gr_vector_void_star &output_items) - { - size_t items_retrieved = 0; - for (int n = 0; n < noutput_items; n++) - { - // TODO: try if performance increases if we copy larger chunks to vector. - // how to read from stream: https://en.cppreference.com/w/cpp/io/basic_ifstream - std::array buffer; // gr_complex is 32bit = 4*char - fifo_.read(reinterpret_cast(&buffer[0]), buffer.size()); - if (fifo_.good()) - { - Type real; - Type imag; - memcpy(&real, &buffer[0], sizeof(real)); - memcpy(&imag, &buffer[2], sizeof(imag)); - static_cast(output_items.at(0))[n] = gr_complex(real, imag); - items_retrieved++; - } - else if (fifo_.eof()) - { - fifo_.clear(); - break; - } - else - { - fifo_error_output(); - break; - } - } - return items_retrieved; - } + size_t read_interleaved(int noutput_items, gr_vector_void_star &output_items); //! this function moves logging output from this header into the source file //! thereby eliminating the need to include glog/logging.h in this header @@ -93,6 +67,9 @@ private: const std::string file_name_; const std::string sample_type_; std::ifstream fifo_; + char *fifo_buffer_; + int buffer_idx_, buffer_size_; + boost::mutex d_mutex; }; /** \} */