jens
d6390820da
- send peak width change messages git-svn-id: http://moon:8086/svn/software/trunk/projects/GnuRadio@536 b431acfa-c32f-4a4a-93f1-934dc6c82436
408 lines
9.1 KiB
C++
408 lines
9.1 KiB
C++
/* -*- c++ -*- */
|
|
/*
|
|
* Copyright 2019 Jay Arrowfield.
|
|
*
|
|
* This is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3, or (at your option)
|
|
* any later version.
|
|
*
|
|
* This software is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this software; see the file COPYING. If not, write to
|
|
* the Free Software Foundation, Inc., 51 Franklin Street,
|
|
* Boston, MA 02110-1301, USA.
|
|
*/
|
|
|
|
#ifndef INCLUDED_JAY_PEAK_DETECT_IMPL_H
|
|
#define INCLUDED_JAY_PEAK_DETECT_IMPL_H
|
|
|
|
#include <jay/peak_detect.h>
|
|
|
|
namespace gr {
|
|
namespace jay {
|
|
|
|
class peak_detect_impl : public peak_detect
|
|
{
|
|
struct Peak
|
|
{
|
|
Peak()
|
|
: Peak(0, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
|
{
|
|
|
|
}
|
|
Peak(uint64_t _id, int _pos, float bandwidth, float fcenter, float frequency)
|
|
: Peak(_id, _pos, 0, bandwidth, fcenter, frequency, 0.0, 0.0)
|
|
{
|
|
|
|
}
|
|
Peak(uint64_t _id, int _pos, int _pbh, float _frequency_resolution, float _fcenter, float _frequency, float _height_rel, float _height_abs)
|
|
: id(_id)
|
|
, isModified(false)
|
|
, state(0)
|
|
, pos(_pos)
|
|
, pbh(_pbh)
|
|
, frequency_resolution(_frequency_resolution)
|
|
, fcenter(_fcenter)
|
|
, frequency(_frequency)
|
|
, height_rel(_height_rel)
|
|
, height_rel_last(_height_rel)
|
|
, height_abs(_height_abs)
|
|
, height_abs_last(_height_abs)
|
|
{
|
|
|
|
}
|
|
uint64_t id;
|
|
bool isModified;
|
|
int state;
|
|
int pos;
|
|
int pbh;
|
|
float height_rel;
|
|
float height_abs;
|
|
float height_rel_last;
|
|
float height_abs_last;
|
|
float frequency_resolution;
|
|
float fcenter;
|
|
float frequency;
|
|
|
|
bool isInRange(const Peak &other) const
|
|
{
|
|
return (other.pos >= left()) and (other.pos <= right());
|
|
}
|
|
|
|
bool isIntersect(const Peak &other) const
|
|
{
|
|
return (right() >= other.left()) and (other.right() >= left());
|
|
}
|
|
|
|
void heightUpdate_rel(float const *pData, int length)
|
|
{
|
|
if (pbh)
|
|
{
|
|
int newPos = -1;
|
|
float newHeight = -100.f;
|
|
for (int i=left(); i <= right(); i++)
|
|
{
|
|
if (newHeight < pData[i])
|
|
{
|
|
newPos = i;
|
|
newHeight = pData[i];
|
|
}
|
|
}
|
|
|
|
if (pos != newPos)
|
|
{
|
|
isModified = true;
|
|
pos = newPos;
|
|
}
|
|
|
|
height_rel = newHeight;
|
|
if (std::fabs(height_rel_last-newHeight) >= 3.0f)
|
|
{
|
|
isModified = true;
|
|
height_rel_last = height_rel;
|
|
}
|
|
}
|
|
}
|
|
|
|
void heightUpdate_abs(float const *pData, int length)
|
|
{
|
|
if (pbh)
|
|
{
|
|
int newPos = -1;
|
|
float newHeight = -100.f;
|
|
for (int i=left(); i <= right(); i++)
|
|
{
|
|
if (newHeight < pData[i])
|
|
{
|
|
newPos = i;
|
|
newHeight = pData[i];
|
|
}
|
|
}
|
|
|
|
if (pos != newPos)
|
|
{
|
|
isModified = true;
|
|
pos = newPos;
|
|
}
|
|
|
|
height_abs = newHeight;
|
|
if (std::fabs(height_abs_last-newHeight) >= 3.0f)
|
|
{
|
|
isModified = true;
|
|
height_abs_last = height_abs;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
bool makeBox(int pbh_min, int pbh_max, float const *pData, int length)
|
|
{
|
|
pbh = 0;
|
|
int left = pos;
|
|
int right = pos;
|
|
float nom = 10;
|
|
int left_found = 0;
|
|
int right_found = 0;
|
|
while(!(left_found & right_found))
|
|
{
|
|
// Determine peak width left from center (pos)
|
|
if (pData[left] > nom)
|
|
{
|
|
if (left > 0)
|
|
{
|
|
left--;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
left_found = 1;
|
|
}
|
|
|
|
// Determine peak width right from center (pos)
|
|
if (pData[right] > nom)
|
|
{
|
|
if (right < (length-1))
|
|
{
|
|
right++;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
right_found = 1;
|
|
}
|
|
|
|
int pbh_left = pos - left;
|
|
int pbh_right = right - pos;
|
|
|
|
// Take larger peak width
|
|
pbh = std::max(pbh_left, pbh_right);
|
|
|
|
// Ensure peak box is at least pb_min
|
|
pbh = std::max(pbh, pbh_min);
|
|
|
|
if (pbh > pbh_max)
|
|
{
|
|
pbh = 0;
|
|
break;
|
|
}
|
|
}
|
|
// isModified = true;
|
|
return pbh > 0;
|
|
}
|
|
|
|
bool update(const Peak &other)
|
|
{
|
|
if (std::fabs(height_rel_last-other.height_rel) >= 3.0f)
|
|
{
|
|
height_rel_last = other.height_rel;
|
|
isModified = true;
|
|
}
|
|
if (std::fabs(height_abs-other.height_abs) >= 3.0f)
|
|
{
|
|
height_abs_last = other.height_abs;
|
|
isModified = true;
|
|
}
|
|
if (pos != other.pos)
|
|
{
|
|
isModified = true;
|
|
}
|
|
if (pbh != other.pbh)
|
|
{
|
|
isModified = true;
|
|
}
|
|
fcenter = other.fcenter;
|
|
frequency = other.frequency;
|
|
pos = other.pos;
|
|
pbh = other.pbh;
|
|
height_rel = other.height_rel;
|
|
height_abs = other.height_abs;
|
|
return isModified;
|
|
}
|
|
|
|
int left() const
|
|
{
|
|
return pos - pbh;
|
|
}
|
|
|
|
int right() const
|
|
{
|
|
return pos + pbh;
|
|
}
|
|
|
|
pmt::pmt_t msg()
|
|
{
|
|
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("pbh_Hz")), pmt::from_float((float)pbh*frequency_resolution));
|
|
dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("freq_abs_Hz")), pmt::from_float(fcenter + frequency));
|
|
dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("freq_rel_Hz")), pmt::from_float(frequency));
|
|
dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("level_abs")), pmt::from_float(height_abs));
|
|
dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("level_rel")), pmt::from_float(height_rel));
|
|
dict = pmt::dict_add(dict, pmt::string_to_symbol(std::string("state")), pmt::from_long(state));
|
|
isModified = false;
|
|
return dict;
|
|
}
|
|
|
|
void setState(int _state)
|
|
{
|
|
if (_state != state)
|
|
{
|
|
isModified = true;
|
|
}
|
|
state = _state;
|
|
}
|
|
|
|
};
|
|
|
|
private:
|
|
boost::mutex m_mutex;
|
|
|
|
void check_pair(pmt::pmt_t pair)
|
|
{
|
|
pmt::pmt_t key = pmt::car(pair);
|
|
pmt::pmt_t val = pmt::cdr(pair);
|
|
|
|
// Check pair for data
|
|
if (pmt::symbol_to_string(key) == std::string("freq"))
|
|
{
|
|
m_fcenter = pmt::to_float(val);
|
|
}
|
|
}
|
|
void set_msg(pmt::pmt_t msg)
|
|
{
|
|
if (pmt::is_dict(msg))
|
|
{
|
|
pmt::pmt_t items = pmt::dict_items(msg);
|
|
try
|
|
{
|
|
size_t len_items = pmt::length(items);
|
|
for (int i=0; i < len_items; i++)
|
|
{
|
|
pmt::pmt_t pair = pmt::nth(i, items);
|
|
check_pair(pair);
|
|
}
|
|
}
|
|
catch (const std::exception& e)
|
|
{
|
|
check_pair(msg);
|
|
}
|
|
}
|
|
}
|
|
|
|
Peak peakCheck(float const *pData, Peak const &peak, float thresh)
|
|
{
|
|
Peak result;
|
|
|
|
|
|
return result;
|
|
}
|
|
|
|
bool addToListByPos(std::vector<Peak>&list, Peak peak)
|
|
{
|
|
bool found = false;
|
|
for (std::vector<Peak>::iterator it = list.begin(); it != list.end(); it++)
|
|
{
|
|
if (it->pos == peak.pos)
|
|
{
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!found)
|
|
{
|
|
list.push_back(peak);
|
|
}
|
|
return !found;
|
|
}
|
|
|
|
int m_vlen;
|
|
float m_framerate;
|
|
float m_bandwidth;
|
|
float m_fcenter;
|
|
float m_peak_thresh;
|
|
float m_peak_hysteresis;
|
|
int m_peak_width_min;
|
|
int m_peak_width_max;
|
|
float m_alpha_s;
|
|
float m_alpha_m;
|
|
float *m_pX;
|
|
float *m_pXs;
|
|
float *m_pXm;
|
|
float *m_pXd;
|
|
float *m_pXds;
|
|
float *m_pPeakBox_rel;
|
|
float *m_pPeakBox_abs;
|
|
uint64_t m_frameCount;
|
|
pmt::pmt_t m_tag_id;
|
|
std::vector<Peak> m_peakList;
|
|
std::vector<Peak> m_peakHistory;
|
|
std::vector<Peak> temp;
|
|
uint64_t m_peak_id;
|
|
pmt::pmt_t m_msg_port_in;
|
|
pmt::pmt_t m_msg_port_out;
|
|
|
|
public:
|
|
peak_detect_impl(int vlen, float framerate, float bandwidth, float fcenter, float peak_thresh, float peak_hysteresis, float peak_width_min, float peak_width_max, float alpha_s, float alpha_m);
|
|
~peak_detect_impl();
|
|
|
|
// Where all the action really happens
|
|
int general_work (int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
|
|
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
|
|
|
|
private:
|
|
void fill(float *pDst, int length, float value);
|
|
float mean(float *pSrc, int length);
|
|
std::vector<int> findPeakPos(float* pSrc, int length, float thresh);
|
|
|
|
// Callbacks
|
|
void set_bandwidth(float bandwidth_Hz) override;
|
|
void set_centerFrequency(float freq_Hz) override;
|
|
void set_peakThresh(float thresh_dB) override;
|
|
void set_peakHysteresis(float hysteresis_dB) override;
|
|
void set_peakWidthMin(float width_Hz) override;
|
|
void set_peakWidthMax(float width_Hz) override;
|
|
|
|
void sort(std::vector<Peak>&peaks)
|
|
{
|
|
int n = peaks.size();
|
|
do
|
|
{
|
|
int newn = 1;
|
|
for (int i=0; i < (n-1); i++)
|
|
{
|
|
if (peaks[i].height_rel < peaks[i+1].height_rel)
|
|
{
|
|
Peak temp = peaks[i+1];
|
|
peaks[i+1] = peaks[i];
|
|
peaks[i] = temp;
|
|
newn = i+1;
|
|
} // ende if
|
|
} // ende for
|
|
n = newn;
|
|
} while (n > 1);
|
|
}
|
|
|
|
|
|
};
|
|
|
|
} // namespace jay
|
|
} // namespace gr
|
|
|
|
#endif /* INCLUDED_JAY_PEAK_DETECT_IMPL_H */
|
|
|