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.
 
 
 
 
 
 

398 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 <algorithm>
#include <cmath>
#include "cwcanceller.h"
namespace dvbt {
const double CWCanceller::m_twoPi = 6.283185307179586;
const double CWCanceller::m_thresholdDb = 12.0;
const double CWCanceller::m_poleRadius = 0.999;
CWCanceller::CWCanceller() :
m_enabled(true),
m_sampleRate(1.0),
m_bandwidth(1.0)
{
configure(9142857.0, 7.61e6);
}
void CWCanceller::configure(double sampleRate, double bandwidth)
{
const int n = m_fftSize;
m_sampleRate = sampleRate;
m_bandwidth = bandwidth;
// Two blocks back to back (their phase difference refines the frequency) every 20 ms
m_interval = std::max(4 * n, (int) (sampleRate * 0.02));
m_window.resize(n);
for (int i = 0; i < n; i++) {
m_window[i] = 0.5 - 0.5 * std::cos(m_twoPi * (i + 0.5) / n);
}
m_twiddles.resize(n / 2);
for (int i = 0; i < n / 2; i++) {
m_twiddles[i] = std::polar(1.0, -m_twoPi * i / n);
}
m_bitReverse.resize(n);
int bits = 0;
while ((1 << bits) < n) {
bits++;
}
for (int i = 0; i < n; i++)
{
int r = 0;
for (int b = 0; b < bits; b++) {
r |= ((i >> b) & 1) << (bits - 1 - b);
}
m_bitReverse[i] = r;
}
// The bins within the band
m_band.clear();
for (int k = 0; k < n; k++)
{
const int signedK = k < n / 2 ? k : k - n;
if (std::abs(signedK * m_sampleRate / n) < m_bandwidth / 2.0) {
m_band.push_back(k);
}
}
reset();
}
void CWCanceller::reset()
{
m_untilNext = 0;
m_block.assign(2 * m_fftSize, Cf(0.0f, 0.0f));
m_blockFill = 0;
m_power.assign(m_fftSize, 0.0);
m_blockCount = 0;
m_candidates.clear();
m_notches.clear();
}
void CWCanceller::setEnabled(bool enabled)
{
m_enabled = enabled;
if (!enabled) {
reset();
}
}
const CWCanceller::Cf *CWCanceller::process(const Cf *samples, int nbSamples, std::vector<Cf>& out)
{
if (!m_enabled) {
return samples;
}
// Measure the input (not what's been notched, so a notch stays while its interferer does)
const int pair = 2 * m_fftSize;
int i = 0;
while (i < nbSamples)
{
if ((m_blockFill > 0) || (m_untilNext <= 0))
{
const int n = std::min(pair - m_blockFill, nbSamples - i);
std::copy(samples + i, samples + i + n, m_block.begin() + m_blockFill);
m_blockFill += n;
i += n;
if (m_blockFill == pair)
{
measure(m_block.data());
m_blockFill = 0;
m_untilNext = m_interval - pair;
}
}
else
{
const int skip = std::min(m_untilNext, nbSamples - i);
m_untilNext -= skip;
i += skip;
}
}
if (m_notches.empty()) {
return samples;
}
// Each notch: w = x + p w', y = w - z w'
out.assign(samples, samples + nbSamples);
float *y = reinterpret_cast<float *>(out.data());
for (Notch& notch : m_notches)
{
const float zr = notch.m_zr, zi = notch.m_zi, pr = notch.m_pr, pi = notch.m_pi;
float wr = notch.m_wr, wi = notch.m_wi;
for (int k = 0; k < nbSamples; k++)
{
const float xr = y[2 * k], xi = y[2 * k + 1];
const float nr = xr + pr * wr - pi * wi;
const float ni = xi + pr * wi + pi * wr;
y[2 * k] = nr - (zr * wr - zi * wi);
y[2 * k + 1] = ni - (zr * wi + zi * wr);
wr = nr;
wi = ni;
}
notch.m_wr = wr;
notch.m_wi = wi;
}
return out.data();
}
// The power spectrum of both blocks, averaged until there are enough, and the second block's spectrum relative to
// the first's, for the frequencies of the peaks
void CWCanceller::measure(const Cf *block)
{
const int n = m_fftSize;
std::vector<std::complex<double>> *spectra = m_spectra;
for (int b = 0; b < 2; b++)
{
std::vector<std::complex<double>>& fft = spectra[b];
fft.resize(n);
for (int i = 0; i < n; i++) {
fft[m_bitReverse[i]] = std::complex<double>(block[b * n + i]) * m_window[i];
}
for (int size = 2; size <= n; size *= 2)
{
const int half = size / 2;
const int stride = n / size;
for (int start = 0; start < n; start += size)
{
for (int j = 0; j < half; j++)
{
const std::complex<double> t = fft[start + j + half] * m_twiddles[j * stride];
fft[start + j + half] = fft[start + j] - t;
fft[start + j] += t;
}
}
}
for (int k = 0; k < n; k++) {
m_power[k] += std::norm(fft[k]);
}
}
m_blockCount += 2;
// Each pair until there's a notch, so one is soon in place (within about 40 ms), before the receiver locks, as
// starting one afterwards changes the channel the receiver is following
if ((m_blockCount < m_blocks) && !m_notches.empty()) {
return;
}
// Peaks well above the signal's median level in the band, strongest first, each refined: roughly by the shape of
// the window's main lobe, then from the phase advance of a block (a block's length at the frequency)
const std::vector<int>& band = m_band;
std::vector<double>& levels = m_levels;
levels.clear();
for (int k : band) {
levels.push_back(m_power[k]);
}
std::nth_element(levels.begin(), levels.begin() + levels.size() / 2, levels.end());
const double median = levels.empty() ? 0.0 : levels[levels.size() / 2];
const double threshold = median * std::pow(10.0, m_thresholdDb / 10.0);
std::vector<std::pair<double, int>> peaks; // Power, bin
for (int k : band)
{
const double p = m_power[k];
if ((median > 0.0) && (p > threshold) && (p >= m_power[(k + n - 1) % n]) && (p >= m_power[(k + 1) % n])) {
peaks.push_back({p, k});
}
}
std::sort(peaks.begin(), peaks.end(), [](const std::pair<double, int>& a, const std::pair<double, int>& b) { return a.first > b.first; });
std::vector<std::pair<double, double>> detected; // Frequency (cycles per sample), level (dB)
for (const auto& peak : peaks)
{
const int k = peak.second;
const int signedK = k < n / 2 ? k : k - n;
bool near = false;
for (const auto& d : detected)
{
if (std::abs(d.first * n - signedK) < 2 * m_matchBins) {
near = true; // A sidelobe of a stronger one
}
}
if (near || ((int) detected.size() >= m_maxNotches)) {
continue;
}
const double lm = std::log(m_power[(k + n - 1) % n]), l0 = std::log(m_power[k]), lp = std::log(m_power[(k + 1) % n]);
const double denominator = lm - 2.0 * l0 + lp;
const double delta = denominator != 0.0 ? 0.5 * (lm - lp) / denominator : 0.0;
const double coarse = signedK + std::max(-0.5, std::min(0.5, delta)); // Bins
// The second block is n samples on, so the tone has advanced by 2 pi f n: whole cycles from the coarse
// estimate, and the fraction from the phase
const double advance = std::arg(spectra[1][k] * std::conj(spectra[0][k])) / m_twoPi;
const double fine = advance + std::round(coarse - advance);
detected.push_back({fine / n, 10.0 * std::log10(peak.first / median)});
}
// Follow the notches, start new ones for interferers seen twice in a row, and stop those that have gone
std::vector<bool> used(detected.size(), false);
for (Notch& notch : m_notches)
{
notch.m_missed++;
for (size_t d = 0; d < detected.size(); d++)
{
if (!used[d] && (std::abs(detected[d].first - notch.m_frequency) * n < m_matchBins))
{
used[d] = true;
notch.m_missed = 0;
notch.m_level = detected[d].second;
setFrequency(notch, detected[d].first);
}
}
}
m_notches.erase(std::remove_if(m_notches.begin(), m_notches.end(), [](const Notch& notch) {
return notch.m_missed >= m_missesToRemove;
}), m_notches.end());
std::vector<double> candidates;
for (size_t d = 0; d < detected.size(); d++)
{
if (used[d]) {
continue;
}
const bool seen = std::any_of(m_candidates.begin(), m_candidates.end(), [&](double f) {
return std::abs(f - detected[d].first) * n < m_matchBins;
});
if (seen && ((int) m_notches.size() < m_maxNotches))
{
Notch notch{};
notch.m_level = detected[d].second;
setFrequency(notch, detected[d].first);
m_notches.push_back(notch);
}
else
{
candidates.push_back(detected[d].first);
}
}
m_candidates = candidates;
std::fill(m_power.begin(), m_power.end(), 0.0);
m_blockCount = 0;
}
void NotchedCarriers::setNotches(const std::vector<double>& frequencies, double inputRate)
{
m_notches = frequencies;
m_inputRate = inputRate;
if (m_notches.empty())
{
m_carriers.clear();
m_factors.clear();
m_erased.clear();
}
}
void NotchedCarriers::prepare(int kMax, double spacing, double offset)
{
for (int k : m_carriers)
{
if (k < (int) m_erased.size()) {
m_erased[k] = 0;
}
}
m_carriers.clear();
m_factors.clear();
m_erased.resize(kMax + 1, 0);
const int width = (int) std::ceil(50e3 / spacing);
std::vector<std::complex<double>> response;
for (int k = 0; k <= kMax; k++)
{
const double f = (k - kMax / 2) * spacing + offset;
std::complex<double> h(1.0, 0.0);
bool near = false;
for (double notch : m_notches)
{
if (std::abs(f - notch) <= width * spacing)
{
// H = (1 - z e^-jw) / (1 - r z e^-jw), with z the zero, at the carrier's frequency w
const std::complex<double> e = std::polar(1.0, CWCanceller::m_twoPi * (notch - f) / m_inputRate);
h *= (1.0 - e) / (1.0 - CWCanceller::m_poleRadius * e);
near = true;
}
}
if (near)
{
const double h2 = std::norm(h);
m_carriers.push_back(k);
m_factors.push_back(Cf(std::conj(h) / std::max(h2, 0.09)));
m_erased[k] = h2 < 0.25;
}
}
}
void NotchedCarriers::compensate(Cf *y) const
{
for (size_t i = 0; i < m_carriers.size(); i++) {
y[m_carriers[i]] *= m_factors[i];
}
}
void CWCanceller::setFrequency(Notch& notch, double frequency)
{
notch.m_frequency = frequency;
notch.m_zr = (float) std::cos(m_twoPi * frequency);
notch.m_zi = (float) std::sin(m_twoPi * frequency);
notch.m_pr = (float) (m_poleRadius * notch.m_zr);
notch.m_pi = (float) (m_poleRadius * notch.m_zi);
}
} // namespace dvbt