From dc6485e73f489d3de6e9dd06e6c1b101bfa12f17 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Tue, 28 May 2019 17:19:19 +0000
Subject: [PATCH] - Peak detector delivers frequency instead of bin

git-svn-id: http://moon:8086/svn/software/trunk/projects/GnuRadio@492 b431acfa-c32f-4a4a-93f1-934dc6c82436
---
 gr-jay/grc/jay_peak_detect.xml   | 14 +++++++++++++-
 gr-jay/grc/jay_peak_manager.xml  | 10 +++++++++-
 gr-jay/include/jay/peak_detect.h |  2 +-
 gr-jay/lib/peak_detect_impl.cc   | 16 ++++++++++++----
 gr-jay/lib/peak_detect_impl.h    | 17 ++++++++++++-----
 gr-jay/python/peak_manager.py    | 32 ++++++++++++++++++++++++++------
 6 files changed, 73 insertions(+), 18 deletions(-)

diff --git a/gr-jay/grc/jay_peak_detect.xml b/gr-jay/grc/jay_peak_detect.xml
index 739bbbf..de5bbf5 100644
--- a/gr-jay/grc/jay_peak_detect.xml
+++ b/gr-jay/grc/jay_peak_detect.xml
@@ -4,7 +4,7 @@
   <key>jay_peak_detect</key>
   <category>[jay]</category>
   <import>import jay</import>
-  <make>jay.peak_detect($vlen, $framerate, $peak_height_min, $peak_width_min, $peak_width_max, $alpha_s, $alpha_m)</make>
+  <make>jay.peak_detect($vlen, $framerate, $bandwidth, $fcenter, $peak_height_min, $peak_width_min, $peak_width_max, $alpha_s, $alpha_m)</make>
   <!-- Make one 'param' node for every Parameter you want settable from the GUI.
        Sub-nodes:
        * name
@@ -21,8 +21,20 @@
     <key>framerate</key>
     <value>30</value>
     <type>float</type>
+  </param>
+    <param>
+    <name>Bandwidth (Hz)</name>
+    <key>bandwidth</key>
+    <value>samp_rate</value>
+    <type>float</type>
   </param>
   <param>
+    <name>Center Frequency (Hz)</name>
+    <key>fcenter</key>
+    <value>0</value>
+    <type>float</type>
+  </param>
+<param>
     <name>Peak Height Minimum [dB]</name>
     <key>peak_height_min</key>
     <value>20</value>
diff --git a/gr-jay/grc/jay_peak_manager.xml b/gr-jay/grc/jay_peak_manager.xml
index 8ef39ed..5ed97fe 100644
--- a/gr-jay/grc/jay_peak_manager.xml
+++ b/gr-jay/grc/jay_peak_manager.xml
@@ -4,7 +4,15 @@
   <key>jay_peak_manager</key>
   <category>[jay]</category>
   <import>import jay</import>
-  <make>jay.peak_manager()</make>
+  <make>jay.peak_manager($test)</make>
+
+  <param>
+    <name>Test</name>
+    <key>test</key>
+    <value>1024</value>
+    <type>int</type>
+  </param>
+
    <sink>
     <name>control</name>
     <type>message</type>
diff --git a/gr-jay/include/jay/peak_detect.h b/gr-jay/include/jay/peak_detect.h
index 5841616..804ea2d 100644
--- a/gr-jay/include/jay/peak_detect.h
+++ b/gr-jay/include/jay/peak_detect.h
@@ -46,7 +46,7 @@ namespace gr {
        * class. jay::peak_detect::make is the public interface for
        * creating new instances.
        */
-      static sptr make(int vlen, float framerate, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m);
+      static sptr make(int vlen, float framerate, float bandwidth, float fcenter, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m);
     };
 
   } // namespace jay
diff --git a/gr-jay/lib/peak_detect_impl.cc b/gr-jay/lib/peak_detect_impl.cc
index 756dcdc..2420f26 100644
--- a/gr-jay/lib/peak_detect_impl.cc
+++ b/gr-jay/lib/peak_detect_impl.cc
@@ -29,20 +29,23 @@ namespace gr {
   namespace jay {
 
     peak_detect::sptr
-    peak_detect::make(int vlen, float framerate, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m)
+    peak_detect::make(int vlen, float framerate, float bandwidth, float fcenter, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m)
     {
       return gnuradio::get_initial_sptr
-        (new peak_detect_impl(vlen, framerate, peak_height_min, peak_width_min, peak_width_max, alpha_s, alpha_m));
+        (new peak_detect_impl(vlen, framerate, bandwidth, fcenter, peak_height_min, peak_width_min, peak_width_max, alpha_s, alpha_m));
     }
 
     /*
      * The private constructor
      */
-    peak_detect_impl::peak_detect_impl(int vlen, float framerate, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m)
+    peak_detect_impl::peak_detect_impl(int vlen, float framerate, float bandwidth, float fcenter, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m)
 		: gr::sync_block("peak_detect"
 		, gr::io_signature::make(1, 1, vlen*sizeof(float))
         , gr::io_signature::make(2, 2, vlen*sizeof(float)))
+		, m_vlen(vlen)
 		, m_framerate (framerate)
+		, m_bandwidth (bandwidth)
+		, m_fcenter (fcenter)
 		, m_peak_height_min (peak_height_min)
 		, m_peak_width_min (peak_width_min)
 		, m_peak_width_max (peak_width_max)
@@ -230,7 +233,12 @@ namespace gr {
 						}
 						else
 						{
-							Peak peak(m_peak_id++, pos);
+//			pos = float(peak['pos'] - self.nfft / 2)
+//			freq_Hz = self.fcenter + 1.0 / self.nfft * self.bandwidth * pos
+
+							float pos_shifted = (float)(pos-m_vlen/2);
+							float freq_Hz = m_fcenter + m_bandwidth*pos_shifted/m_vlen;
+							Peak peak(m_peak_id++, pos, freq_Hz);
 							peak.height_rel = m_pXds[pos];
 							addToListByPos(peaks, peak);
 							numPeaksClimbed++;
diff --git a/gr-jay/lib/peak_detect_impl.h b/gr-jay/lib/peak_detect_impl.h
index 54c6d50..bf94aaf 100644
--- a/gr-jay/lib/peak_detect_impl.h
+++ b/gr-jay/lib/peak_detect_impl.h
@@ -31,19 +31,20 @@ namespace jay {
 		struct Peak
 		{
 			Peak()
-			: Peak(0, 0, 0, 0.0, 0.0)
+			: Peak(0, 0, 0, 0.0, 0.0, 0.0)
 			{
 				
 			}
-			Peak(uint64_t _id, int _pos)
-			: Peak(_id, _pos, 0, 0.0, 0.0)
+			Peak(uint64_t _id, int _pos, float frequency)
+			: Peak(_id, _pos, 0, frequency, 0.0, 0.0)
 			{
 				
 			}
-			Peak(uint64_t _id, int _pos, int _pbh, float _height_rel, float _height_abs)
+			Peak(uint64_t _id, int _pos, int _pbh, float _frequency, float _height_rel, float _height_abs)
 			: id(_id)
 			, pos(_pos)
 			, pbh(_pbh)
+			, frequency(_frequency)
 			, height_rel(_height_rel)
 			, height_abs(_height_abs)
 			{
@@ -54,6 +55,7 @@ namespace jay {
 			int pbh;
 			float height_rel;
 			float height_abs;
+			float frequency;
 			
 			bool isInRange(const Peak &other) const
 			{
@@ -153,6 +155,7 @@ namespace jay {
 				float alpha = (1.f-beta);
 				float pos_f = alpha*(float)pos + beta*(float)other.pos;
 				float pbh_f = alpha*(float)pbh + beta*(float)other.pbh;
+				float frequency = alpha*(float)frequency + beta*(float)other.frequency;
 				pos = (int)(pos_f + 0.5f);
 				pbh = (int)(pbh_f + 0.5f);
 				height_rel = other.height_rel;
@@ -175,6 +178,7 @@ namespace jay {
 				pmt::pmt_t dict = pmt::make_dict();
 				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("id")), pmt::from_long(id));
 				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("pos")), pmt::from_long(pos));
+				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("freq_Hz")), pmt::from_float(frequency));
 				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("height")), pmt::from_float(height_rel));
 				dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("state")), pmt::from_long(state));
 				
@@ -222,7 +226,10 @@ namespace jay {
 			return !found;
 		}
 		
+		int m_vlen;
 		float m_framerate;
+		float m_bandwidth;
+		float m_fcenter;
 		float m_peak_height_min;
 		int m_peak_width_min;
 		int m_peak_width_max;
@@ -245,7 +252,7 @@ namespace jay {
 		pmt::pmt_t m_msg_port_out;
 		
 	public:
-		peak_detect_impl(int vlen, float framerate, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m);
+		peak_detect_impl(int vlen, float framerate, float bandwidth, float fcenter, float peak_height_min, int peak_width_min, int peak_width_max, float alpha_s, float alpha_m);
 		~peak_detect_impl();
 
 		// Where all the action really happens
diff --git a/gr-jay/python/peak_manager.py b/gr-jay/python/peak_manager.py
index 1e087f8..ab1a69f 100644
--- a/gr-jay/python/peak_manager.py
+++ b/gr-jay/python/peak_manager.py
@@ -27,12 +27,16 @@ class peak_manager(gr.basic_block):
 	"""
 	docstring for block peak_manager
 	"""
-	def __init__(self):
+	def __init__(self, test):
 		gr.basic_block.__init__(self,
 								name="peak_manager",
 								in_sig=None,
 								out_sig=None)
 
+		self.test = test
+
+		print ("test       : ", test)
+
 		# Register in / out message ports
 		self.message_port_register_in(pmt.intern("control"))
 		self.message_port_register_out(pmt.intern("status"))
@@ -43,22 +47,38 @@ class peak_manager(gr.basic_block):
 		#self.message_port_pub(pmt.intern("port name"), < pmt message >)
 
 	def msg_handler(self, msg):
-
-		print ("Message received:")
 		if pmt.is_dict(msg):
 			items = pmt.dict_items(msg)
 
+			peak = {}
 			for n in range(0, pmt.length(items)):
 				pair = pmt.nth(n, items)
 				key = pmt.car(pair)
 				val = pmt.cdr(pair)
 
+				key_str = pmt.symbol_to_string(key)
+
 				if pmt.is_integer(val):
-					print("{}={}".format(pmt.symbol_to_string(key), pmt.to_long(val)))
+					value = pmt.to_long(val)
+					peak[key_str] = value
+#					print("{}={}".format(key_str, value))
 				elif pmt.is_real(val):
-					print("{}={}".format(pmt.symbol_to_string(key), pmt.to_float(val)))
+					value = pmt.to_float(val)
+					peak[key_str] = value
+#					print("{}={}".format(key_str, value))
 				elif pmt.is_bool(val):
-					print("{}={}".format(pmt.symbol_to_string(key), pmt.to_bool(val)))
+					value = pmt.to_bool(val)
+					peak[key_str] = value
+#					print("{}={}".format(key_str, value))
+
+			id = peak['id']
+			freq_MHz = peak['freq_Hz']*1e-6
+
+			if peak['state'] == 1:
+				print ("New peak #{} at {:0.2f} MHz".format(id, freq_MHz))
+			else:
+				print ("Lost peak #{} at {:0.2f} MHz".format(id, freq_MHz))
+
 		else:
 			print(pmt.symbol_to_string(msg))