diff --git a/gr-jay/grc/jay_peak_detect.xml b/gr-jay/grc/jay_peak_detect.xml index 11cf225..a23ab2a 100644 --- a/gr-jay/grc/jay_peak_detect.xml +++ b/gr-jay/grc/jay_peak_detect.xml @@ -1,6 +1,6 @@ - peak_detect + Jay's Peak Detector jay_peak_detect [jay] import jay diff --git a/gr-jay/lib/peak_detect_impl.cc b/gr-jay/lib/peak_detect_impl.cc index c63da45..a7cb4ac 100644 --- a/gr-jay/lib/peak_detect_impl.cc +++ b/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 peakPos; - for (int i = 0; i < numPeaks; i++) - { - peakPos.push_back(m_pPeakPos[i]); - } - - Compare cmp(m_pPeakData); - std::sort(peakPos.begin(), peakPos.end(), cmp); - + sort(m_pXd, m_pPeakPos, numPeaks); // ----------------------------------------------------------------- // 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]); - add_item_tag(0, nitems_written(0) + m_pPeakPos[i], tag_key, tag_value, m_tag_id); + tag_value = pmt::from_long(m_pPeakPos_final[i]); + 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 */ diff --git a/gr-jay/lib/peak_detect_impl.h b/gr-jay/lib/peak_detect_impl.h index a3f0f5b..e8d40f5 100644 --- a/gr-jay/lib/peak_detect_impl.h +++ b/gr-jay/lib/peak_detect_impl.h @@ -29,21 +29,7 @@ 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; int m_peak_width_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