resample.cpp (2462B)
1 2 #include <assert.h> 3 #include <math.h> 4 #include <algorithm> 5 6 #include "filters/resample.hpp" 7 8 using namespace bogaudio::dsp; 9 10 void LPFDecimator::setParams(float sampleRate, int factor) { 11 _factor = factor; 12 _filter.setParams( 13 MultipoleFilter::LP_TYPE, 14 4, 15 factor * sampleRate, 16 0.45f * sampleRate, 17 0 18 ); 19 } 20 21 float LPFDecimator::next(const float* buf) { 22 float s = 0.0f; 23 for (int i = 0; i < _factor; ++i) { 24 s = _filter.next(buf[i]); 25 } 26 return s; 27 } 28 29 30 // cascaded integrator–comb decimator: https://en.wikipedia.org/wiki/Cascaded_integrator%E2%80%93comb_filter 31 CICDecimator::CICDecimator(int stages, int factor) { 32 assert(stages > 0); 33 _stages = stages; 34 _integrators = new T[_stages + 1] {}; 35 _combs = new T[_stages] {}; 36 setParams(0.0f, factor); 37 } 38 39 CICDecimator::~CICDecimator() { 40 delete[] _integrators; 41 delete[] _combs; 42 } 43 44 void CICDecimator::setParams(float _sampleRate, int factor) { 45 assert(factor > 0); 46 if (_factor != factor) { 47 _factor = factor; 48 _gainCorrection = 1.0f / (float)(pow(_factor, _stages)); 49 } 50 } 51 52 float CICDecimator::next(const float* buf) { 53 for (int i = 0; i < _factor; ++i) { 54 _integrators[0] = buf[i] * scale; 55 for (int j = 1; j <= _stages; ++j) { 56 _integrators[j] += _integrators[j - 1]; 57 } 58 } 59 T s = _integrators[_stages]; 60 for (int i = 0; i < _stages; ++i) { 61 T t = s; 62 s -= _combs[i]; 63 _combs[i] = t; 64 } 65 return _gainCorrection * (s / (float)scale); 66 } 67 68 69 CICInterpolator::CICInterpolator(int stages, int factor) { 70 assert(stages > 0); 71 _stages = stages; 72 _integrators = new T[_stages + 1] {}; 73 _combs = new T[_stages] {}; 74 _buffer = NULL; 75 setParams(0.0f, factor); 76 } 77 78 CICInterpolator::~CICInterpolator() { 79 delete[] _integrators; 80 delete[] _combs; 81 delete[] _buffer; 82 } 83 84 void CICInterpolator::setParams(float _sampleRate, int factor) { 85 assert(factor > 0); 86 if (_factor != factor) { 87 _factor = factor; 88 _gainCorrection = 1.0f / 512.0f; // (float)(pow(_factor, _stages / 2)); 89 if (_buffer) { 90 delete[] _buffer; 91 } 92 _buffer = new T[_factor] {}; 93 } 94 } 95 96 void CICInterpolator::next(float sample, float* buf) { 97 T s = sample * scale; 98 for (int i = 0; i < _stages; ++i) { 99 T t = s; 100 s -= _combs[i]; 101 _combs[i] = t; 102 } 103 104 std::fill(_buffer, _buffer + _factor, (T)0); 105 _buffer[0] = s; 106 for (int i = 0; i < _factor; ++i) { 107 _integrators[0] = _buffer[i]; 108 for (int j = 1; j <= _stages; ++j) { 109 _integrators[j] += _integrators[j - 1]; 110 } 111 buf[i] = _gainCorrection * (_integrators[_stages] / (float)scale); 112 } 113 }