diff options
Diffstat (limited to 'common')
| -rw-r--r-- | common/polyphase_resampler.cpp | 214 | ||||
| -rw-r--r-- | common/polyphase_resampler.h | 45 |
2 files changed, 259 insertions, 0 deletions
diff --git a/common/polyphase_resampler.cpp b/common/polyphase_resampler.cpp new file mode 100644 index 00000000..4605b732 --- /dev/null +++ b/common/polyphase_resampler.cpp @@ -0,0 +1,214 @@ + +#include "polyphase_resampler.h" + +#include <algorithm> +#include <cmath> + + +namespace { + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define EPSILON 1e-9 + +using uint = unsigned int; + +/* This is the normalized cardinal sine (sinc) function. + * + * sinc(x) = { 1, x = 0 + * { sin(pi x) / (pi x), otherwise. + */ +double Sinc(const double x) +{ + if(std::abs(x) < EPSILON) + return 1.0; + return std::sin(M_PI * x) / (M_PI * x); +} + +/* The zero-order modified Bessel function of the first kind, used for the + * Kaiser window. + * + * I_0(x) = sum_{k=0}^inf (1 / k!)^2 (x / 2)^(2 k) + * = sum_{k=0}^inf ((x / 2)^k / k!)^2 + */ +double BesselI_0(const double x) +{ + double term, sum, x2, y, last_sum; + int k; + + // Start at k=1 since k=0 is trivial. + term = 1.0; + sum = 1.0; + x2 = x/2.0; + k = 1; + + // Let the integration converge until the term of the sum is no longer + // significant. + do { + y = x2 / k; + k++; + last_sum = sum; + term *= y * y; + sum += term; + } while(sum != last_sum); + return sum; +} + +/* Calculate a Kaiser window from the given beta value and a normalized k + * [-1, 1]. + * + * w(k) = { I_0(B sqrt(1 - k^2)) / I_0(B), -1 <= k <= 1 + * { 0, elsewhere. + * + * Where k can be calculated as: + * + * k = i / l, where -l <= i <= l. + * + * or: + * + * k = 2 i / M - 1, where 0 <= i <= M. + */ +double Kaiser(const double b, const double k) +{ + if(!(k >= -1.0 && k <= 1.0)) + return 0.0; + return BesselI_0(b * std::sqrt(1.0 - k*k)) / BesselI_0(b); +} + +// Calculates the greatest common divisor of a and b. +uint Gcd(uint x, uint y) +{ + while(y > 0) + { + uint z{y}; + y = x % y; + x = z; + } + return x; +} + +/* Calculates the size (order) of the Kaiser window. Rejection is in dB and + * the transition width is normalized frequency (0.5 is nyquist). + * + * M = { ceil((r - 7.95) / (2.285 2 pi f_t)), r > 21 + * { ceil(5.79 / 2 pi f_t), r <= 21. + * + */ +uint CalcKaiserOrder(const double rejection, const double transition) +{ + double w_t = 2.0 * M_PI * transition; + if(rejection > 21.0) + return static_cast<uint>(std::ceil((rejection - 7.95) / (2.285 * w_t))); + return static_cast<uint>(std::ceil(5.79 / w_t)); +} + +// Calculates the beta value of the Kaiser window. Rejection is in dB. +double CalcKaiserBeta(const double rejection) +{ + if(rejection > 50.0) + return 0.1102 * (rejection - 8.7); + if(rejection >= 21.0) + return (0.5842 * std::pow(rejection - 21.0, 0.4)) + + (0.07886 * (rejection - 21.0)); + return 0.0; +} + +/* Calculates a point on the Kaiser-windowed sinc filter for the given half- + * width, beta, gain, and cutoff. The point is specified in non-normalized + * samples, from 0 to M, where M = (2 l + 1). + * + * w(k) 2 p f_t sinc(2 f_t x) + * + * x -- centered sample index (i - l) + * k -- normalized and centered window index (x / l) + * w(k) -- window function (Kaiser) + * p -- gain compensation factor when sampling + * f_t -- normalized center frequency (or cutoff; 0.5 is nyquist) + */ +double SincFilter(const uint l, const double b, const double gain, const double cutoff, + const uint i) +{ + const double x{static_cast<double>(i) - l}; + return Kaiser(b, x / l) * 2.0 * gain * cutoff * Sinc(2.0 * cutoff * x); +} + +} // namespace + +// Calculate the resampling metrics and build the Kaiser-windowed sinc filter +// that's used to cut frequencies above the destination nyquist. +void PPhaseResampler::init(const uint srcRate, const uint dstRate) +{ + const uint gcd{Gcd(srcRate, dstRate)}; + mP = dstRate / gcd; + mQ = srcRate / gcd; + + /* The cutoff is adjusted by half the transition width, so the transition + * ends before the nyquist (0.5). Both are scaled by the downsampling + * factor. + */ + double cutoff, width; + if(mP > mQ) + { + cutoff = 0.475 / mP; + width = 0.05 / mP; + } + else + { + cutoff = 0.475 / mQ; + width = 0.05 / mQ; + } + // A rejection of -180 dB is used for the stop band. Round up when + // calculating the left offset to avoid increasing the transition width. + const uint l{(CalcKaiserOrder(180.0, width)+1) / 2}; + const double beta{CalcKaiserBeta(180.0)}; + mM = l*2 + 1; + mL = l; + mF.resize(mM); + for(uint i{0};i < mM;i++) + mF[i] = SincFilter(l, beta, mP, cutoff, i); +} + +// Perform the upsample-filter-downsample resampling operation using a +// polyphase filter implementation. +void PPhaseResampler::process(const uint inN, const double *in, const uint outN, double *out) +{ + if(outN == 0) + return; + + const uint p{mP}, q{mQ}, m{mM}, l{mL}; + + // Handle in-place operation. + std::vector<double> workspace; + double *work{out}; + if(work == in) + { + workspace.resize(outN); + work = workspace.data(); + } + + // Resample the input. + const double *f{mF.data()}; + for(uint i{0};i < outN;i++) + { + double r{0.0}; + // Input starts at l to compensate for the filter delay. This will + // drop any build-up from the first half of the filter. + uint j_f{(l + (q * i)) % p}; + uint j_s{(l + (q * i)) / p}; + while(j_f < m) + { + // Only take input when 0 <= j_s < inN. This single unsigned + // comparison catches both cases. + if(j_s < inN) + r += f[j_f] * in[j_s]; + j_f += p; + j_s--; + } + work[i] = r; + } + // Clean up after in-place operation. + if(work != out) + std::copy_n(work, outN, out); +} diff --git a/common/polyphase_resampler.h b/common/polyphase_resampler.h new file mode 100644 index 00000000..e9833d12 --- /dev/null +++ b/common/polyphase_resampler.h @@ -0,0 +1,45 @@ +#ifndef POLYPHASE_RESAMPLER_H +#define POLYPHASE_RESAMPLER_H + +#include <vector> + + +/* This is a polyphase sinc-filtered resampler. It is built for very high + * quality results, rather than real-time performance. + * + * Upsample Downsample + * + * p/q = 3/2 p/q = 3/5 + * + * M-+-+-+-> M-+-+-+-> + * -------------------+ ---------------------+ + * p s * f f f f|f| | p s * f f f f f | + * | 0 * 0 0 0|0|0 | | 0 * 0 0 0 0|0| | + * v 0 * 0 0|0|0 0 | v 0 * 0 0 0|0|0 | + * s * f|f|f f f | s * f f|f|f f | + * 0 * |0|0 0 0 0 | 0 * 0|0|0 0 0 | + * --------+=+--------+ 0 * |0|0 0 0 0 | + * d . d .|d|. d . d ----------+=+--------+ + * d . . . .|d|. . . . + * q-> + * q-+-+-+-> + * + * P_f(i,j) = q i mod p + pj + * P_s(i,j) = floor(q i / p) - j + * d[i=0..N-1] = sum_{j=0}^{floor((M - 1) / p)} { + * { f[P_f(i,j)] s[P_s(i,j)], P_f(i,j) < M + * { 0, P_f(i,j) >= M. } + */ + +struct PPhaseResampler { + using uint = unsigned int; + + void init(const uint srcRate, const uint dstRate); + void process(const uint inN, const double *in, const uint outN, double *out); + +private: + uint mP, mQ, mM, mL; + std::vector<double> mF; +}; + +#endif /* POLYPHASE_RESAMPLER_H */ |