Commit the new FFT detect code (WIP)

Signed-off-by: Sylvain Munaut <tnt@246tNt.com>

5 files changed, 986 insertions, 27 deletions

diff --git a/gr-gmr1/apps/gmr_rach_scan.grc b/gr-gmr1/apps/gmr_rach_scan.grc
new file mode 100644
index 0000000..33e34d4
--- /dev/null
+++ b/gr-gmr1/apps/gmr_rach_scan.grc
@@ -0,0 +1,688 @@
+<?xml version='1.0' encoding='ASCII'?>
+<?grc format='1' created='3.7.5'?>
+<flow_graph>
+  <timestamp>Sat Oct 11 16:48:14 2014</timestamp>
+  <block>
+    <key>blocks_complex_to_float</key>
+    <param>
+      <key>id</key>
+      <value>blocks_complex_to_float_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>vlen</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(560, 80)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>blocks_float_to_complex</key>
+    <param>
+      <key>id</key>
+      <value>blocks_float_to_complex_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>vlen</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(560, 152)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>180</value>
+    </param>
+  </block>
+  <block>
+    <key>blocks_tagged_stream_multiply_length</key>
+    <param>
+      <key>id</key>
+      <value>blocks_tagged_stream_multiply_length_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>complex</value>
+    </param>
+    <param>
+      <key>lengthtagname</key>
+      <value>packet_len</value>
+    </param>
+    <param>
+      <key>vlen</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>c</key>
+      <value>93600 / samp_rate</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(536, 352)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>rach_demod</key>
+    <param>
+      <key>id</key>
+      <value>rach_demod_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>sps</key>
+      <value>4</value>
+    </param>
+    <param>
+      <key>etoa</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>len_tag_key</key>
+      <value>packet_len</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(280, 467)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>gsmtap_sink</key>
+    <param>
+      <key>id</key>
+      <value>gsmtap_sink_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>host</key>
+      <value>127.0.0.1</value>
+    </param>
+    <param>
+      <key>port</key>
+      <value>4729</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(552, 475)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>blocks_tag_debug</key>
+    <param>
+      <key>id</key>
+      <value>blocks_tag_debug_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>complex</value>
+    </param>
+    <param>
+      <key>name</key>
+      <value></value>
+    </param>
+    <param>
+      <key>filter</key>
+      <value>""</value>
+    </param>
+    <param>
+      <key>num_inputs</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>vlen</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>display</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(832, 259)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>blocks_file_sink</key>
+    <param>
+      <key>id</key>
+      <value>blocks_file_sink_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>file</key>
+      <value>/tmp/bursts.cfile</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>complex</value>
+    </param>
+    <param>
+      <key>vlen</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>unbuffered</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>append</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(832, 347)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>pfb_arb_resampler_xxx</key>
+    <param>
+      <key>id</key>
+      <value>pfb_arb_resampler_xxx_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>ccf</value>
+    </param>
+    <param>
+      <key>rrate</key>
+      <value>93600 / samp_rate</value>
+    </param>
+    <param>
+      <key>taps</key>
+      <value>firdes.low_pass(1.0, 32 * samp_rate, 16e3, 2e3)</value>
+    </param>
+    <param>
+      <key>nfilts</key>
+      <value>32</value>
+    </param>
+    <param>
+      <key>atten</key>
+      <value>100</value>
+    </param>
+    <param>
+      <key>samp_delay</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(280, 323)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>parameter</key>
+    <param>
+      <key>id</key>
+      <value>samp_rate</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>label</key>
+      <value></value>
+    </param>
+    <param>
+      <key>value</key>
+      <value>4e6</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>eng_float</value>
+    </param>
+    <param>
+      <key>short_id</key>
+      <value></value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(344, 11)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>parameter</key>
+    <param>
+      <key>id</key>
+      <value>filename</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>label</key>
+      <value></value>
+    </param>
+    <param>
+      <key>value</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>string</value>
+    </param>
+    <param>
+      <key>short_id</key>
+      <value></value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(224, 11)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>blocks_file_source</key>
+    <param>
+      <key>id</key>
+      <value>blocks_file_source_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>file</key>
+      <value>filename</value>
+    </param>
+    <param>
+      <key>type</key>
+      <value>complex</value>
+    </param>
+    <param>
+      <key>repeat</key>
+      <value>False</value>
+    </param>
+    <param>
+      <key>vlen</key>
+      <value>1</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(272, 83)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>rach_detect_fft</key>
+    <param>
+      <key>id</key>
+      <value>rach_detect_fft_0</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>fft_size</key>
+      <value>1 &lt;&lt; int(round(math.log(samp_rate / 1e3) / math.log(2)))</value>
+    </param>
+    <param>
+      <key>overlap_ratio</key>
+      <value>2</value>
+    </param>
+    <param>
+      <key>threshold</key>
+      <value>8.5</value>
+    </param>
+    <param>
+      <key>burst_length</key>
+      <value>int(3 * samp_rate * 15e-3)</value>
+    </param>
+    <param>
+      <key>burst_offset</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>freq_offset</key>
+      <value>- (math.pi / 4.0) * (23400.0 / samp_rate)</value>
+    </param>
+    <param>
+      <key>len_tag_key</key>
+      <value>packet_len</value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>affinity</key>
+      <value></value>
+    </param>
+    <param>
+      <key>minoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>maxoutbuf</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(40, 299)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <block>
+    <key>options</key>
+    <param>
+      <key>id</key>
+      <value>gmr1_rach_scan</value>
+    </param>
+    <param>
+      <key>_enabled</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>title</key>
+      <value></value>
+    </param>
+    <param>
+      <key>author</key>
+      <value></value>
+    </param>
+    <param>
+      <key>description</key>
+      <value></value>
+    </param>
+    <param>
+      <key>window_size</key>
+      <value>1280, 1024</value>
+    </param>
+    <param>
+      <key>generate_options</key>
+      <value>no_gui</value>
+    </param>
+    <param>
+      <key>category</key>
+      <value>Custom</value>
+    </param>
+    <param>
+      <key>run_options</key>
+      <value>run</value>
+    </param>
+    <param>
+      <key>run</key>
+      <value>True</value>
+    </param>
+    <param>
+      <key>max_nouts</key>
+      <value>0</value>
+    </param>
+    <param>
+      <key>realtime_scheduling</key>
+      <value></value>
+    </param>
+    <param>
+      <key>alias</key>
+      <value></value>
+    </param>
+    <param>
+      <key>_coordinate</key>
+      <value>(8, 11)</value>
+    </param>
+    <param>
+      <key>_rotation</key>
+      <value>0</value>
+    </param>
+  </block>
+  <connection>
+    <source_block_id>rach_detect_fft_0</source_block_id>
+    <sink_block_id>pfb_arb_resampler_xxx_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_complex_to_float_0</source_block_id>
+    <sink_block_id>blocks_float_to_complex_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>1</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_complex_to_float_0</source_block_id>
+    <sink_block_id>blocks_float_to_complex_0</sink_block_id>
+    <source_key>1</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_file_source_0</source_block_id>
+    <sink_block_id>blocks_complex_to_float_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_float_to_complex_0</source_block_id>
+    <sink_block_id>rach_detect_fft_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>rach_demod_0</source_block_id>
+    <sink_block_id>gsmtap_sink_0</sink_block_id>
+    <source_key>pdus</source_key>
+    <sink_key>pdus</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>pfb_arb_resampler_xxx_0</source_block_id>
+    <sink_block_id>blocks_tagged_stream_multiply_length_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_tagged_stream_multiply_length_0</source_block_id>
+    <sink_block_id>rach_demod_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_tagged_stream_multiply_length_0</source_block_id>
+    <sink_block_id>blocks_tag_debug_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+  <connection>
+    <source_block_id>blocks_tagged_stream_multiply_length_0</source_block_id>
+    <sink_block_id>blocks_file_sink_0</sink_block_id>
+    <source_key>0</source_key>
+    <sink_key>0</sink_key>
+  </connection>
+</flow_graph>
diff --git a/gr-gmr1/grc/rach_detect_fft.xml b/gr-gmr1/grc/rach_detect_fft.xml
index b67d016..406db06 100644
--- a/gr-gmr1/grc/rach_detect_fft.xml
+++ b/gr-gmr1/grc/rach_detect_fft.xml
@@ -3,8 +3,51 @@
  <name>RACH FFT Detection</name>
  <key>rach_detect_fft</key>
  <category>GMR-1</category>
+ <import>import math</import>
  <import>from gnuradio import gmr1</import>
- <make>gmr1.rach_detect_fft()</make>
+ <make>gmr1.rach_detect_fft($fft_size, $overlap_ratio, $threshold, $burst_length, $burst_offset, $freq_offset, $len_tag_key)</make>
+ <param>
+   <name>FFT size</name>
+   <key>fft_size</key>
+   <value>1 &lt;&lt; int(round(math.log(samp_rate / 1e3) / math.log(2)))</value>
+   <type>int</type>
+ </param>
+ <param>
+   <name>Overlap ratio</name>
+   <key>overlap_ratio</key>
+   <value>2</value>
+   <type>int</type>
+ </param>
+ <param>
+   <name>Detection Threshold</name>
+   <key>threshold</key>
+   <value>8.5</value>
+   <type>float</type>
+ </param>
+ <param>
+   <name>Burst length</name>
+   <key>burst_length</key>
+   <value>int(3 * samp_rate * 15e-3)</value>
+   <type>int</type>
+ </param>
+ <param>
+   <name>Burst offset</name>
+   <key>burst_offset</key>
+   <value>0</value>
+   <type>int</type>
+ </param>
+ <param>
+   <name>Frequency offset</name>
+   <key>freq_offset</key>
+   <value>- (math.pi / 4.0) * (23400.0 / samp_rate)</value>
+   <type>float</type>
+ </param>
+ <param>
+   <name>Length Tag Name</name>
+   <key>len_tag_key</key>
+   <value>packet_len</value>
+   <type>string</type>
+ </param>
  <sink>
    <name>in</name>
    <type>complex</type>
diff --git a/gr-gmr1/include/gnuradio/gmr1/rach_detect_fft.h b/gr-gmr1/include/gnuradio/gmr1/rach_detect_fft.h
index 7eebab3..fbe3441 100644
--- a/gr-gmr1/include/gnuradio/gmr1/rach_detect_fft.h
+++ b/gr-gmr1/include/gnuradio/gmr1/rach_detect_fft.h
@@ -37,7 +37,9 @@ namespace gr {
      public:
       typedef boost::shared_ptr<rach_detect_fft> sptr;
 
-      static sptr make();
+      static sptr make(int fft_size, int overlap_ratio, float threshold,
+                       int burst_length, int burst_offset, float freq_offset,
+                       const std::string& len_tag_key);
     };
 
   } // namespace gmr1
diff --git a/gr-gmr1/lib/rach_detect_fft_impl.cc b/gr-gmr1/lib/rach_detect_fft_impl.cc
index 0b527fd..c835550 100644
--- a/gr-gmr1/lib/rach_detect_fft_impl.cc
+++ b/gr-gmr1/lib/rach_detect_fft_impl.cc
@@ -35,31 +35,46 @@ namespace gr {
   namespace gmr1 {
 
 rach_detect_fft::sptr
-rach_detect_fft::make()
+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()
+		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()
+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)))
+                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_len_tag_key(pmt::string_to_symbol(len_tag_key)),
+      d_burst_length_pmt(pmt::from_long(burst_length))
 {
-	this->d_threshold = 8.5f; // 7.5f;
-	this->d_overlap_ratio = 2;
-	this->d_fft_size = 512;
 	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_pos = 0;
+	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()
@@ -67,6 +82,7 @@ rach_detect_fft_impl::~rach_detect_fft_impl()
 	volk_free(this->d_pwr);
 	volk_free(this->d_win);
 	volk_free(this->d_buf);
+
 	delete this->d_fft;
 }
 
@@ -74,6 +90,7 @@ rach_detect_fft_impl::~rach_detect_fft_impl()
 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;
@@ -87,10 +104,68 @@ rach_detect_fft_impl::peak_detect(uint64_t position)
 	/* 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))
-			printf("(%lld, %d)\n", (long long)position, 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);
+			}
+
+			/* 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->time() - it2->time() <= 4 * this->d_overlap_ratio) &&
+				    (it1->time() - it2->time() >= 2 * this->d_overlap_ratio))
+				{
+					matched = true;
+					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++;
+		}
+	}
 }
 
 
@@ -101,7 +176,7 @@ rach_detect_fft_impl::general_work(
 	gr_vector_const_void_star &input_items,
 	gr_vector_void_star &output_items)
 {
-	int read;
+	int read, max_read;
 
 	/* Buffers pointer */
 	const gr_complex *sig_in = reinterpret_cast<const gr_complex *>(input_items[0]);
@@ -109,26 +184,84 @@ rach_detect_fft_impl::general_work(
 	gr_complex *fft_in  = this->d_fft->get_inbuf();
 	gr_complex *fft_out = this->d_fft->get_outbuf();
 
-	/* Process as much as we can */
-	for (read=0; read<noutput_items;)
+	/* 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 (this->d_out_pos == 0)
+		{
+			float phase_inc;
+
+			printf("(%lld, %d)\n", pk.time(), (int)pk.bin());
+
+			/* 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
+			);
+		}
+
+		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_pos;
-		if (n_fill > noutput_items - read)
-			n_fill = noutput_items - read;
+		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_pos,
+		memcpy(this->d_buf + this->d_in_pos,
 		       sig_in + read,
 		       n_fill * sizeof(gr_complex));
 
 		read += n_fill;
-		this->d_pos += n_fill;
+		this->d_in_pos += n_fill;
 
-		if (this->d_pos != this->d_fft_size)
+		if (this->d_in_pos != this->d_fft_size)
 			break;
 
 		/* Apply window */
@@ -147,16 +280,20 @@ rach_detect_fft_impl::general_work(
 
 		/* Run the peak detection */
 		this->peak_detect(
-			this->nitems_read(0) + read - (this->d_fft_size / 2)
+			(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_pos = n_reuse;
+			this->d_in_pos = n_reuse;
 		} else {
-			this->d_pos = 0;
+			this->d_in_pos = 0;
 		}
+
+		/* If we have anything pending, don't continue */
+		if (!this->d_peaks_pending.empty())
+			break;
 	}
 
 	/* We read some stuff */
@@ -165,5 +302,62 @@ rach_detect_fft_impl::general_work(
 	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] + 1))
+		return false;
+
+	if (bin < (this->d_bin[0] - 1))
+		return false;
+
+	if (time > (this->d_time[1] + 1))
+		return false;
+
+	if (time < (this->d_time[0] - 1))
+		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
index 2cb4d44..9adc2b5 100644
--- a/gr-gmr1/lib/rach_detect_fft_impl.h
+++ b/gr-gmr1/lib/rach_detect_fft_impl.h
@@ -24,6 +24,7 @@
 
 #include <gnuradio/gmr1/rach_detect_fft.h>
 
+#include <gnuradio/blocks/rotator.h>
 #include <gnuradio/fft/fft.h>
 
 namespace gr {
@@ -36,20 +37,51 @@ namespace gr {
     class rach_detect_fft_impl : public rach_detect_fft
     {
      private:
-      float d_threshold;
-      int d_overlap_ratio;
+
+      class peak
+      {
+       private:
+        uint64_t d_time[2];
+	int d_bin[2];
+
+       public:
+        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_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;
 
-      int d_pos;
+      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();
+      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,