- fixed peak sorting

git-svn-id: http://moon:8086/svn/software/trunk/projects/GnuRadio@461 b431acfa-c32f-4a4a-93f1-934dc6c82436

gr-jay/grc/jay_peak_detect.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <block>
-  <name>peak_detect</name>
+  <name>Jay's Peak Detector</name>
   <key>jay_peak_detect</key>
   <category>[jay]</category>
   <import>import jay</import>

gr-jay/lib/peak_detect_impl.cc

@@ -56,6 +56,7 @@ namespace gr {
 		m_pXm = new float[vlen];
 		m_pXd = new float[vlen];
 		m_pPeakPos = new int[vlen];
+		m_pPeakPos_final = new int[vlen];
 		m_pPeakBox_rel = new float[vlen];
 		m_pPeakBox_abs = new float[vlen];
 		
@@ -65,6 +66,7 @@ namespace gr {
 		memset(m_pXm, 0, vlen*sizeof(float));
 		memset(m_pXd, 0, vlen*sizeof(float));
 		memset(m_pPeakPos, 0, vlen*sizeof(int));
+		memset(m_pPeakPos_final, 0, vlen*sizeof(int));
 		memset(m_pPeakBox_rel, 0, vlen*sizeof(float));
 		memset(m_pPeakBox_abs, 0, vlen*sizeof(float));
 		
@@ -80,6 +82,7 @@ namespace gr {
     {
 		delete [] m_pPeakBox_abs;
 		delete [] m_pPeakBox_rel;
+		delete [] m_pPeakPos_final;
 		delete [] m_pPeakPos;
 		delete [] m_pXd;
 		delete [] m_pXm;
@@ -143,11 +146,6 @@ namespace gr {
 			// Initialization on first frame
 			if (m_frameCount == 0)
 			{
-				fprintf(stderr, "Peak Detector: Initialize\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 xmean = mean(iptr, nitems_per_block);
 				fill(m_pXs, nitems_per_block, xmean);
 				fill(m_pXm, nitems_per_block, xmean);
@@ -254,29 +252,23 @@ namespace gr {
 			// -----------------------------------------------------------------
 			// Sort peaks
 			// -----------------------------------------------------------------
-			std::vector<int> peakPos;
+			sort(m_pXd, m_pPeakPos, numPeaks);
-			for (int i = 0; i < numPeaks; i++)
-			{
-				peakPos.push_back(m_pPeakPos[i]);
-			}
-			
-			Compare cmp(m_pPeakData);
-			std::sort(peakPos.begin(), peakPos.end(), cmp);
-
 			
 			// -----------------------------------------------------------------
 			// Construct peak boxes
 			// -----------------------------------------------------------------
-			numPeaks = 0;
+			int numPeaks_final = 0;
+			memset(m_pPeakPos_final, 0, nitems_per_block*sizeof(int));
 			memcpy(m_pPeakBox_abs, m_pXm, nitems_per_block*sizeof(float));
 			memset(m_pPeakBox_rel, 0, nitems_per_block*sizeof(float));
-			for (int i = 0; i < peakPos.size(); i++)
+			
+			for (int i = 0; i < numPeaks; i++)
 			{
 				int pbh = 0;
-				int pos = peakPos[i];
+				int pos = m_pPeakPos[i];
 				int left = pos;
 				int right = pos;
-				float nom = m_pXd[pos] - 10;
+				float nom = m_pXd[pos];
 				int left_found = 0;
 				int right_found = 0;
 				while(!(left_found & right_found))
@@ -359,7 +351,7 @@ namespace gr {
 						m_pPeakBox_abs[n] = box_height_abs;
 						m_pPeakBox_rel[n] = box_height_rel;
 					}
-					m_pPeakPos[numPeaks++] = pos;
+					m_pPeakPos_final[numPeaks_final++] = pos;
 				}
 			}
 			
@@ -374,15 +366,15 @@ namespace gr {
 			// -----------------------------------------------------------------
 			// Output tags
 			// -----------------------------------------------------------------
-			for (int i = 0; i < numPeaks; i++)
+			for (int i = 0; i < numPeaks_final; i++)
 			{
 				// Create peak tags
 				pmt::pmt_t tag_key = pmt::string_to_symbol("id");
 				pmt::pmt_t tag_value = pmt::from_long(i);
-				add_item_tag(0, nitems_written(0) + m_pPeakPos[i], tag_key, tag_value, m_tag_id);
+				add_item_tag(0, nitems_written(0) + m_pPeakPos_final[i], tag_key, tag_value, m_tag_id);
 				tag_key = pmt::string_to_symbol("pos");
-				tag_value = pmt::from_long(m_pPeakPos[i]);
+				tag_value = pmt::from_long(m_pPeakPos_final[i]);
-				add_item_tag(0, nitems_written(0) + m_pPeakPos[i], tag_key, tag_value, m_tag_id);
+				add_item_tag(0, nitems_written(0) + m_pPeakPos_final[i], tag_key, tag_value, m_tag_id);
 			}
 
 			m_frameCount++;
@@ -392,14 +384,6 @@ namespace gr {
 		return noutput_items;
     }
 
-	int peak_detect_impl::cmp(const void *pA, const void *pB)
-	{
-		int a = *((int*)pA);
-		int b = *((int*)pB);
-		
-		return m_pPeakData[a] > m_pPeakData[b];
-	}
-
   } /* namespace jay */
 } /* namespace gr */
 

gr-jay/lib/peak_detect_impl.h

@@ -29,20 +29,6 @@ namespace jay {
     class peak_detect_impl : public peak_detect
     {
 	private:
-		struct Compare
-		{
-			float const *m_pData;
-			Compare(float const *pData)
-			: m_pData(pData)
-			{
-				
-			}
-			bool operator() (int i,int j)
-			{
-				return (m_pData[i] < m_pData[j]);
-			}
-		  
-		};
 	
 		float m_framerate;
 		float m_peak_height_min;
@@ -56,6 +42,7 @@ namespace jay {
 		float *m_pXm;
 		float *m_pXd;
 		int *m_pPeakPos;
+		int *m_pPeakPos_final;
 		float *m_pPeakBox_rel;
 		float *m_pPeakBox_abs;
 		uint64_t m_frameCount;
@@ -67,12 +54,31 @@ 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);
+	private:
 		void fill(float *pDst, int length, float value);
 		float mean(float *pSrc, int length);
 		int threshold(float *pDst, float *pSrc, int length, float thresh);
 		int findPeakPos(int *pDst, float *pSrc, int length);
 		
-		int cmp(const void *a, const void *b);
+		inline void sort(float const * const pData, int *pPos, int numPos)
+		{
+			int n = numPos;
+			do
+			{
+				int newn = 1;
+				for (int i=0; i < (n-1); i++)
+				{
+					if (pData[pPos[i]] < pData[pPos[i+1]])
+					{
+						int temp = pPos[i+1];
+						pPos[i+1] = pPos[i];
+						pPos[i] = temp;
+						newn = i+1;
+					} // ende if
+				} // ende for
+				n = newn;
+			} while (n > 1);
+		}
     };
 
 } // namespace jay