From 7c73e82a2b2d94854267a54a49bf119998982fff Mon Sep 17 00:00:00 2001 From: Sylvain Munaut Date: Sat, 15 Oct 2011 00:28:48 +0200 Subject: [PATCH] Initial import of the actual sources/headers Signed-off-by: Sylvain Munaut --- include/osmocom/sdr/cfile.h | 50 +++ include/osmocom/sdr/cxvec.h | 67 ++++ include/osmocom/sdr/cxvec_math.h | 123 +++++++ src/cfile.c | 113 +++++++ src/cxvec.c | 131 ++++++++ src/cxvec_math.c | 532 +++++++++++++++++++++++++++++++ 6 files changed, 1016 insertions(+) create mode 100644 include/osmocom/sdr/cfile.h create mode 100644 include/osmocom/sdr/cxvec.h create mode 100644 include/osmocom/sdr/cxvec_math.h create mode 100644 src/cfile.c create mode 100644 src/cxvec.c create mode 100644 src/cxvec_math.c diff --git a/include/osmocom/sdr/cfile.h b/include/osmocom/sdr/cfile.h new file mode 100644 index 0000000..58c65c5 --- /dev/null +++ b/include/osmocom/sdr/cfile.h @@ -0,0 +1,50 @@ +/* + * cfile.h + * + * Helpers to read .cfile (complex samples from gnuradio) + * + * 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. + */ + +#ifndef __OSMO_SDR_CFILE_H__ +#define __OSMO_SDR_CFILE_H__ + +/*! \defgroup cfile .cfile helpers + * @{ + */ + +/*! \file cfile.h + * \brief Osmocom .cfile helpers header + */ + +#include + +/*! \brief Structure representing a currently mapped .cfile */ +struct cfile { + float complex *data; /*!< \brief Data array (read only !) */ + unsigned int len; /*!< \brief Length (in samples) of the data */ + size_t _blen; /*!< \brief Length (in bytes) of the data */ +}; + +struct cfile *cfile_load(const char *filename); +void cfile_release(struct cfile *cf); + +/*! }@ */ + +#endif /* __OSMO_SDR_CFILE_H__ */ diff --git a/include/osmocom/sdr/cxvec.h b/include/osmocom/sdr/cxvec.h new file mode 100644 index 0000000..284d161 --- /dev/null +++ b/include/osmocom/sdr/cxvec.h @@ -0,0 +1,67 @@ +/* + * cxvec.h + * + * Complex vectors handling + * + * 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. + */ + +#ifndef __OSMO_SDR_CXVEC_H__ +#define __OSMO_SDR_CXVEC_H__ + +/*! \defgroup cxvec Complex vectors + * @{ + */ + +/*! \file cxvec.h + * \brief Osmocom Complex vectors header + */ + +#include + +#define CXVEC_FLG_REAL_ONLY (1<<0) /*!< \brief Real values only */ + +/*! \brief Complex vector */ +struct osmo_cxvec { + int len; /*!< \brief Valid length */ + int max_len; /*!< \brief Maximum length in data field */ + int flags; /*!< \brief Flags, see CXVEC_FLG_xxx */ + float complex *data; /*!< \brief Data field */ + float complex _data[0]; /*!< \brief Optional inline data array */ +}; + +void +osmo_cxvec_init_from_data(struct osmo_cxvec *cv, + float complex *data, int len); + +struct osmo_cxvec * +osmo_cxvec_alloc_from_data(float complex *data, int len); + +struct osmo_cxvec * +osmo_cxvec_alloc(int max_len); + +void +osmo_cxvec_free(struct osmo_cxvec *cv); + +void +osmo_cxvec_dbg_dump(struct osmo_cxvec *cv, const char *fname); + +/*! }@ */ + +#endif /* __OSMO_SDR_CXVEC_H__ */ diff --git a/include/osmocom/sdr/cxvec_math.h b/include/osmocom/sdr/cxvec_math.h new file mode 100644 index 0000000..adf3657 --- /dev/null +++ b/include/osmocom/sdr/cxvec_math.h @@ -0,0 +1,123 @@ +/* + * cxvec_math.h + * + * 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. + */ + +#ifndef __OSMO_SDR_CXVEC_MATH_H__ +#define __OSMO_SDR_CXVEC_MATH_H__ + +/*! \defgroup cxvec_math Complex vectors math and signal processing + * \ingroup cxvec + * @{ + */ + +/*! \file cxvec_math.h + * \brief Osmocom Complex vectors math header + */ + +#include +#include + +#include + + + /* Generic math stuff */ + +#define M_PIf (3.14159265358979323846264338327f) /*!< \brief PI value float */ + +/*! \brief Unnormalized sinc function + * \param[in] x Value for which to compute the sinc function. + * \returns The sinc(x) value + * + * The function is defined as \f$\frac{\sin(x)}{x}\f$ + */ +static inline float +osmo_sinc(float x) +{ + if ((x >= 0.01f) || (x <= -0.01f)) return (sinf(x)/x); + return 1.0f; +} + +/*! \brief Squared norm of a given complex + * \param[in] c Complex number for which to compute the squared norm + * \returns \f$|c|^2\f$ + */ +static inline float +osmo_normsqf(float complex c) +{ + return crealf(c) * crealf(c) + cimagf(c) * cimagf(c); +} + + + /* Complex vector math */ + +struct osmo_cxvec * +osmo_cxvec_scale(struct osmo_cxvec *in, float complex scale, + struct osmo_cxvec *out); + +struct osmo_cxvec * +osmo_cxvec_rotate(struct osmo_cxvec *in, float freq_shift, + struct osmo_cxvec *out); + +/*! \brief Various possible types of convolution span */ +enum osmo_cxvec_conv_type { + /*! \brief Full span (every possible overlap of f onto g) */ + CONV_FULL_SPAN, + /*! \brief Every possible full overlap of f onto g */ + CONV_OVERLAP_ONLY, + /*! \brief Center f sequence on every g sample */ + CONV_NO_DELAY, +}; + +struct osmo_cxvec * +osmo_cxvec_convolve(struct osmo_cxvec *f, struct osmo_cxvec *g, + enum osmo_cxvec_conv_type type, struct osmo_cxvec *out); + +struct osmo_cxvec * +osmo_cxvec_correlate(struct osmo_cxvec *f, struct osmo_cxvec *g, int g_corr_step, + struct osmo_cxvec *out); + +float complex +osmo_cxvec_interpolate_point(struct osmo_cxvec *cv, float pos); + +/*! \brief Various possible peak finding algorithms */ +enum osmo_cxvec_peak_alg { + /*! \brief Weigthed position for the max pwr window */ + PEAK_WEIGH_WIN, + /*! \brief Weighted position of the peak centered window */ + PEAK_WEIGH_WIN_CENTER, + /*! \brief Early-Late balancing around peak */ + PEAK_EARLY_LATE, +}; + +float +osmo_cxvec_peak_energy_find(struct osmo_cxvec *cv, int win_size, + enum osmo_cxvec_peak_alg alg, + float complex *peak_val_p); + +struct osmo_cxvec * +osmo_cxvec_sig_normalize(struct osmo_cxvec *sig, int decim, float freq_shift, + struct osmo_cxvec *out); + +/*! }@ */ + +#endif /* __OSMO_SDR_CXVEC_MATH_H__ */ diff --git a/src/cfile.c b/src/cfile.c new file mode 100644 index 0000000..c94ebf1 --- /dev/null +++ b/src/cfile.c @@ -0,0 +1,113 @@ +/* + * cfile.c + * + * Helpers to read .cfile (complex samples from gnuradio) + * + * 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 cfile + * @{ + */ + +/*! \file cfile.c + * \brief Osmocom .cfile helpers implementation + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +/*! \brief .cfile loader: mmap() the data into memory (read-only) + * \param[in] filename Filename of the .cfile to map + * \returns A structure desribing the mapped data + */ +struct cfile * +cfile_load(const char *filename) +{ + struct cfile *cf; + struct stat st; + int rv, fd = -1; + + cf = calloc(1, sizeof(struct cfile)); + if (!cf) { + perror("calloc"); + goto err; + } + + fd = open(filename, O_RDONLY); + if (fd < 0) { + perror("open"); + goto err; + } + + rv = fstat(fd, &st); + if (rv) { + perror("stat"); + goto err; + } + + cf->_blen = st.st_size; + cf->len = cf->_blen / sizeof(float complex); + + cf->data = mmap(NULL, cf->_blen, PROT_READ, MAP_SHARED, fd, 0); + if (!cf->data) { + perror("mmap"); + goto err; + } + + close(fd); + + return cf; + +err: + if (cf) { + if (fd >= 0) + close(fd); + + free(cf); + } + + return NULL; +} + +/*! \brief Release all resources associated with a mapped .cfile + * \param[in] cf Structure describing the cfile to unmap + */ +void +cfile_release(struct cfile *cf) +{ + int rv; + + rv = munmap(cf->data, cf->_blen); + if (rv) + perror("munmap"); + + free(cf); +} + +/*! }@ */ diff --git a/src/cxvec.c b/src/cxvec.c new file mode 100644 index 0000000..c049b34 --- /dev/null +++ b/src/cxvec.c @@ -0,0 +1,131 @@ +/* + * cxvec.c + * + * Complex vectors handling + * + * 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 + * @{ + */ + +/*! \file cxvec.c + * \brief Osmocom Complex vectors implementation + */ + +#include +#include +#include +#include + +#include + +/*! \brief Initialize a vector structure with a given data array + * \param[out] cv The vector to be initialized + * \param[in] data Pointer to the complex data array + * \param[in] len Number of complex samples + * + * The data is not copied, it is just referenced. + */ +void +osmo_cxvec_init_from_data(struct osmo_cxvec *cv, + float complex *data, int len) +{ + cv->len = cv->max_len = len; + cv->flags = 0; + cv->data = data; +} + +/*! \brief Allocate a complex vector referencing a given data array + * \param[in] data Pointer to the complex data array + * \param[in] len Number of complex samples + * + * The data is not copied, it is just referenced. + */ +struct osmo_cxvec * +osmo_cxvec_alloc_from_data(float complex *data, int len) +{ + struct osmo_cxvec *cv; + + cv = malloc(sizeof(struct osmo_cxvec)); + if (!cv) + return NULL; + + osmo_cxvec_init_from_data(cv, data, len); + + return cv; +} + +/*! \brief Allocate a complex vector of a given maximum length + * \param[in] max_len Maximum length of data + * + * Data array is allocated along with the structure, but is uninitialized. + * Length is set to 0. + */ +struct osmo_cxvec * +osmo_cxvec_alloc(int max_len) +{ + struct osmo_cxvec *cv; + + cv = malloc(sizeof(struct osmo_cxvec) + max_len * sizeof(float complex)); + if (!cv) + return NULL; + + cv->len = 0; + cv->max_len = max_len; + cv->flags = 0; + cv->data = &cv->_data[0]; + + return cv; +} + +/*! \brief Free a complex vector (and possibly associated data) + * \param[in] cv Complex vector to free + * + * Notes: - Can be safely called with NULL + * - If the data was allocated with the vector using + * \ref osmo_cxvec_alloc , it will be free as well. If the + * data was pre-existing ( \ref osmo_cxvec_init_from_data or + * \ref osmo_cxvec_alloc_from_data ) it will not be free'd. + */ +void +osmo_cxvec_free(struct osmo_cxvec *cv) +{ + free(cv); +} + +/*! \brief Save the data contained of a vector into a .cfile for debug + * \param[in] cv Complex vector to save + * \param[in] fname Filename to save the data to + */ +void +osmo_cxvec_dbg_dump(struct osmo_cxvec *cv, const char *fname) +{ + FILE *f = fopen(fname, "wb"); + int rv; + if (!f) + return; + rv = fwrite(cv->data, sizeof(float complex), cv->len, f); + if (rv != cv->len) + fprintf(stderr, "[!] osmo_cxvec_dbg_dump: fwrite failed !\n"); + fclose(f); +} + +/*! }@ */ 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; +} + +/*! }@ */