diff options
Diffstat (limited to 'gr-gmr1/lib')
| -rw-r--r-- | gr-gmr1/lib/CMakeLists.txt | 77 | ||||
| -rw-r--r-- | gr-gmr1/lib/cxvec_compat.h | 43 | ||||
| -rw-r--r-- | gr-gmr1/lib/gsmtap_sink_impl.cc | 98 | ||||
| -rw-r--r-- | gr-gmr1/lib/gsmtap_sink_impl.h | 57 | ||||
| -rw-r--r-- | gr-gmr1/lib/rach_demod_impl.cc | 243 | ||||
| -rw-r--r-- | gr-gmr1/lib/rach_demod_impl.h | 62 | ||||
| -rw-r--r-- | gr-gmr1/lib/rach_detect_fft_impl.cc | 399 | ||||
| -rw-r--r-- | gr-gmr1/lib/rach_detect_fft_impl.h | 97 |
8 files changed, 1076 insertions, 0 deletions
diff --git a/gr-gmr1/lib/CMakeLists.txt b/gr-gmr1/lib/CMakeLists.txt new file mode 100644 index 0000000..9f7573a --- /dev/null +++ b/gr-gmr1/lib/CMakeLists.txt @@ -0,0 +1,77 @@ +# Copyright 2011 Free Software Foundation, Inc. +# +# 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. + +######################################################################## +# Setup library +######################################################################## +include(GrPlatform) #define LIB_SUFFIX + +include_directories(${Boost_INCLUDE_DIR}) +link_directories(${Boost_LIBRARY_DIRS}) + +list(APPEND gmr1_sources + ../../src/sdr/dkab.c + ../../src/sdr/fcch.c + ../../src/sdr/nb.c + ../../src/sdr/pi4cxpsk.c + ../../src/l1/a5.c + ../../src/l1/bcch.c + ../../src/l1/ccch.c + ../../src/l1/conv.c + ../../src/l1/crc.c + ../../src/l1/facch3.c + ../../src/l1/facch9.c + ../../src/l1/interleave.c + ../../src/l1/punct.c + ../../src/l1/rach.c + ../../src/l1/scramb.c + ../../src/l1/tch3.c + ../../src/l1/tch9.c + ../../src/gsmtap.c + gsmtap_sink_impl.cc + rach_demod_impl.cc + rach_detect_fft_impl.cc +) + +add_library(gnuradio-gmr1 SHARED ${gmr1_sources}) +target_link_libraries(gnuradio-gmr1 + ${Boost_LIBRARIES} + ${FFTW3F_LIBRARIES} + ${LIBOSMOCORE_LIBRARIES} + ${LIBOSMODSP_LIBRARIES} + ${GNURADIO_ALL_LIBRARIES} +) +set_target_properties(gnuradio-gmr1 PROPERTIES DEFINE_SYMBOL "gnuradio_gmr1_EXPORTS") + +######################################################################## +# Install built library files +######################################################################## +install(TARGETS gnuradio-gmr1 + LIBRARY DESTINATION lib${LIB_SUFFIX} # .so/.dylib file + ARCHIVE DESTINATION lib${LIB_SUFFIX} # .lib file + RUNTIME DESTINATION bin # .dll file +) + +######################################################################## +# Build and register unit test +######################################################################## +find_package(Boost COMPONENTS unit_test_framework) + +include(GrTest) +set(GR_TEST_TARGET_DEPS gnuradio-gmr1) +#turn each test cpp file into an executable with an int main() function +add_definitions(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN) diff --git a/gr-gmr1/lib/cxvec_compat.h b/gr-gmr1/lib/cxvec_compat.h new file mode 100644 index 0000000..8457b89 --- /dev/null +++ b/gr-gmr1/lib/cxvec_compat.h @@ -0,0 +1,43 @@ +/* -*- c++ -*- */ +/* + * Copyright 2014 Sylvain Munaut <tnt@246tNt.com> + * + * This 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. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifndef INCLUDED_GR_GMR1_CXVEC_COMPAT_H +#define INCLUDED_GR_GMR1_CXVEC_COMPAT_H + +#include <complex.h> +#include <gnuradio/types.h> + +#define GCC_VERSION ( \ + __GNUC__ * 10000 + \ + __GNUC_MINOR__ * 100 + \ + __GNUC_PATCHLEVEL__ \ +) + +#if GCC_VERSION >= 40800 +# define complex _Complex +# undef _GLIBCXX_HAVE_COMPLEX_H +#endif + +extern "C" { +#include <osmocom/dsp/cxvec.h> +#include <osmocom/dsp/cxvec_math.h> +} + +#endif /* INCLUDED_GR_GMR1_CXVEC_COMPAT_H */ diff --git a/gr-gmr1/lib/gsmtap_sink_impl.cc b/gr-gmr1/lib/gsmtap_sink_impl.cc new file mode 100644 index 0000000..6a53f8a --- /dev/null +++ b/gr-gmr1/lib/gsmtap_sink_impl.cc @@ -0,0 +1,98 @@ +/* -*- c++ -*- */ +/* + * Copyright 2014 Sylvain Munaut <tnt@246tNt.com> + * + * This 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. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <string.h> + +#include <gnuradio/io_signature.h> + +#include "gsmtap_sink_impl.h" + +extern "C" { +#include <osmocom/core/gsmtap.h> +#include <osmocom/core/gsmtap_util.h> +#include <osmocom/gmr1/gsmtap.h> +} + + +namespace gr { + namespace gmr1 { + +gsmtap_sink::sptr +gsmtap_sink::make(const std::string &host, uint16_t port) +{ + return gnuradio::get_initial_sptr( + new gsmtap_sink_impl(host, port) + ); +} + +gsmtap_sink_impl::gsmtap_sink_impl(const std::string &host, uint16_t port) + : gr::block("gsmtap_sink", + io_signature::make(0, 0, 0), + io_signature::make(0, 0, 0)), + d_host(host), d_port(port) +{ + this->d_gti = gsmtap_source_init(host.c_str(), port, 0); + gsmtap_source_add_sink(this->d_gti); + + message_port_register_in(PDU_PORT_ID); + set_msg_handler(PDU_PORT_ID, boost::bind(&gsmtap_sink_impl::send_pdu, this, _1)); +} + +gsmtap_sink_impl::~gsmtap_sink_impl() +{ + /* FIXME: No destroy */ +} + + +std::string +gsmtap_sink_impl::host() const +{ + return this->d_host; +} + +uint16_t +gsmtap_sink_impl::port() const +{ + return this->d_port; +} + + +void +gsmtap_sink_impl::send_pdu(pmt::pmt_t pdu) +{ + pmt::pmt_t meta = pmt::car(pdu); + pmt::pmt_t vector = pmt::cdr(pdu); + size_t len = pmt::length(vector); + size_t offset(0); + const uint8_t* rach = (const uint8_t*) pmt::uniform_vector_elements(vector, offset); + + /* Send to GSMTap */ + gsmtap_sendmsg(this->d_gti, gmr1_gsmtap_makemsg( + GSMTAP_GMR1_RACH, + 0, 0, rach, len + )); +} + + } // namespace gmr1 +} // namespace gr diff --git a/gr-gmr1/lib/gsmtap_sink_impl.h b/gr-gmr1/lib/gsmtap_sink_impl.h new file mode 100644 index 0000000..21bcc03 --- /dev/null +++ b/gr-gmr1/lib/gsmtap_sink_impl.h @@ -0,0 +1,57 @@ +/* -*- c++ -*- */ +/* + * Copyright 2014 Sylvain Munaut <tnt@246tNt.com> + * + * This 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. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + + +#ifndef INCLUDED_GR_GMR1_GSMTAP_SINK_IMPL_H +#define INCLUDED_GR_GMR1_GSMTAP_SINK_IMPL_H + +#include <gnuradio/gmr1/gsmtap_sink.h> + +struct gsmtap_inst; + +namespace gr { + namespace gmr1 { + + /*! + * \brief + * \ingroup gmr1 + */ + class gsmtap_sink_impl : public gsmtap_sink + { + private: + std::string d_host; + uint16_t d_port; + + struct gsmtap_inst *d_gti; + + void send_pdu(pmt::pmt_t pdu); + + public: + gsmtap_sink_impl(const std::string &host, uint16_t port); + virtual ~gsmtap_sink_impl(); + + std::string host() const; + uint16_t port() const; + }; + + } // namespace gmr1 +} // namespace gr + +#endif /* INCLUDED_GR_GMR1_GSMTAP_SINK_IMPL_H */ diff --git a/gr-gmr1/lib/rach_demod_impl.cc b/gr-gmr1/lib/rach_demod_impl.cc new file mode 100644 index 0000000..8d0960c --- /dev/null +++ b/gr-gmr1/lib/rach_demod_impl.cc @@ -0,0 +1,243 @@ +/* -*- c++ -*- */ +/* + * Copyright 2014 Sylvain Munaut <tnt@246tNt.com> + * + * This 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. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <stdio.h> + +#include <gnuradio/io_signature.h> + +#include "rach_demod_impl.h" + + +#include "cxvec_compat.h" + +extern "C" { +#include <osmocom/core/bits.h> +#include <osmocom/gmr1/l1/rach.h> +#include <osmocom/gmr1/sdr/pi4cxpsk.h> +#include <osmocom/gmr1/sdr/nb.h> +} + + +namespace gr { + namespace gmr1 { + +rach_demod::sptr +rach_demod::make(int sps, int etoa, const std::string& len_tag_key) +{ + return gnuradio::get_initial_sptr( + new rach_demod_impl(sps, etoa, len_tag_key) + ); +} + +rach_demod_impl::rach_demod_impl(int sps, int etoa, const std::string& len_tag_key) + : gr::tagged_stream_block("rach_demod", + io_signature::make(1, 1, sizeof(gr_complex)), + io_signature::make(0, 0, 0), + len_tag_key), + d_sps(sps), d_etoa(etoa) +{ + message_port_register_out(PDU_PORT_ID); +} + +rach_demod_impl::~rach_demod_impl() +{ + /* Nothing to do */ +} + + +int +rach_demod_impl::sps() const +{ + return this->d_sps; +} + +int +rach_demod_impl::etoa() const +{ + return this->d_etoa; +} + + +/* + * Attempts to detect a burst assuming a given TOA + */ +float +rach_demod_impl::estimate(struct osmo_cxvec *burst, int etoa, float *cw_freq) +{ + const int guard = 2 * this->d_sps; + struct osmo_cxvec _cw1, *cw1 = &_cw1; + struct osmo_cxvec _cw2, *cw2 = &_cw2; + float complex c1, c2; + float r; + int i; + + /* Map the CW1 and CW2 part of the sync sequence */ + osmo_cxvec_init_from_data(cw1, + burst->data + etoa + this->d_sps * 127, + this->d_sps * 32 + ); + + osmo_cxvec_init_from_data(cw2, + burst->data + etoa + this->d_sps * 191, + this->d_sps * 32 + ); + + /* Compute the average rotation speed per sample of cw1 & cw2 */ + r = 0.0f; + + for (i=guard; i<(cw1->len-1-guard); i++) + { + r += cargf(cw1->data[i+1] * conjf(cw1->data[i])); + r += cargf(cw2->data[i+1] * conjf(cw2->data[i])); + } + + r /= 2.0f * (cw1->len-1-2*guard); + + /* Compute the correlation value using that speed to revert rotation */ + c1 = c2 = 0.0f; + + for (i=0; i<cw1->len; i++) + { + float complex e = cexpf(I * i * -r); + c1 += cw1->data[i] * e; + c2 += cw2->data[i] * e; + } + + /* Return the power & cw freq */ + *cw_freq = r; + + return cabsf(c1) + cabsf(c2); +} + +int +rach_demod_impl::process(struct osmo_cxvec *burst, float freq_corr, uint8_t *rach, uint8_t *sb_mask) +{ + sbit_t ebits[494]; + int sync_id; + float toa, freq_err; + int crc[2], conv; + int rv; + + /* Demodulate to soft bits */ + rv = gmr1_pi4cxpsk_demod( + &gmr1_rach_burst, + burst, this->d_sps, freq_corr, + ebits, &sync_id, &toa, &freq_err + ); + + /* Decode */ + rv = gmr1_rach_decode(rach, ebits, 0x00, &conv, crc, sb_mask); + + /* Debug */ + //printf("rv: %d | crc: %d %d | conv: %d | sb_mask: %02hhx\n", rv, crc[0], crc[1], conv, *sb_mask); + + return crc[1]; +} + + +int +rach_demod_impl::work(int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); + struct osmo_cxvec _burst, *burst = &_burst; + uint8_t rach[18], sb_mask; + float peak_corr, peak_cw_freq; + int ws, peak_etoa, etoa; + int rv; + + /* Init a burst with the data */ + osmo_cxvec_init_from_data(burst, (float complex *)in, ninput_items[0]); + ws = burst->len - gmr1_rach_burst.len * this->d_sps + 1; + + /* Scan all possible TOA for the best fit */ + peak_corr = 0.0f; + + for (etoa=0; etoa<ws; etoa++) + { + float cw_freq, corr; + + corr = this->estimate(burst, etoa, &cw_freq); + + if (corr > peak_corr) { + peak_corr = corr; + peak_etoa = etoa; + peak_cw_freq = cw_freq; + } + } + + /* Narrow down the window to 6 symbols around position we found */ + if (peak_etoa < (3 * this->d_sps)) + peak_etoa = 3 * this->d_sps; + if (peak_etoa > (ws - 3 * this->d_sps)) + peak_etoa = ws - 3 * this->d_sps; + + burst->data += peak_etoa - 3 * this->d_sps; + burst->len = (gmr1_rach_burst.len + 6) * this->d_sps; + + /* Attempt demodulation and decoding */ + rv = this->process(burst, -((d_sps * peak_cw_freq) - (M_PIf / 4.0f)), rach, &sb_mask); + + /* Send as PDU */ + if (!rv) + { + std::vector<tag_t> tags; + std::vector<tag_t>::iterator tags_itr; + pmt::pmt_t pdu_meta, pdu_vector, msg; + + /* Grab the tags into a dict */ + get_tags_in_range(tags, 0, + nitems_read(0), + nitems_read(0) + ninput_items[0] + ); + + pdu_meta = pmt::make_dict(); + for (tags_itr = tags.begin(); tags_itr != tags.end(); tags_itr++) { + pdu_meta = dict_add(pdu_meta, (*tags_itr).key, (*tags_itr).value); + } + + /* Add the guessed SB_Mask */ + pdu_meta = dict_add(pdu_meta, + pmt::string_to_symbol("sb_mask"), + pmt::from_long(sb_mask) + ); + + /* Build vector with the data bytes */ + pdu_vector = blocks::pdu::make_pdu_vector(blocks::pdu::byte_t, rach, 18); + + /* Builds message */ + msg = pmt::cons(pdu_meta, pdu_vector); + + /* Send it out */ + message_port_pub(PDU_PORT_ID, msg); + } + + /* Consume the burst */ + return ninput_items[0]; +} + + } // namespace gmr1 +} // namespace gr diff --git a/gr-gmr1/lib/rach_demod_impl.h b/gr-gmr1/lib/rach_demod_impl.h new file mode 100644 index 0000000..64d1afb --- /dev/null +++ b/gr-gmr1/lib/rach_demod_impl.h @@ -0,0 +1,62 @@ +/* -*- c++ -*- */ +/* + * Copyright 2014 Sylvain Munaut <tnt@246tNt.com> + * + * This 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. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + + +#ifndef INCLUDED_GR_GMR1_RACH_DEMOD_IMPL_H +#define INCLUDED_GR_GMR1_RACH_DEMOD_IMPL_H + +#include <gnuradio/gmr1/rach_demod.h> + +struct osmo_cxvec; + +namespace gr { + namespace gmr1 { + + /*! + * \brief + * \ingroup gmr1 + */ + class rach_demod_impl : public rach_demod + { + private: + int d_sps; + int d_etoa; + + float estimate(struct osmo_cxvec *burst, int etoa, float *cw_freq); + int process(struct osmo_cxvec *burst, float freq_corr, uint8_t *rach, uint8_t *sb_mask); + + public: + rach_demod_impl(int sps, int etoa, + const std::string& len_tag_key); + virtual ~rach_demod_impl(); + + int sps() const; + int etoa() const; + + int work(int noutput_items, + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + }; + + } // namespace gmr1 +} // namespace gr + +#endif /* INCLUDED_GR_GMR1_RACH_DEMOD_IMPL_H */ diff --git a/gr-gmr1/lib/rach_detect_fft_impl.cc b/gr-gmr1/lib/rach_detect_fft_impl.cc new file mode 100644 index 0000000..5f7d047 --- /dev/null +++ b/gr-gmr1/lib/rach_detect_fft_impl.cc @@ -0,0 +1,399 @@ +/* -*- c++ -*- */ +/* + * Copyright 2014 Sylvain Munaut <tnt@246tNt.com> + * + * This 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. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include <stdio.h> + +#include <gnuradio/io_signature.h> +#include <gnuradio/fft/window.h> +#include <volk/volk.h> + +#include "rach_detect_fft_impl.h" + + +namespace gr { + namespace gmr1 { + +rach_detect_fft::sptr +rach_detect_fft::make( + int fft_size, int overlap_ratio, float threshold, + int burst_length, int burst_offset, float freq_offset, + const std::string& len_tag_key) +{ + return gnuradio::get_initial_sptr( + new rach_detect_fft_impl( + fft_size, overlap_ratio, threshold, + burst_length, burst_offset, freq_offset, + len_tag_key + ) + ); +} + +rach_detect_fft_impl::rach_detect_fft_impl( + int fft_size, int overlap_ratio, float threshold, + int burst_length, int burst_offset, float freq_offset, + const std::string& len_tag_key) + : gr::block("rach_detect_fft", + io_signature::make(1, 1, sizeof(gr_complex)), + io_signature::make(1, 1, sizeof(gr_complex))), + d_fft_size(fft_size), d_overlap_ratio(overlap_ratio), + d_threshold(threshold), + d_burst_length(burst_length), d_burst_offset(burst_offset), + d_freq_offset(freq_offset), + d_freq_tag_key(pmt::string_to_symbol("freq")), + d_len_tag_key(pmt::string_to_symbol(len_tag_key)), + d_burst_length_pmt(pmt::from_long(burst_length)) +{ + this->d_fft = new gr::fft::fft_complex(this->d_fft_size, true, 1); + + this->d_buf = (gr_complex *) volk_malloc(this->d_fft_size * sizeof(gr_complex), 128); + this->d_win = (float *) volk_malloc(this->d_fft_size * sizeof(float), 128); + this->d_pwr = (float *) volk_malloc(this->d_fft_size * sizeof(float), 128); + this->d_avg = (float *) volk_malloc(this->d_fft_size * sizeof(float), 128); + + memset(this->d_avg, 0x00, this->d_fft_size * sizeof(float)); + + this->d_in_pos = 0; + this->d_out_pos = 0; + + std::vector<float> win = gr::fft::window::blackmanharris(this->d_fft_size); + memcpy(this->d_win, &win[0], this->d_fft_size * sizeof(float)); + + this->set_history(burst_length + 1); +} + +rach_detect_fft_impl::~rach_detect_fft_impl() +{ + volk_free(this->d_avg); + volk_free(this->d_pwr); + volk_free(this->d_win); + volk_free(this->d_buf); + + delete this->d_fft; +} + + +void +rach_detect_fft_impl::peak_detect(uint64_t position) +{ + std::vector<peak>::iterator it1, it2; + const int avg_hwin = 15; + const int avg_win = (avg_hwin * 2) + 1; + float sum; + int i; + + /* Prime the moving average */ + sum = 0.0f; + for (i=0; i<avg_win; i++) + sum += this->d_pwr[i]; + + /* Do a scan and compare with avg with peak */ + for (i=avg_hwin; i<this->d_fft_size-avg_hwin; i++) + { + /* Is this a peak ? */ + if (this->d_pwr[i] > (this->d_threshold * sum / (float)avg_win) && + this->d_pwr[i] > (this->d_threshold * this->d_avg[i])) + { + bool merged = false; + + /* Attempt merge with existing */ + for (it1=this->d_peaks_l1.begin(); it1!=this->d_peaks_l1.end() && !merged; it1++) + { + merged |= it1->merge(position, i); + } + +#if 0 + printf("%lld %d %d (%f %f %f)\n", position, i, merged, this->d_pwr[i], sum / (float)avg_win, this->d_avg[i]); +#endif + + /* No match, insert new peak */ + if (!merged) + this->d_peaks_l1.push_back(peak(position, i)); + } + + /* Update moving average */ + sum += this->d_pwr[i+avg_hwin+1] - this->d_pwr[i-avg_hwin]; + } + + /* Scan for expired peak at Level 1 */ + for (it1=this->d_peaks_l1.begin(); it1!=this->d_peaks_l1.end();) + { + /* Expired ? */ + if (it1->expired(position - 20)) + { + bool matched = false; + + /* Scan Level 2 for a match at the right distance */ + for (it2=this->d_peaks_l2.begin(); it2 != this->d_peaks_l2.end() && !matched; it2++) + { + if ((fabsf(it1->bin() - it2->bin()) < 1.0f) && + (it1->d_time[1] - it2->d_time[0] <= 4 * this->d_overlap_ratio) && + (it1->d_time[1] - it2->d_time[0] >= 2 * this->d_overlap_ratio)) + { + matched = true; + +#if 0 + printf("Match: (%llu %llu %d %d) (%llu %llu %d %d)\n", + it1->d_time[0], it1->d_time[1], it1->d_bin[0], it1->d_bin[1], + it2->d_time[0], it2->d_time[1], it2->d_bin[0], it2->d_bin[1] + ); +#endif + + this->d_peaks_pending.push_back(*it1); + this->d_peaks_l2.erase(it2); + } + } + + /* No match, insert in Level 2 */ + if (!matched) + this->d_peaks_l2.push_back(*it1); + + /* Remove from Level 1 */ + it1 = this->d_peaks_l1.erase(it1); + } else { + /* Just move on */ + it1++; + } + } + + /* Scan for expired peaks at Level 2 */ + for (it1=this->d_peaks_l2.begin(); it1!=this->d_peaks_l2.end();) + { + if (it1->expired(position - 40)) { + it1 = this->d_peaks_l2.erase(it1); + } else { + it1++; + } + } +} + + +int +rach_detect_fft_impl::general_work( + int noutput_items, + gr_vector_int& ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + int read, max_read; + + /* Buffers pointer */ + const gr_complex *sig_in = reinterpret_cast<const gr_complex *>(input_items[0]); + gr_complex *burst_out = reinterpret_cast<gr_complex *>(output_items[0]); + gr_complex *fft_in = this->d_fft->get_inbuf(); + gr_complex *fft_out = this->d_fft->get_outbuf(); + + /* Skip over history */ + sig_in += this->history() - 1; + + /* If there is pending output, process this first */ + if (!this->d_peaks_pending.empty()) + { + peak pk = this->d_peaks_pending.back(); + int to_copy = this->d_burst_length - this->d_out_pos; + + if (to_copy > noutput_items) + to_copy = noutput_items; + + if (to_copy == 0) + return 0; + + if (this->d_out_pos == 0) + { + float phase_inc; + + /* Configure the rotator */ + phase_inc = - (2.0f * (float)M_PI / this->d_fft_size) * ( + fmodf( + pk.bin() + (float)(this->d_fft_size / 2), + (float)this->d_fft_size + ) - (float)(this->d_fft_size / 2) + ); + + phase_inc += this->d_freq_offset; + + this->d_r.set_phase_incr( exp(gr_complex(0, phase_inc)) ); + + /* Burst start */ + add_item_tag( + 0, + this->nitems_written(0), + this->d_len_tag_key, + this->d_burst_length_pmt + ); + + /* Burst angular frequency */ + add_item_tag( + 0, + this->nitems_written(0), + this->d_freq_tag_key, + pmt::from_double(phase_inc) + ); + } + + this->d_r.rotateN( + burst_out, + sig_in + this->d_out_pos - this->history() + 1, + to_copy + ); + + this->d_out_pos += to_copy; + + if (this->d_out_pos == this->d_burst_length) + { + this->d_peaks_pending.pop_back(); + this->d_out_pos = 0; + } + + return to_copy; + } + + /* Process as much input as we can */ + max_read = ninput_items[0] - this->history() + 1; + + for (read=0; read<max_read;) + { + int n_adv = this->d_fft_size / this->d_overlap_ratio; + int n_reuse = this->d_fft_size - n_adv; + int n_fill; + + /* Fill our internal buffer */ + n_fill = this->d_fft_size - this->d_in_pos; + if (n_fill > max_read - read) + n_fill = max_read - read; + + memcpy(this->d_buf + this->d_in_pos, + sig_in + read, + n_fill * sizeof(gr_complex)); + + read += n_fill; + this->d_in_pos += n_fill; + + if (this->d_in_pos != this->d_fft_size) + break; + + /* Apply window */ + volk_32fc_32f_multiply_32fc( + fft_in, + this->d_buf, + this->d_win, + this->d_fft_size + ); + + /* Compute FFT */ + this->d_fft->execute(); + + /* Compute the squared power */ + volk_32fc_magnitude_squared_32f(this->d_pwr, fft_out, this->d_fft_size); + + /* Compute the per-bin IIR average */ + for (int i=0; i<this->d_fft_size; i++) + { + const float alpha = 0.01f, one_minus_alpha = 1.0f - alpha; + this->d_avg[i] = (this->d_avg[i] * one_minus_alpha) + + (this->d_pwr[i] * alpha); + } + + /* Run the peak detection */ + this->peak_detect( + (this->nitems_read(0) + read) / (this->d_fft_size / this->d_overlap_ratio) + ); + + /* Handle overlap */ + if (this->d_overlap_ratio > 1) { + memmove(this->d_buf, this->d_buf + n_adv, n_reuse * sizeof(gr_complex)); + this->d_in_pos = n_reuse; + } else { + this->d_in_pos = 0; + } + + /* If we have anything pending, don't continue */ + if (!this->d_peaks_pending.empty()) + break; + } + + /* We read some stuff */ + this->consume_each(read); + + return 0; +} + + +rach_detect_fft_impl::peak::peak(uint64_t time, int bin) +{ + this->d_time[0] = this->d_time[1] = time; + this->d_bin[0] = this->d_bin[1] = bin; +} + + +bool +rach_detect_fft_impl::peak::merge(uint64_t time, int bin) +{ + /* Check if mergeable */ + if (bin > (this->d_bin[1] + 2)) + return false; + + if (bin < (this->d_bin[0] - 2)) + return false; + + if (time > (this->d_time[1] + 2)) + return false; + + if (time < (this->d_time[0] - 2)) + return false; + + /* Do the merge */ + if (bin > this->d_bin[1]) + this->d_bin[1] = bin; + else if (bin < this->d_bin[0]) + this->d_bin[0] = bin; + + if (time > this->d_time[1]) + this->d_time[1] = time; + else if (time < this->d_time[0]) + this->d_time[0] = time; + + return true; +} + +bool +rach_detect_fft_impl::peak::expired(uint64_t time_limit) const +{ + return time_limit > this->d_time[1]; +} + +uint64_t +rach_detect_fft_impl::peak::time() const +{ + return this->d_time[0] + ((this->d_time[1] - this->d_time[0]) >> 1); +} + +float +rach_detect_fft_impl::peak::bin() const +{ + return (float)this->d_bin[0] + ((float)(this->d_bin[1] - this->d_bin[0]) / 2.0f); +} + + + } // namespace gmr1 +} // namespace gr diff --git a/gr-gmr1/lib/rach_detect_fft_impl.h b/gr-gmr1/lib/rach_detect_fft_impl.h new file mode 100644 index 0000000..415c6df --- /dev/null +++ b/gr-gmr1/lib/rach_detect_fft_impl.h @@ -0,0 +1,97 @@ +/* -*- c++ -*- */ +/* + * Copyright 2014 Sylvain Munaut <tnt@246tNt.com> + * + * This 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. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software; see the file COPYING. If not, write to + * the Free Software Foundation, Inc., 51 Franklin Street, + * Boston, MA 02110-1301, USA. + */ + + +#ifndef INCLUDED_GR_GMR1_RACH_DETECT_FFT_IMPL_H +#define INCLUDED_GR_GMR1_RACH_DETECT_FFT_IMPL_H + +#include <gnuradio/gmr1/rach_detect_fft.h> + +#include <gnuradio/blocks/rotator.h> +#include <gnuradio/fft/fft.h> + +namespace gr { + namespace gmr1 { + + /*! + * \brief + * \ingroup gmr1 + */ + class rach_detect_fft_impl : public rach_detect_fft + { + private: + + class peak + { + public: + uint64_t d_time[2]; + int d_bin[2]; + + peak(uint64_t time, int bin); + bool merge(uint64_t time, int bin); + + bool expired(uint64_t time_limit) const; + uint64_t time() const; + float bin() const; + }; + + int d_fft_size; + int d_overlap_ratio; + float d_threshold; + int d_burst_length; + int d_burst_offset; + float d_freq_offset; + + pmt::pmt_t d_freq_tag_key; + pmt::pmt_t d_len_tag_key; + pmt::pmt_t d_burst_length_pmt; + + gr::fft::fft_complex *d_fft; + gr_complex *d_buf; + float *d_win; + float *d_pwr; + float *d_avg; + + int d_in_pos; + std::vector<peak> d_peaks_l1; + std::vector<peak> d_peaks_l2; + + int d_out_pos; + gr::blocks::rotator d_r; + std::vector<peak> d_peaks_pending; + + void peak_detect(uint64_t position); + + public: + rach_detect_fft_impl(int fft_size, int overlap_ratio, float threshold, + int burst_length, int burst_offset, float freq_offset, + const std::string& len_tag_key); + virtual ~rach_detect_fft_impl(); + + int general_work(int noutput_items, + gr_vector_int& ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); + }; + + } // namespace gmr1 +} // namespace gr + +#endif /* INCLUDED_GR_GMR1_RACH_DETECT_FFT_IMPL_H */ |