diff options
author | Sylvain Munaut <tnt@246tNt.com> | 2016-09-16 13:39:31 -0600 |
---|---|---|
committer | Sylvain Munaut <tnt@246tNt.com> | 2016-12-30 09:39:22 +0100 |
commit | e82886ad0e13bb012e4a244df2ce6d84cd277763 (patch) | |
tree | 312435bcf6766a44f7785cf0fcf97e9e620c5a28 | |
parent | c3eabba5d240b99a48d8696b36a41dc17727a4e4 (diff) |
cxvec/math: Add helper to find the N highest energy values in vector
Signed-off-by: Sylvain Munaut <tnt@246tNt.com>
-rw-r--r-- | include/osmocom/dsp/cxvec_math.h | 3 | ||||
-rw-r--r-- | src/cxvec_math.c | 45 |
2 files changed, 48 insertions, 0 deletions
diff --git a/include/osmocom/dsp/cxvec_math.h b/include/osmocom/dsp/cxvec_math.h index 7e372c4..77e9ab7 100644 --- a/include/osmocom/dsp/cxvec_math.h +++ b/include/osmocom/dsp/cxvec_math.h @@ -103,6 +103,9 @@ osmo_cxvec_correlate(const struct osmo_cxvec *f, const struct osmo_cxvec *g, float complex osmo_cxvec_interpolate_point(const struct osmo_cxvec *cv, float pos); +int +osmo_cxvec_peaks_scan(const struct osmo_cxvec *cv, int *peaks_idx, int N); + /*! \brief Various possible peak finding algorithms */ enum osmo_cxvec_peak_alg { /*! \brief Weigthed position for the max pwr window */ diff --git a/src/cxvec_math.c b/src/cxvec_math.c index 9f81aa5..9dc6157 100644 --- a/src/cxvec_math.c +++ b/src/cxvec_math.c @@ -428,6 +428,51 @@ osmo_cxvec_interpolate_point(const struct osmo_cxvec *cv, float pos) return val; } +/*! \brief Find the index of the N highest energy (\f$|x|^2\f$) peaks + * \param[in] cv Input complex vector + * \param[out] peaks_idx Return array of the peak indexes + * \param[in] Size of the \ref peaks_idx return array + * \returns Number of peaks (will be N if there is enough points) + */ +int +osmo_cxvec_peaks_scan(const struct osmo_cxvec *cv, int *peaks_idx, int N) +{ + int i, j; + float peaks_mag[N]; + + /* Pre-init */ + for (i=0; i<N; i++) { + peaks_idx[i] = -1; + peaks_mag[i] = 0.0f; + } + + /* Scan all */ + for (i=0; i<cv->len; i++) + { + /* Magnitude */ + float mag = osmo_normsqf(cv->data[i]); + + /* Worth it ? */ + if (mag < peaks_mag[N-1]) + continue; + + /* Find insertion point in sorted array and pre-move */ + for (j=N-1; j>0; j--) { + if (mag < peaks_mag[j-1]) + break; + + peaks_mag[j] = peaks_mag[j-1]; + peaks_idx[j] = peaks_idx[j-1]; + } + + /* Do the insert */ + peaks_mag[j] = mag; + peaks_idx[j] = i; + } + + return i < N ? i : N; +} + /*! \brief Find the maximum energy (\f$|x|^2\f$) peak in a sequence * \param[in] cv Input complex vector * \param[in] win_size Size of the window (for algorithms using windows) |