From e79f780754b7db3a292aa08546b29b756d6ea541 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Fri, 24 May 2019 16:04:32 +0000
Subject: [PATCH] - improved peak detect

git-svn-id: http://moon:8086/svn/software/trunk/projects/GnuRadio@452 b431acfa-c32f-4a4a-93f1-934dc6c82436
---
 gr-jay/lib/peak_detect_impl.cc | 93 ++++++++++++++++++++++++++--------
 gr-jay/lib/peak_detect_impl.h  |  7 +--
 2 files changed, 75 insertions(+), 25 deletions(-)

diff --git a/gr-jay/lib/peak_detect_impl.cc b/gr-jay/lib/peak_detect_impl.cc
index 70ba6ea..78297c4 100644
--- a/gr-jay/lib/peak_detect_impl.cc
+++ b/gr-jay/lib/peak_detect_impl.cc
@@ -44,17 +44,16 @@ namespace gr {
         , gr::io_signature::make(1, 1, vlen*sizeof(float)))
 		, m_alpha_s (alpha_s)
 		, m_alpha_m(alpha_m)
-		, m_vlen(vlen)
+		, m_frameCount(0)
     {
-		m_pPeakCount = new int[vlen];
+		m_pPeak = new int[vlen];
 		m_pXs = new float[vlen];
 		m_pXm = new float[vlen];
 		
-		memset(m_pPeakCount, 0, vlen*sizeof(int));
+		memset(m_pPeak, 0, vlen*sizeof(int));
 		memset(m_pXs, 0, vlen*sizeof(float));
 		memset(m_pXm, 0, vlen*sizeof(float));
-		
-		m_pFile_frames = fopen("/home/jens/frames2.dat", "w");
+	
 	}
 
     /*
@@ -62,35 +61,85 @@ namespace gr {
      */
     peak_detect_impl::~peak_detect_impl()
     {
-		fclose(m_pFile_frames);
-		
 		delete [] m_pXm;
 		delete [] m_pXs;
-		delete [] m_pPeakCount;
+		delete [] m_pPeak;
     }
 
+	float peak_detect_impl::mean(float* pSrc, int length)
+	{
+		float res = 0;
+		for (int i=0; i < length; i++)
+		{
+			res += pSrc[i];
+		}
+		return res / length;
+	}
+
+	void peak_detect_impl::fill(float* pDst, int length, float value)
+	{
+		for (int i=0; i < length; i++)
+		{
+			pDst[i] = value;
+		}
+	}
+
     int
     peak_detect_impl::work(int noutput_items,
         gr_vector_const_void_star &input_items,
         gr_vector_void_star &output_items)
     {
-		int k=0;
-		for(int i = 0; i < (noutput_items * m_vlen); i++)
+		int nitems_per_block = this->output_signature()->sizeof_stream_item(0)/sizeof(float);
+
+		fprintf(stderr, "Hallo\n");
+		fprintf(stderr, "noutput_items = %u\n", (int)noutput_items);
+		fprintf(stderr, "ninput_items = %u\n", (int)input_items.size());
+		fprintf(stderr, "nitems_per_block = %u\n", (int)nitems_per_block);
+		
+		float *iptr = (float *)input_items[0];
+		float *optr = (float *)output_items[0];
+		
+		for (int k = 0; k < noutput_items; k++)
 		{
-			float x = ((float*)input_items[k])[i];
-			m_pXs[i] = (1.f-m_alpha_s)*m_pXs[i] + m_alpha_s*x;
-			int gate = (int)(m_pXs[i] > (10+m_pXm[i]));
-			m_pPeakCount[i] = gate*m_pPeakCount[i] + gate;
-			float peak = (float)(m_pPeakCount[i] > 1);
-
-			m_pXm[i] = (1.f-peak)*((1.f-m_alpha_m)*m_pXm[i] + m_alpha_m*x) + peak*m_pXm[i];
-
-			((float*)output_items[k])[i] = m_pXm[i];
-			
-			fprintf(m_pFile_frames, "%0.3f ", x);
+			// Initialization on first frame
+			if (m_frameCount == 0)
+			{
+				float xmean = mean(iptr, nitems_per_block);
+				fill(m_pXs, nitems_per_block, xmean);
+				fill(m_pXm, nitems_per_block, xmean);
+			}
 			
+			// Peak detection
+			for (int i = 0; i < nitems_per_block; i++)
+			{
+				float x = *(iptr++);
+				
+				// Update of Xs
+				m_pXs[i] = (1.f-m_alpha_s)*m_pXs[i] + m_alpha_s*x;
+				
+				// Update of Xm
+				m_pXm[i] = (1.f-m_alpha_m)*m_pXm[i] + m_alpha_m*x;
+	
+				// Conditional update of Xm
+				float last = m_pXm[0];
+				for (int j=1; j < nitems_per_block; j++)
+				{
+					if (m_pXm[j] < (last+2))
+					{
+						last = (1.f-m_alpha_m)*last + m_alpha_m*m_pXm[j];
+					}
+					else
+					{
+						m_pXm[j] = last;	
+					}
+				}
+				
+				m_pPeak[i] = (int)((x-m_pXm[i]) > 10.f);
+				
+				*optr++ = x-m_pXm[i];
+			}
+			m_frameCount++;
 		}
-		fprintf(m_pFile_frames, "\n");
 
 		// Tell runtime system how many output items we produced.
 		return noutput_items;
diff --git a/gr-jay/lib/peak_detect_impl.h b/gr-jay/lib/peak_detect_impl.h
index f742616..5000c67 100644
--- a/gr-jay/lib/peak_detect_impl.h
+++ b/gr-jay/lib/peak_detect_impl.h
@@ -32,11 +32,10 @@ namespace jay {
 		// Nothing to declare in this block.
 		float m_alpha_s;
 		float m_alpha_m;
-		int m_vlen;
-		int *m_pPeakCount;
+		int *m_pPeak;
 		float *m_pXs;
 		float *m_pXm;
-		FILE *m_pFile_frames;
+		uint64_t m_frameCount;
 		 
 		public:
 		peak_detect_impl(float alpha_s, float alpha_m, int vlen);
@@ -44,6 +43,8 @@ namespace jay {
 
 		// Where all the action really happens
 		int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
+		void fill(float *pDst, int length, float value);
+		float mean(float *pSrc, int length);
     };
 
 } // namespace jay