You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
365 lines
12 KiB
365 lines
12 KiB
/////////////////////////////////////////////////////////////////////////////////// |
|
// Copyright (C) 2026 Jon Beniston, M7RCE <jon@beniston.com> // |
|
// Some code by AI // |
|
// // |
|
// 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 as version 3 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 V3 for more details. // |
|
// // |
|
// You should have received a copy of the GNU General Public License // |
|
// along with this program. If not, see <http://www.gnu.org/licenses/>. // |
|
/////////////////////////////////////////////////////////////////////////////////// |
|
|
|
#include "drmresampler.h" |
|
|
|
#include <algorithm> |
|
#include <cmath> |
|
#include <numeric> |
|
|
|
namespace modemdrm |
|
{ |
|
|
|
namespace |
|
{ |
|
|
|
constexpr double Pi = 3.14159265358979323846; |
|
|
|
double besselI0(double x) |
|
{ |
|
double sum = 1.0; |
|
double term = 1.0; |
|
const double quarterSquare = x * x / 4.0; |
|
for (int k = 1; k < 64; ++k) |
|
{ |
|
term *= quarterSquare / (static_cast<double>(k) * k); |
|
sum += term; |
|
if (term < sum * 1.0e-12) { |
|
break; |
|
} |
|
} |
|
|
|
return sum; |
|
} |
|
|
|
// Kaiser window at position u in [-1, 1]. |
|
double kaiser(double u, double beta) |
|
{ |
|
if (u <= -1.0 || u >= 1.0) { |
|
return 0.0; |
|
} |
|
|
|
return besselI0(beta * std::sqrt(1.0 - u * u)) / besselI0(beta); |
|
} |
|
|
|
double sinc(double x) |
|
{ |
|
return std::abs(x) < 1.0e-12 ? 1.0 : std::sin(Pi * x) / (Pi * x); |
|
} |
|
|
|
// Kaiser design relations for a given stopband attenuation (dB). |
|
double kaiserBeta(double attenuation) |
|
{ |
|
return attenuation > 50.0 ? 0.1102 * (attenuation - 8.7) |
|
: 0.5842 * std::pow(attenuation - 21.0, 0.4) + 0.07886 * (attenuation - 21.0); |
|
} |
|
|
|
constexpr double ResamplerAttenuation = 70.0; |
|
constexpr double FilterAttenuation = 60.0; |
|
|
|
} // namespace |
|
|
|
void FractionalResampler::configure(int inputRate, int outputRate, double passbandHz, double stopbandHz) |
|
{ |
|
m_inputRate = inputRate; |
|
m_outputRate = outputRate; |
|
m_bypass = inputRate <= 0 || outputRate <= 0 || inputRate == outputRate; |
|
m_kernel.clear(); |
|
m_phaseTaps.clear(); |
|
m_phaseStep = 0; |
|
m_halfWidth = 0; |
|
if (m_bypass) |
|
{ |
|
m_passbandHz = outputRate / 2.0; |
|
reset(); |
|
return; |
|
} |
|
|
|
const double nyquist = std::min(inputRate, outputRate) / 2.0; |
|
stopbandHz = std::min(stopbandHz, nyquist); |
|
passbandHz = std::min(passbandHz, stopbandHz * 0.95); |
|
m_passbandHz = passbandHz; |
|
const double cutoff = (passbandHz + stopbandHz) / 2.0; |
|
const double transition = std::max(stopbandHz - passbandHz, 1.0); |
|
const double durationSeconds = (ResamplerAttenuation - 7.95) / (14.36 * transition); |
|
m_halfWidth = std::max(2, static_cast<int>(std::ceil(durationSeconds * inputRate / 2.0))); |
|
const double beta = kaiserBeta(ResamplerAttenuation); |
|
const int points = 2 * m_halfWidth * TableResolution + 2; |
|
m_kernel.resize(static_cast<std::size_t>(points)); |
|
const double scale = 2.0 * cutoff / inputRate; |
|
for (int i = 0; i < points; ++i) |
|
{ |
|
const double offset = static_cast<double>(i) / TableResolution - m_halfWidth; |
|
m_kernel[static_cast<std::size_t>(i)] = static_cast<float>( |
|
scale * sinc(scale * offset) * kaiser(offset / m_halfWidth, beta)); |
|
} |
|
|
|
// Normalise the average DC gain over output phases to unity. |
|
double gain = 0.0; |
|
constexpr int Phases = 64; |
|
for (int phase = 0; phase < Phases; ++phase) |
|
{ |
|
const double fraction = static_cast<double>(phase) / Phases; |
|
for (int k = -m_halfWidth; k <= m_halfWidth + 1; ++k) { |
|
gain += kernel(fraction - k); |
|
} |
|
} |
|
|
|
gain /= Phases; |
|
if (gain > 0.0) |
|
{ |
|
for (float& value : m_kernel) { |
|
value = static_cast<float>(value / gain); |
|
} |
|
} |
|
|
|
// Tabulate the same kernel lookups push() would make for each phase, |
|
// ordered by ascending input index. |
|
const int step = std::gcd(inputRate, outputRate); |
|
const int phases = outputRate / step; |
|
if (phases <= MaximumPhases) |
|
{ |
|
m_phaseStep = step; |
|
const int taps = 2 * m_halfWidth; |
|
m_phaseTaps.resize(static_cast<std::size_t>(phases) * static_cast<std::size_t>(taps)); |
|
for (int phase = 0; phase < phases; ++phase) |
|
{ |
|
const double fraction = static_cast<double>(static_cast<long long>(phase) * step) / m_outputRate; |
|
for (int tap = 0; tap < taps; ++tap) |
|
{ |
|
// Tap t multiplies input whole - halfWidth + 1 + t. |
|
m_phaseTaps[static_cast<std::size_t>(phase) * static_cast<std::size_t>(taps) |
|
+ static_cast<std::size_t>(tap)] = kernel(static_cast<double>(m_halfWidth - 1 - tap) + fraction); |
|
} |
|
} |
|
} |
|
|
|
reset(); |
|
} |
|
|
|
void FractionalResampler::reset() |
|
{ |
|
m_history.clear(); |
|
m_historyStart = 0; |
|
m_received = 0; |
|
m_baseInput = 0; |
|
m_outputCount = 0; |
|
} |
|
|
|
long long FractionalResampler::outputTime(double& fraction) const |
|
{ |
|
const long long scaled = m_outputCount * static_cast<long long>(m_inputRate); |
|
fraction = static_cast<double>(scaled % m_outputRate) / m_outputRate; |
|
return m_baseInput + scaled / m_outputRate; |
|
} |
|
|
|
float FractionalResampler::kernel(double offset) const |
|
{ |
|
const double position = (offset + m_halfWidth) * TableResolution; |
|
if (position < 0.0 || position >= static_cast<double>(m_kernel.size() - 1)) { |
|
return 0.0f; |
|
} |
|
|
|
const std::size_t index = static_cast<std::size_t>(position); |
|
const float fraction = static_cast<float>(position - static_cast<double>(index)); |
|
return m_kernel[index] + fraction * (m_kernel[index + 1] - m_kernel[index]); |
|
} |
|
|
|
void FractionalResampler::push(const std::complex<float>& input, std::vector<std::complex<float>>& output) |
|
{ |
|
if (m_bypass) |
|
{ |
|
output.push_back(input); |
|
return; |
|
} |
|
|
|
m_history.push_back(input); |
|
++m_received; |
|
// Output times are exact rationals, so the timeline cannot drift however |
|
// long the stream runs. Every outputRate outputs span inputRate inputs. |
|
for (;;) |
|
{ |
|
double fraction = 0.0; |
|
const long long whole = outputTime(fraction); |
|
if (whole + m_halfWidth >= m_received) { |
|
break; |
|
} |
|
|
|
std::complex<float> sum(0.0f, 0.0f); |
|
const long long first = std::max<long long>(whole - m_halfWidth + 1, m_historyStart); |
|
const long long last = whole + m_halfWidth; |
|
if (m_phaseStep > 0) |
|
{ |
|
const long long phase = (m_outputCount * static_cast<long long>(m_inputRate)) % m_outputRate / m_phaseStep; |
|
const float* taps = m_phaseTaps.data() + phase * 2 * m_halfWidth; |
|
const long long firstTapInput = whole - m_halfWidth + 1; |
|
for (long long k = first; k <= last; ++k) { |
|
sum += taps[k - firstTapInput] * m_history[static_cast<std::size_t>(k - m_historyStart)]; |
|
} |
|
} |
|
else |
|
{ |
|
for (long long k = first; k <= last; ++k) |
|
{ |
|
const float weight = kernel(static_cast<double>(whole - k) + fraction); |
|
sum += weight * m_history[static_cast<std::size_t>(k - m_historyStart)]; |
|
} |
|
} |
|
|
|
output.push_back(sum); |
|
if (++m_outputCount == m_outputRate) |
|
{ |
|
m_baseInput += m_inputRate; |
|
m_outputCount = 0; |
|
} |
|
} |
|
|
|
// Keep only the history the next output can still reach. |
|
double fraction = 0.0; |
|
const long long keepFrom = outputTime(fraction) - m_halfWidth + 1; |
|
if (keepFrom - m_historyStart > 4096) |
|
{ |
|
m_history.erase(m_history.begin(), |
|
m_history.begin() + static_cast<std::ptrdiff_t>(keepFrom - m_historyStart)); |
|
m_historyStart = keepFrom; |
|
} |
|
} |
|
|
|
void BandFilter::configure(int sampleRate) |
|
{ |
|
m_sampleRate = sampleRate; |
|
int taps = std::max(31, static_cast<int>(std::lround(sampleRate / 750.0))); |
|
if ((taps & 1) == 0) { |
|
++taps; |
|
} |
|
|
|
m_taps = taps; |
|
m_fftSize = 1; |
|
while (m_fftSize < 4 * m_taps) { |
|
m_fftSize <<= 1; |
|
} |
|
|
|
m_blockSize = m_fftSize - (m_taps - 1); |
|
m_forward.configure(m_fftSize, false); |
|
m_inverse.configure(m_fftSize, true); |
|
m_buffer.assign(static_cast<std::size_t>(m_fftSize), {0.0f, 0.0f}); |
|
m_spectrum.assign(static_cast<std::size_t>(m_fftSize), {0.0f, 0.0f}); |
|
m_result.assign(static_cast<std::size_t>(m_fftSize), {0.0f, 0.0f}); |
|
setBand(-sampleRate / 2.0, sampleRate / 2.0); |
|
reset(); |
|
} |
|
|
|
double BandFilter::transitionHz() const |
|
{ |
|
return m_taps > 1 |
|
? (FilterAttenuation - 7.95) * m_sampleRate / (14.36 * (m_taps - 1)) : 0.0; |
|
} |
|
|
|
void BandFilter::setBand(double lowHz, double highHz) |
|
{ |
|
if (m_sampleRate <= 0 || m_taps <= 0) { |
|
return; |
|
} |
|
|
|
if (highHz < lowHz) { |
|
std::swap(lowHz, highHz); |
|
} |
|
|
|
m_lowHz = lowHz; |
|
m_highHz = highHz; |
|
const int middle = (m_taps - 1) / 2; |
|
std::vector<std::complex<float>> taps(static_cast<std::size_t>(m_fftSize), {0.0f, 0.0f}); |
|
const double halfWidth = (highHz - lowHz) / 2.0 + transitionHz() / 2.0; |
|
if (halfWidth >= m_sampleRate / 2.0) { |
|
// Everything passes: a pure delay keeps the timing identical. |
|
taps[static_cast<std::size_t>(middle)] = {1.0f, 0.0f}; |
|
} |
|
else |
|
{ |
|
const double centre = (lowHz + highHz) / 2.0; |
|
const double scale = 2.0 * halfWidth / m_sampleRate; |
|
const double beta = kaiserBeta(FilterAttenuation); |
|
for (int n = 0; n < m_taps; ++n) |
|
{ |
|
const double offset = static_cast<double>(n - middle); |
|
const double lowpass = scale * sinc(scale * offset) |
|
* kaiser(offset / (middle + 1), beta); |
|
const double phase = 2.0 * Pi * centre * offset / m_sampleRate; |
|
taps[static_cast<std::size_t>(n)] = std::complex<float>( |
|
static_cast<float>(lowpass * std::cos(phase)), |
|
static_cast<float>(lowpass * std::sin(phase))); |
|
} |
|
|
|
// Unity gain at the band centre. |
|
std::complex<double> gain(0.0, 0.0); |
|
for (int n = 0; n < m_taps; ++n) |
|
{ |
|
const double phase = -2.0 * Pi * centre * (n - middle) / m_sampleRate; |
|
gain += std::complex<double>(taps[static_cast<std::size_t>(n)]) |
|
* std::polar(1.0, phase); |
|
} |
|
|
|
const float magnitude = static_cast<float>(std::abs(gain)); |
|
if (magnitude > 0.0f) |
|
{ |
|
for (int n = 0; n < m_taps; ++n) { |
|
taps[static_cast<std::size_t>(n)] /= magnitude; |
|
} |
|
} |
|
} |
|
|
|
m_response.resize(static_cast<std::size_t>(m_fftSize)); |
|
m_forward.transform(taps.data(), m_response.data()); |
|
const float normalise = 1.0f / static_cast<float>(m_fftSize); |
|
for (auto& value : m_response) { |
|
value *= normalise; |
|
} |
|
} |
|
|
|
void BandFilter::reset() |
|
{ |
|
std::fill(m_buffer.begin(), m_buffer.end(), std::complex<float>(0.0f, 0.0f)); |
|
m_fill = 0; |
|
} |
|
|
|
void BandFilter::push(const std::complex<float>& input, std::vector<std::complex<float>>& output) |
|
{ |
|
if (m_fftSize == 0) |
|
{ |
|
output.push_back(input); |
|
return; |
|
} |
|
|
|
const int overlap = m_taps - 1; |
|
m_buffer[static_cast<std::size_t>(overlap + m_fill)] = input; |
|
if (++m_fill < m_blockSize) { |
|
return; |
|
} |
|
|
|
m_forward.transform(m_buffer.data(), m_spectrum.data()); |
|
for (int i = 0; i < m_fftSize; ++i) { |
|
m_spectrum[static_cast<std::size_t>(i)] *= m_response[static_cast<std::size_t>(i)]; |
|
} |
|
|
|
m_inverse.transform(m_spectrum.data(), m_result.data()); |
|
output.insert(output.end(), m_result.begin() + overlap, m_result.end()); |
|
std::copy(m_buffer.end() - overlap, m_buffer.end(), m_buffer.begin()); |
|
m_fill = 0; |
|
} |
|
|
|
} // namespace modemdrm
|
|
|