From 7c73e82a2b2d94854267a54a49bf119998982fff Mon Sep 17 00:00:00 2001 From: Sylvain Munaut Date: Sat, 15 Oct 2011 00:28:48 +0200 Subject: Initial import of the actual sources/headers Signed-off-by: Sylvain Munaut --- src/cxvec_math.c | 532 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 532 insertions(+) create mode 100644 src/cxvec_math.c (limited to 'src/cxvec_math.c') diff --git a/src/cxvec_math.c b/src/cxvec_math.c new file mode 100644 index 0000000..4146ae7 --- /dev/null +++ b/src/cxvec_math.c @@ -0,0 +1,532 @@ +/* + * cxvec_math.c + * + * Complex vectors math and signal processing + * + * Copyright (C) 2011 Sylvain Munaut + * + * All Rights Reserved + * + * This program 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 2 of the License, or + * (at your option) any later version. + * + * This program 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 program; if not, write to the Free Software Foundation, Inc., + * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + */ + +/*! \addtogroup cxvec_math + * @{ + */ + +/*! \file cxvec_math.c + * \brief Osmocom Complex vectors math implementation + */ + +#include +#include +#include +#include + +#include +#include + +/*! \brief Scale a complex vector (multiply by a constant) + * \param[in] in Input complex vector + * \param[in] scale Factor to apply to each sample + * \param[out] out Output complex vector + * \returns The output complex vector (or NULL if error) + * + * \f$out(k) = in(k) \cdot scale\f$ + * + * The output vector parameter 'out' can be NULL to allocate a new + * vector, or can be equal to the 'in' input vector to perform the + * transform in-place. If it's different, it must be long enough + * to contain the result (i.e. in->len) + */ +struct osmo_cxvec * +osmo_cxvec_scale(struct osmo_cxvec *in, float complex scale, + struct osmo_cxvec *out) +{ + int i; + int seq_real; + + seq_real = !!(in->flags & CXVEC_FLG_REAL_ONLY); + + if (!out) + out = osmo_cxvec_alloc(in->len); + else if (out->max_len < in->len) + return NULL; + + if (cimagf(scale) == 0.0f) + { + float scalef = crealf(scale); + + if (seq_real) { + for (i=0; ilen; i++) + out->data[i] = crealf(in->data[i]) * scalef; + } else { + for (i=0; ilen; i++) + out->data[i] = in->data[i] * scalef; + } + } + else + { + if (seq_real) { + for (i=0; ilen; i++) + out->data[i] = crealf(in->data[i]) * scale; + } else { + for (i=0; ilen; i++) + out->data[i] = in->data[i] * scale; + } + + out->flags &= ~CXVEC_FLG_REAL_ONLY; + } + + out->len = in->len; + + return out; +} + +/*! \brief Rotate a complex vector (frequency shift) + * \param[in] in Input complex vector + * \param[in] rps Rotation to apply in radian per sample + * \param[out] out Output complex vector + * \returns The output complex vector (or NULL if error) + * + * \f$out(k) = in(k) \cdot e^{j \cdot rps \cdot k}\f$ + * + * The output vector parameter 'out' can be NULL to allocate a new + * vector, or can be equal to the 'in' input vector to perform the + * transform in-place. If it's different, it must be long enough + * to contain the result (i.e. in->len) + */ +struct osmo_cxvec * +osmo_cxvec_rotate(struct osmo_cxvec *in, float rps, + struct osmo_cxvec *out) +{ + int i; + + if (!out) + out = osmo_cxvec_alloc(in->len); + else if (out->max_len < in->len) + return NULL; + + for (i=0; ilen; i++) + out->data[i] = in->data[i] * cexpf(I*(rps*i)); + + out->len = in->len; + out->flags &= ~CXVEC_FLG_REAL_ONLY; + + return out; +} + +/*! \brief Convolve two complex vectors + * \param[in] f First input complex vector + * \param[in] g Second input complex vector + * \param[in] type The convolution span type + * \returns The output complex vector (or NULL if error) + * + * The convolution of discrete sequences is defined as : + * + * \f$(f * g)[n] = \sum_{m=-\infty}^{\infty} f[m] \; g[n-m]\f$ + * + * Altough the mathematical operation is commutative, the constraint + * of implementation limit this method. Depending on the type of span + * chosen, it might not be and it's always recommended that 'g' be the longer + * sequence. It should not be much of a limitation when this methos is used + * for filtering or pulseshaping : use 'f' as the filter and 'g' as the + * signal. + * + * The output vector parameter 'out' can be NULL to allocate a new + * vector. If it's not NULL, it must be long enough to contain the result + * (length depends on the exact convolution type) + */ +struct osmo_cxvec * +osmo_cxvec_convolve(struct osmo_cxvec *f, struct osmo_cxvec *g, + enum osmo_cxvec_conv_type type, struct osmo_cxvec *out) +{ + int Lf, Lg, Lo, si, ei, i, jf, jg; + int f_real, g_real; + + if (!f || !g) + return NULL; + + f_real = !!(f->flags & CXVEC_FLG_REAL_ONLY); + g_real = !!(g->flags & CXVEC_FLG_REAL_ONLY); + + /* index / size */ + Lf = f->len; + Lg = g->len; + + switch (type) { + case CONV_FULL_SPAN: + si = 0; + Lo = Lf + Lg - 1; + break; + case CONV_OVERLAP_ONLY: + si = Lf; + Lo = abs(Lf-Lg) + 1; + break; + case CONV_NO_DELAY: + si = (Lf >> 1) - ((Lf & 1) ^ 1); + Lo = Lg; + break; + default: + return NULL; + } + + ei = si + Lo; + + /* Output vector */ + if (!out) + out = osmo_cxvec_alloc(Lo); + else if (out->max_len < Lo) + return NULL; + + out->flags = 0; + + /* Do the math */ + if (f_real && g_real) { + for (i=si; i=0; jf++,jg--) + if (jg < Lg) + sum += crealf(f->data[jf]) * crealf(g->data[jg]); + out->data[i-si] = sum; + } + out->flags |= CXVEC_FLG_REAL_ONLY; + } else if (f_real) { + for (i=si; i=0; jf++,jg--) + if (jg < Lg) + sum += crealf(f->data[jf]) * g->data[jg]; + out->data[i-si] = sum; + } + } else if (g_real) { + for (i=si; i=0; jf++,jg--) + if (jg < Lg) + sum += f->data[jf] * crealf(g->data[jg]); + out->data[i-si] = sum; + } + } else { + for (i=si; i=0; jf++,jg--) + if (jg < Lg) + sum += f->data[jf] * g->data[jg]; + out->data[i-si] = sum; + } + } + + out->len = Lo; + + return out; +} + +/*! \brief Cross-correlate two complex vectors + * \param[in] f First input complex vector + * \param[in] g Second input complex vector + * \param[in] g_corr_step Allow for oversampling of 'g' compared to 'f' + * \param[out] out Output complex vector + * \returns The output complex vector (or NULL if error) + * + * The cross-correlation of discrete sequences is defined as : + * + * \f$(f \star g)[n] = \sum_{m=-\infty}^{\infty} f^*[m] \; g[n+m]\f$ + * + * In this implementation, the output vector will be for every n value + * between 0 and (g->len - f->len + 1). This assumes that g is the longer + * sequence and we 'fit' f at every positition inside it. + * + * With the parameter g_corr_step, it's also possible to have a g sequence + * that is oversampled with regard to f. (if g_corr_step > 1) + * + * The output vector parameter 'out' can be NULL to allocate a new + * vector. If it's not NULL, it must be long enough to contain the result + * (i.e. g->len - f->len + 1) + */ +struct osmo_cxvec * +osmo_cxvec_correlate(struct osmo_cxvec *f, struct osmo_cxvec *g, int g_corr_step, + struct osmo_cxvec *out) +{ + int l, m, n, mn; + int f_real, g_real; + + f_real = !!(f->flags & CXVEC_FLG_REAL_ONLY); + g_real = !!(g->flags & CXVEC_FLG_REAL_ONLY); + + l = g->len - (f->len * g_corr_step) + 1; + + if (l < 0) + return NULL; + + if (!out) + out = osmo_cxvec_alloc(l); + else if (out->max_len < l) + return NULL; + + out->flags = 0; + + if (f_real && g_real) { + for (m=0; mlen; n++,mn+=g_corr_step) + v += crealf(f->data[n]) * crealf(g->data[mn]); + out->data[m] = v; + } + out->flags |= CXVEC_FLG_REAL_ONLY; + } else if (f_real) { + for (m=0; mlen; n++,mn+=g_corr_step) + v += crealf(f->data[n]) * g->data[mn]; + out->data[m] = v; + } + } else if (g_real) { + for (m=0; mlen; n++,mn+=g_corr_step) + v += f->data[n] * crealf(g->data[mn]); + out->data[m] = conj(v); + } + } else { + for (m=0; mlen; n++,mn+=g_corr_step) + v += conj(f->data[n]) * g->data[mn]; + out->data[m] = v; + } + } + + out->len = l; + + return out; +} + +/*! \brief Interpolate any fractional position in a vector using sinc filtering + * \param[in] cv Input complex vector + * \param[in] pos Position to interpolate + * + * pos must be >= 0 and < cv->len + */ +float complex +osmo_cxvec_interpolate_point(struct osmo_cxvec *cv, float pos) +{ + const int N = 10; + int b, e, i; + float complex val; + + /* Index */ + i = (int)(floorf(pos)); + b = i - N; + e = i + N + 1; + + if (b < 0) + b = 0; + + if (e >= cv->len) + e = cv->len - 1; + + /* Interpolate */ + if (cv->flags & CXVEC_FLG_REAL_ONLY) { + float valf = 0.0f; + for (i=b; idata[i]) * osmo_sinc(M_PIf * (i - pos)); + val = valf; + } else { + val = 0.0f; + for (i=b; idata[i] * osmo_sinc(M_PIf * (i - pos)); + } + + return val; +} + +/*! \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) + * \param[in] alg Peak detection algorithm to use + * \param[out] peak_val_p Returns interpolated peak value if non-NULL + * \returns Peak position with sub-sample accuracy + */ +float +osmo_cxvec_peak_energy_find(struct osmo_cxvec *cv, int win_size, + enum osmo_cxvec_peak_alg alg, + float complex *peak_val_p) +{ + float val, max_val; + int idx, max_idx, hi; + float he[win_size]; + float peak_pos = 0.0f; + + /* Safety */ + if (cv->len < win_size) + win_size = cv->len; + + /* Scan for the window */ + /* State init */ + val = 0.0f; + max_val = 0.0f; + max_idx = 0; + + /* Prefill energy history array */ + for (hi=0; hidata[hi]); + + /* Main scan */ + for (idx=0; idxlen-win_size; idx++) + { + hi = idx % win_size; + + val -= he[hi]; + he[hi] = osmo_normsqf(cv->data[idx+win_size]); + val += he[hi]; + + if (val > max_val) { + max_val = val; + max_idx = idx + 1; + } + } + + /* Find maximum peak within the window */ + /* (for PEAK_WEIGH_WIN_CENTER & PEAK_EARLY_LATE */ + if (alg == PEAK_WEIGH_WIN_CENTER || alg == PEAK_EARLY_LATE) + { + int mwi = 0; + float mwv = 0.0f; + + for (idx=max_idx; idx<(max_idx+win_size); idx++) { + val = osmo_normsqf(cv->data[idx]); + if (val > mwv) { + mwv = val; + mwi = idx; + } + } + + if (alg == PEAK_WEIGH_WIN_CENTER) { + max_idx = mwi - (win_size >> 1); + + if (max_idx < 0) + max_idx = 0; + if (max_idx > (cv->len - win_size - 1)) + max_idx = cv->len - win_size - 1; + } else { + max_idx = mwi; + } + } + + /* Find the fractional position */ + if (alg == PEAK_WEIGH_WIN || alg == PEAK_WEIGH_WIN_CENTER) + { + float wes = 0.0f; + float es = 0.0f; + + for (idx=max_idx; idx<(max_idx+win_size); idx++) { + val = osmo_normsqf(cv->data[idx]); + wes += val * idx; + es += val; + } + + peak_pos = wes / es; + } + else if (alg == PEAK_EARLY_LATE) + { + float early_idx = max_idx - 1.0f; + float late_idx = max_idx + 1.0f; + float complex early_pt; + float complex late_pt; + float incr = 0.5f; + + while (incr > (1.0f/1024.0f)) + { + early_pt = osmo_cxvec_interpolate_point(cv, early_idx); + late_pt = osmo_cxvec_interpolate_point(cv, late_idx); + + if (osmo_normsqf(early_pt) < osmo_normsqf(late_pt)) + early_idx += incr; + else if (osmo_normsqf(early_pt) > osmo_normsqf(late_pt)) + early_idx -= incr; + else + break; + + incr /= 2.0f; + late_idx = early_idx + 2.0f; + } + + peak_pos = early_idx + 1.0f; + } + + /* Interpolate peak (if asked to) */ + if (peak_val_p) + *peak_val_p = osmo_cxvec_interpolate_point(cv, peak_pos); + + return peak_pos; +} + +/*! \brief 'Normalize' an IQ signal and apply decimation/frequency shift + * \param[in] sig Input complex signal + * \param[in] decim Decimation factor + * \param[in] freq_shift Frequency shift in radian per output sample + * \param[out] out Output complex vector + * \returns The output complex vector (or NULL if error) + * + * The operation performed are DC removal, amplitude normalization (divided + * by the standard deviation), decimation, frequency shift. + * + * The output vector parameter 'out' can be NULL to allocate a new + * vector, or can be equal to the 'in' input vector to perform the + * transform in-place. If it's different, it must be long enough to contain + * the result (i.e. (sig->len + decim - 1) / decim) + */ +struct osmo_cxvec * +osmo_cxvec_sig_normalize(struct osmo_cxvec *sig, int decim, float freq_shift, + struct osmo_cxvec *out) +{ + float complex avg = 0.0f; + float sigma = 0.0f, stddev; + int l, i, j; + + l = sig->len / decim; + + if (!out) + out = osmo_cxvec_alloc(l); + else if (out->max_len < l) + return NULL; + + for (i=0; ilen; i++) + avg += sig->data[i]; + avg /= sig->len; + + for (i=0; ilen; i++) + sigma += osmo_normsqf(sig->data[i] - avg); + sigma /= sig->len; + + stddev = sqrtf(sigma); + if (stddev == 0.0f) + stddev = 1.0f; /* Safety against constant signals */ + + for (i=0, j=0; idata[i] = (sig->data[j] - avg) / stddev; + + out->len = l; + out->flags = sig->flags; + + if (freq_shift != 0.0f) + for (i=0; ilen; i++) + out->data[i] *= cexpf( I * (freq_shift * i) ); + + return out; +} + +/*! }@ */ -- cgit v1.2.3