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.
950 lines
30 KiB
950 lines
30 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 "dvbs.h" |
|
#include "dvbsfrontend.h" |
|
#include "resampler.h" |
|
|
|
namespace dvbs { |
|
|
|
// Raised cosine pulse (the transmit and receive filters together) |
|
double FrontEnd::raisedCosine(double t, double a) |
|
{ |
|
const double x = 2.0 * a * t; |
|
const double sinc = std::fabs(t) < 1e-9 ? 1.0 : std::sin(m_pi * t) / (m_pi * t); |
|
|
|
if (std::fabs(std::fabs(x) - 1.0) < 1e-9) { |
|
return m_pi / 4.0 * (a > 0.0 ? std::sin(m_pi / (2.0 * a)) / (m_pi / (2.0 * a)) : 1.0); |
|
} |
|
|
|
return sinc * std::cos(m_pi * a * t) / (1.0 - x * x); |
|
} |
|
|
|
double FrontEnd::besselI0(double x) |
|
{ |
|
double sum = 1.0, term = 1.0; |
|
|
|
for (int k = 1; k < 50; k++) |
|
{ |
|
term *= (x / (2.0 * k)) * (x / (2.0 * k)); |
|
sum += term; |
|
|
|
if (term < sum * 1e-12) { |
|
break; |
|
} |
|
} |
|
|
|
return sum; |
|
} |
|
|
|
FrontEnd::FrontEnd() : |
|
m_inputRate(2e6), |
|
m_symbolRate(1e6), |
|
m_rollOff(0.35), |
|
m_samplesPerSymbol(2.0), |
|
m_taps(0), |
|
m_ncoFrequency(0.0), |
|
m_ncoPhase(0.0), |
|
m_frequencyKnown(false), |
|
m_spectrumFill(0), |
|
m_estimateBlocks(0), |
|
m_untilDisplayBlock(0), |
|
m_time(0.0), |
|
m_step(1.0), |
|
m_onSymbol(true), |
|
m_haveLast(false), |
|
m_integrator(0.0), |
|
m_kp(0.0), |
|
m_ki(0.0), |
|
m_bandwidth(2e-3), |
|
m_detectorGain(1.0), |
|
m_power(1.0), |
|
m_powerValid(false), |
|
m_inputPowerSum(0.0), |
|
m_inputPowerCount(0), |
|
m_eqEnabled(true), |
|
m_eqLocked(false), |
|
m_eqEsN0(10.0), |
|
m_eqBlocks(0) |
|
{ |
|
m_window.resize(m_fftSize); |
|
m_twiddles.resize(m_fftSize / 2); |
|
m_bitReverse.resize(m_fftSize); |
|
int bits = 0; |
|
|
|
while ((1 << bits) < m_fftSize) { |
|
bits++; |
|
} |
|
|
|
for (int i = 0; i < m_fftSize; i++) |
|
{ |
|
m_window[i] = 0.5 - 0.5 * std::cos(2.0 * m_pi * i / m_fftSize); |
|
int r = 0; |
|
|
|
for (int b = 0; b < bits; b++) { |
|
r |= ((i >> b) & 1) << (bits - 1 - b); |
|
} |
|
|
|
m_bitReverse[i] = r; |
|
} |
|
|
|
for (int i = 0; i < m_fftSize / 2; i++) { |
|
m_twiddles[i] = std::polar(1.0, -2.0 * m_pi * i / m_fftSize); |
|
} |
|
|
|
configure(m_inputRate, m_symbolRate, m_rollOff); |
|
} |
|
|
|
void FrontEnd::configure(double inputRate, double symbolRate, double rollOff) |
|
{ |
|
m_inputRate = inputRate; |
|
m_symbolRate = symbolRate; |
|
m_rollOff = rollOff; |
|
m_samplesPerSymbol = inputRate / symbolRate; |
|
m_step = m_samplesPerSymbol / 2.0; |
|
m_taps = 0; |
|
m_eqGainDb.clear(); |
|
makeFilter(); |
|
computeGains(); |
|
reset(); |
|
} |
|
|
|
void FrontEnd::setRollOff(double rollOff) |
|
{ |
|
if (rollOff == m_rollOff) { |
|
return; |
|
} |
|
|
|
// The timing and carrier carry on: only the filter changes |
|
m_rollOff = rollOff; |
|
makeFilter(); |
|
computeGains(); |
|
} |
|
|
|
void FrontEnd::makeFilter() |
|
{ |
|
// A low roll-off's pulse falls off over about 1 / roll-off symbols: spanned twice that (at 0.05 and 0.10, MER 19.7 |
|
// to 19.8 dB at Es/N0 20, against 19.9 at 0.35; once that, 18.7 to 19.0), at the cost of as many more taps |
|
const double span = std::max(m_filterSpan, 2.0 / std::max(0.01, m_rollOff)); |
|
int taps = (int) std::ceil(span * m_samplesPerSymbol); |
|
taps = std::max(8, std::min(1024, (taps + 3) & ~3)); |
|
const int oldTaps = m_taps; |
|
m_taps = taps; |
|
const double half = m_taps / 2.0; |
|
const double beta = 5.0; |
|
const double i0Beta = besselI0(beta); |
|
const double sps = m_samplesPerSymbol; |
|
const double a = m_rollOff; |
|
|
|
if (m_eqGainDb.empty()) |
|
{ |
|
dvbt::makePolyphaseFilter(m_taps, m_phases, [half, beta, i0Beta, sps, a](double x) { |
|
const double arg = x / half; |
|
const double window = std::fabs(arg) < 1.0 ? besselI0(beta * std::sqrt(1.0 - arg * arg)) / i0Beta : 0.0; |
|
return rootRaisedCosine(x / sps, a) * window; |
|
}, m_pairs); |
|
} |
|
else |
|
{ |
|
// Equalised: the root raised cosine's frequency response times the correction (both even, so the impulse is |
|
// real), its impulse the integral of that by cosines, from points across the signal's band |
|
const double outer = (1.0 + a) / 2.0; // Of the symbol rate |
|
const double binRate = m_inputRate / m_fftSize / m_symbolRate; |
|
const double spacing = outer / m_eqFrequencies / sps; // Of the points, cycles an input sample |
|
std::vector<double> response(m_eqFrequencies); |
|
|
|
for (int k = 0; k < m_eqFrequencies; k++) |
|
{ |
|
const double f = (k + 0.5) * outer / m_eqFrequencies; // Of the symbol rate |
|
const double inner = (1.0 - a) / 2.0; |
|
const double rc = f <= inner ? 1.0 : f >= outer ? 0.0 : 0.5 * (1.0 + std::cos(m_pi * (f - inner) / a)); |
|
const int bin = std::min((int) m_eqGainDb.size() - 1, (int) std::lround(f / binRate)); |
|
response[k] = std::sqrt(rc) * std::pow(10.0, m_eqGainDb[bin] / 20.0); |
|
} |
|
|
|
// Point k's at (k + 0.5) spacing: its cosine the real part of a phasor turned by 2 pi spacing x a point (in |
|
// double, to about 1e-14 over the 128), rather than a cosine each, taps x phases x 128 of them |
|
dvbt::makePolyphaseFilter(m_taps, m_phases, [&](double x) { |
|
const double arg = x / half; |
|
|
|
if (std::fabs(arg) >= 1.0) { |
|
return 0.0; |
|
} |
|
|
|
const double window = besselI0(beta * std::sqrt(1.0 - arg * arg)) / i0Beta; |
|
const double stepRe = std::cos(2.0 * m_pi * spacing * x), stepIm = std::sin(2.0 * m_pi * spacing * x); |
|
double re = std::cos(m_pi * spacing * x), im = std::sin(m_pi * spacing * x); |
|
double sum = 0.0; |
|
|
|
for (int k = 0; k < m_eqFrequencies; k++) |
|
{ |
|
sum += response[k] * re; |
|
const double t = re * stepRe - im * stepIm; |
|
im = re * stepIm + im * stepRe; |
|
re = t; |
|
} |
|
|
|
return sum * window; |
|
}, m_pairs); |
|
} |
|
|
|
// The output's time is its centre's (the filter's window starts taps / 2 - 1 before it, whatever its length), kept |
|
// so the timing doesn't jump. A longer filter's first taps reach before the history kept: zeros there (the pulse's |
|
// tails, a small transient), and the time moved with the samples (left, the symbols jumped 5 at 2.5 samples a |
|
// symbol as a low roll-off's longer filter came in, and the lock was lost). A shorter one's are within it |
|
const int grow = (m_taps / 2 - 1) - (oldTaps / 2 - 1); |
|
|
|
if (oldTaps && (grow > 0) && !m_buffer.empty()) |
|
{ |
|
m_buffer.insert(m_buffer.begin(), grow, Cf(0.0f, 0.0f)); |
|
m_time += grow; |
|
} |
|
} |
|
|
|
// The normalised Gardner detector's gain, from the raised cosine pulse, and the loop's gains from it |
|
void FrontEnd::computeGains() |
|
{ |
|
auto detector = [this](double tau) { |
|
double e = 0.0, p = 0.0; |
|
|
|
for (int m = -40; m <= 40; m++) |
|
{ |
|
e += raisedCosine(m - 0.5 + tau, m_rollOff) * (raisedCosine(m + tau, m_rollOff) - raisedCosine(m - 1 + tau, m_rollOff)); |
|
p += raisedCosine(m + tau, m_rollOff) * raisedCosine(m + tau, m_rollOff); |
|
} |
|
|
|
return e / p; |
|
}; |
|
|
|
m_detectorGain = (detector(0.01) - detector(-0.01)) / 0.02; |
|
const double zeta = 0.707; |
|
const double theta = m_bandwidth / (zeta + 1.0 / (4.0 * zeta)); |
|
const double d = 1.0 + 2.0 * zeta * theta + theta * theta; |
|
m_kp = 4.0 * zeta * theta / d / m_detectorGain; |
|
m_ki = 4.0 * theta * theta / d / m_detectorGain; |
|
} |
|
|
|
void FrontEnd::adjustClock(double ppm) |
|
{ |
|
m_integrator = std::max(-m_maxClockOffset, std::min(m_maxClockOffset, m_integrator + ppm * 1e-6)); |
|
m_status.m_clockAdjustments++; |
|
} |
|
|
|
void FrontEnd::setTimingBandwidth(double bnT) |
|
{ |
|
if (bnT != m_bandwidth) |
|
{ |
|
m_bandwidth = bnT; |
|
computeGains(); |
|
} |
|
} |
|
|
|
void FrontEnd::reset() |
|
{ |
|
m_buffer.assign(m_taps, Cf(0.0f, 0.0f)); |
|
m_time = m_taps - 1; |
|
m_onSymbol = true; |
|
m_haveLast = false; |
|
m_lastSymbol = Cf(0.0f, 0.0f); |
|
m_lastMid = Cf(0.0f, 0.0f); |
|
m_integrator = 0.0; |
|
m_power = 1.0; |
|
m_powerValid = false; |
|
m_spectrumFill = 0; |
|
m_spectrumBlock.assign(m_fftSize, Cf(0.0f, 0.0f)); |
|
m_estimatePower.assign(m_fftSize, 0.0); |
|
m_estimateBlocks = 0; |
|
m_untilDisplayBlock = 0; |
|
m_ncoPhase = 0.0; |
|
m_inputPowerSum = 0.0; |
|
m_inputPowerCount = 0; |
|
const double power = m_status.m_power; |
|
// The clock adjustments made count on (a receiver may be waiting for one) |
|
const uint64_t clockAdjustments = m_status.m_clockAdjustments; |
|
m_status = Status(); |
|
m_status.m_clockAdjustments = clockAdjustments; |
|
m_status.m_power = power; |
|
clearEqualiser(); |
|
|
|
if (m_frequencyKnown) |
|
{ |
|
m_status.m_estimating = false; |
|
m_frequencyKnown = false; |
|
} |
|
else |
|
{ |
|
m_ncoFrequency = 0.0; |
|
} |
|
|
|
m_status.m_frequencyOffset = m_ncoFrequency; |
|
} |
|
|
|
void FrontEnd::setFrequency(double hz) |
|
{ |
|
m_ncoFrequency = hz; |
|
m_frequencyKnown = true; |
|
m_status.m_frequencyOffset = hz; |
|
} |
|
|
|
int FrontEnd::heldSymbols() const |
|
{ |
|
// The next output's at m_time in the buffer |
|
return m_samplesPerSymbol > 0.0 ? std::max(0, (int) std::lround(((double) m_buffer.size() - m_time) / m_samplesPerSymbol)) : 0; |
|
} |
|
|
|
void FrontEnd::adjustFrequency(double hz) |
|
{ |
|
m_ncoFrequency += hz; |
|
m_status.m_frequencyOffset = m_ncoFrequency; |
|
} |
|
|
|
void FrontEnd::fft(std::vector<std::complex<double>>& x) const |
|
{ |
|
const int n = m_fftSize; |
|
|
|
for (int i = 0; i < n; i++) |
|
{ |
|
if (i < m_bitReverse[i]) { |
|
std::swap(x[i], x[m_bitReverse[i]]); |
|
} |
|
} |
|
|
|
for (int len = 2; len <= n; len <<= 1) |
|
{ |
|
const int half = len / 2; |
|
const int stride = n / len; |
|
|
|
for (int i = 0; i < n; i += len) |
|
{ |
|
for (int j = 0; j < half; j++) |
|
{ |
|
const std::complex<double> t = x[i + j + half] * m_twiddles[j * stride]; |
|
x[i + j + half] = x[i + j] - t; |
|
x[i + j] += t; |
|
} |
|
} |
|
} |
|
} |
|
|
|
void FrontEnd::setEqualiserReference(bool locked, double esn0) |
|
{ |
|
m_eqLocked = locked; |
|
|
|
if (locked) { |
|
m_eqEsN0 = esn0; |
|
} |
|
|
|
if (!m_eqEnabled && !m_eqGainDb.empty()) { |
|
clearEqualiser(); |
|
} |
|
} |
|
|
|
void FrontEnd::clearEqualiser() |
|
{ |
|
m_eqPower.clear(); |
|
m_eqBlocks = 0; |
|
m_status.m_equalising = false; |
|
m_status.m_equaliserEdgeDb = 0.0; |
|
|
|
if (!m_eqGainDb.empty()) |
|
{ |
|
m_eqGainDb.clear(); |
|
makeFilter(); |
|
} |
|
} |
|
|
|
// A spectrum block (of the input, taken for the display), averaged while a receiver's locked, the equaliser updated from |
|
// each m_eqUpdateBlocks |
|
void FrontEnd::accumulateEqualiser(const std::vector<double>& power) |
|
{ |
|
if (!m_eqEnabled || !m_eqLocked || (m_eqEsN0 < m_eqMinEsN0) || (m_samplesPerSymbol > m_eqMaxSamplesPerSymbol)) |
|
{ |
|
m_eqPower.clear(); |
|
m_eqBlocks = 0; |
|
return; |
|
} |
|
|
|
if (m_eqPower.size() != power.size()) { |
|
m_eqPower.assign(power.size(), 0.0); |
|
} |
|
|
|
for (size_t k = 0; k < power.size(); k++) { |
|
m_eqPower[k] += power[k]; |
|
} |
|
|
|
if (++m_eqBlocks >= m_eqUpdateBlocks) |
|
{ |
|
computeEqualiser(); |
|
m_eqPower.assign(power.size(), 0.0); |
|
m_eqBlocks = 0; |
|
} |
|
} |
|
|
|
// The receive chain's response: the averaged spectrum over what it'd be (the raised cosine at the Es/N0 measured, over |
|
// flat noise), relative to the middle of the band, smoothed, made symmetric (the matched filter's real), and the |
|
// correction its inverse, limited. Averaged with the last, so it changes gradually |
|
void FrontEnd::computeEqualiser() |
|
{ |
|
const int n = m_fftSize; |
|
const double binHz = m_inputRate / n; |
|
const double snr = std::pow(10.0, std::min(30.0, m_eqEsN0) / 10.0); |
|
const double inner = (1.0 - m_rollOff) * m_symbolRate / 2.0; |
|
const double outer = (1.0 + m_rollOff) * m_symbolRate / 2.0; |
|
const int maxBin = (int) std::ceil(outer / binHz) + 1; |
|
// The spectrum's in order from -inputRate/2, of the input before the NCO: the signal's centre is at its frequency |
|
const double centreBin = n / 2 + m_ncoFrequency / binHz; |
|
std::vector<double> ratio(2 * maxBin + 1, 0.0); |
|
std::vector<double> core; |
|
|
|
for (int d = -maxBin; d <= maxBin; d++) |
|
{ |
|
const double fb = centreBin + d; |
|
const int i0 = (int) std::floor(fb); |
|
const double t = fb - i0; |
|
const double p = (i0 >= 0) && (i0 + 1 < n) ? (1.0 - t) * m_eqPower[i0] + t * m_eqPower[i0 + 1] : 0.0; |
|
const double f = std::fabs(d * binHz); |
|
const double rc = f <= inner ? 1.0 : f >= outer ? 0.0 : 0.5 * (1.0 + std::cos(m_pi * (f - inner) / (m_rollOff * m_symbolRate))); |
|
ratio[d + maxBin] = p / (rc * snr + 1.0); |
|
|
|
if (f < 0.3 * m_symbolRate) { |
|
core.push_back(ratio[d + maxBin]); |
|
} |
|
} |
|
|
|
if (core.empty()) { |
|
return; |
|
} |
|
|
|
std::sort(core.begin(), core.end()); |
|
const double reference = core[core.size() / 2]; |
|
|
|
if (reference <= 0.0) { |
|
return; |
|
} |
|
|
|
// Smoothed, and the two sides averaged (in dB) |
|
const int smooth = std::max(1, (int) std::lround(m_eqSmoothing * m_symbolRate / binHz)); |
|
std::vector<double> responseDb(2 * maxBin + 1); |
|
|
|
for (int d = -maxBin; d <= maxBin; d++) |
|
{ |
|
double sum = 0.0; |
|
int count = 0; |
|
|
|
for (int j = std::max(-maxBin, d - smooth); j <= std::min(maxBin, d + smooth); j++, count++) { |
|
sum += ratio[j + maxBin]; |
|
} |
|
|
|
responseDb[d + maxBin] = 10.0 * std::log10(std::max(1e-6, sum / count / reference)); |
|
} |
|
|
|
std::vector<double> gainDb(maxBin + 1); |
|
|
|
for (int d = 0; d <= maxBin; d++) |
|
{ |
|
const double response = 0.5 * (responseDb[maxBin + d] + responseDb[maxBin - d]); |
|
gainDb[d] = std::max(m_eqMinGainDb, std::min(m_eqMaxGainDb, -response)); |
|
} |
|
|
|
if (m_eqGainDb.size() == gainDb.size()) |
|
{ |
|
double change = 0.0; |
|
|
|
for (size_t k = 0; k < gainDb.size(); k++) |
|
{ |
|
gainDb[k] = 0.5 * (gainDb[k] + m_eqGainDb[k]); |
|
change = std::max(change, std::fabs(gainDb[k] - m_eqGainDb[k])); |
|
} |
|
|
|
// Too little to matter: the filter (made in this, the front end's, thread) kept, and the correction it's of |
|
if (change < m_eqMinChangeDb) { |
|
return; |
|
} |
|
} |
|
|
|
m_eqGainDb = gainDb; |
|
makeFilter(); |
|
m_status.m_equalising = true; |
|
m_status.m_equaliserEdgeDb = m_eqGainDb[std::min(maxBin, (int) std::lround(m_symbolRate / 2.0 / binHz))]; |
|
} |
|
|
|
// Windowed power spectrum of a block, added to power, in order from -inputRate/2 |
|
void FrontEnd::spectrumBlock(const Cf *block, std::vector<double>& power) |
|
{ |
|
std::vector<std::complex<double>> x(m_fftSize); |
|
|
|
for (int i = 0; i < m_fftSize; i++) { |
|
x[i] = std::complex<double>(block[i].real(), block[i].imag()) * m_window[i]; |
|
} |
|
|
|
fft(x); |
|
|
|
for (int i = 0; i < m_fftSize; i++) { |
|
power[(i + m_fftSize / 2) % m_fftSize] += std::norm(x[i]); |
|
} |
|
} |
|
|
|
// The signal's centre, from the averaged spectrum: where it correlates best with the raised cosine's power spectrum |
|
// (less the noise floor, so the shape's what counts), interpolated between bins. And its half power width, which is |
|
// the symbol rate |
|
void FrontEnd::estimateCarrier() |
|
{ |
|
const int n = m_fftSize; |
|
const double binHz = m_inputRate / n; |
|
std::vector<double> smooth(n, 0.0); |
|
|
|
for (int i = 0; i < n; i++) |
|
{ |
|
double sum = 0.0; |
|
int count = 0; |
|
|
|
for (int j = std::max(0, i - 2); j <= std::min(n - 1, i + 2); j++, count++) { |
|
sum += m_estimatePower[j]; |
|
} |
|
|
|
smooth[i] = sum / count; |
|
} |
|
|
|
std::vector<double> sorted(smooth); |
|
std::sort(sorted.begin(), sorted.end()); |
|
const double floor = sorted[n / 5]; |
|
std::vector<double> excess(n); |
|
|
|
for (int i = 0; i < n; i++) { |
|
excess[i] = std::max(0.0, smooth[i] - floor); |
|
} |
|
|
|
const double inner = (1.0 - m_rollOff) * m_symbolRate / 2.0; |
|
const double outer = (1.0 + m_rollOff) * m_symbolRate / 2.0; |
|
const int width = (int) std::ceil(outer / binHz); |
|
std::vector<double> shape(2 * width + 1); |
|
|
|
for (int d = -width; d <= width; d++) |
|
{ |
|
const double f = std::fabs(d * binHz); |
|
shape[d + width] = f <= inner ? 1.0 : f >= outer ? 0.0 : 0.5 * (1.0 + std::cos(m_pi * (f - inner) / (m_rollOff * m_symbolRate))); |
|
} |
|
|
|
const int maxOffset = std::max(0, (int) ((m_inputRate / 2.0 - outer * 0.9) / binHz)); |
|
std::vector<double> scores(2 * maxOffset + 1, 0.0); |
|
int best = 0; |
|
|
|
for (int c = -maxOffset; c <= maxOffset; c++) |
|
{ |
|
double score = 0.0; |
|
|
|
for (int d = -width; d <= width; d++) |
|
{ |
|
const int k = n / 2 + c + d; |
|
|
|
if ((k >= 0) && (k < n)) { |
|
score += shape[d + width] * excess[k]; |
|
} |
|
} |
|
|
|
scores[c + maxOffset] = score; |
|
|
|
if (score > scores[best + maxOffset]) { |
|
best = c; |
|
} |
|
} |
|
|
|
double offset = best; |
|
|
|
if ((best > -maxOffset) && (best < maxOffset)) |
|
{ |
|
const double a = scores[best + maxOffset - 1], b = scores[best + maxOffset], c = scores[best + maxOffset + 1]; |
|
const double denom = a - 2.0 * b + c; |
|
|
|
if (denom < 0.0) { |
|
offset += std::max(-0.5, std::min(0.5, 0.5 * (a - c) / denom)); |
|
} |
|
} |
|
|
|
if (scores[best + maxOffset] > 0.0) { |
|
m_ncoFrequency = offset * binHz; |
|
} |
|
|
|
// Half power width: the in-band level, then outwards from the centre to where it's half way to the floor |
|
const int centre = n / 2 + best; |
|
const int core = std::max(1, (int) (0.6 * inner / binHz)); |
|
std::vector<double> band; |
|
|
|
for (int k = centre - core; k <= centre + core; k++) |
|
{ |
|
if ((k >= 0) && (k < n)) { |
|
band.push_back(smooth[k]); |
|
} |
|
} |
|
|
|
m_status.m_measuredSymbolRate = 0.0; |
|
m_status.m_spectralEsN0 = 99.0; |
|
|
|
if (!band.empty()) |
|
{ |
|
std::sort(band.begin(), band.end()); |
|
const double level = band[band.size() / 2]; |
|
|
|
// The raised cosine's power spectrum integrates to the symbol rate times its in-band level, which noise in the |
|
// estimate doesn't bias as it does the half power points. Only if a quarter of the input's band is outside the |
|
// signal, so the floor (its 20th percentile) is noise: at 1.25 or 1.5 times the symbol rate, it was the signal's |
|
// edges, and the rate measured 75 to 90 % of the true one |
|
if ((level > 2.0 * floor) && (outer <= 0.375 * m_inputRate)) |
|
{ |
|
double area = 0.0; |
|
const int extent = (int) std::ceil(1.25 * outer / binHz); |
|
|
|
// Limited to twice the level, so a line (an unmodulated carrier, or the transmitter's) doesn't count much |
|
for (int k = std::max(0, centre - extent); k <= std::min(n - 1, centre + extent); k++) { |
|
area += std::min(smooth[k] - floor, 2.0 * (level - floor)); |
|
} |
|
|
|
m_status.m_measuredSymbolRate = area / (level - floor) * binHz; |
|
} |
|
|
|
// A raised cosine's in-band level is its Es: over the noise's, Es/N0 |
|
if ((outer <= 0.375 * m_inputRate) && (floor > 0.0)) { |
|
m_status.m_spectralEsN0 = 10.0 * std::log10(std::max(1e-3, (level - floor) / floor)); |
|
} |
|
} |
|
|
|
std::lock_guard<std::mutex> lock(m_displayMutex); |
|
m_displayPower = m_estimatePower; |
|
|
|
for (double& p : m_displayPower) { |
|
p /= m_spectrumBlocks; |
|
} |
|
} |
|
|
|
void FrontEnd::getSpectrum(std::vector<float>& db, int points) const |
|
{ |
|
std::lock_guard<std::mutex> lock(m_displayMutex); |
|
db.clear(); |
|
|
|
if (m_displayPower.empty() || (points <= 0)) { |
|
return; |
|
} |
|
|
|
db.resize(points); |
|
const int per = std::max(1, m_fftSize / points); |
|
|
|
for (int p = 0; p < points; p++) |
|
{ |
|
double sum = 0.0; |
|
const int first = (int) ((int64_t) p * m_fftSize / points); |
|
int count = 0; |
|
|
|
for (int i = first; i < std::min(m_fftSize, first + per); i++, count++) { |
|
sum += m_displayPower[i]; |
|
} |
|
|
|
db[p] = (float) (10.0 * std::log10(std::max(1e-20, sum / std::max(1, count)))); |
|
} |
|
} |
|
|
|
// A block of the input for display (and the equaliser), the next a tenth of a second on |
|
void FrontEnd::displayBlock(const Cf *block) |
|
{ |
|
std::vector<double> power(m_fftSize, 0.0); |
|
spectrumBlock(block, power); |
|
|
|
{ |
|
std::lock_guard<std::mutex> lock(m_displayMutex); |
|
|
|
if (m_displayPower.size() != power.size()) { |
|
m_displayPower = power; |
|
} |
|
|
|
for (int k = 0; k < m_fftSize; k++) { |
|
m_displayPower[k] = 0.8 * m_displayPower[k] + 0.2 * power[k]; |
|
} |
|
} |
|
|
|
accumulateEqualiser(power); |
|
m_untilDisplayBlock = (uint64_t) (m_inputRate / 10.0); |
|
} |
|
|
|
void FrontEnd::process(const Cf *in, int nbIn, std::vector<Cf>& symbols) |
|
{ |
|
if (nbIn <= 0) { |
|
return; |
|
} |
|
|
|
// The input's power, for the status, from up to 4096 samples of each block, 8 partial sums at a time (as the |
|
// compiler won't reorder a float sum to vectorise it) |
|
{ |
|
const int n = std::min(nbIn, 4096) & ~3; |
|
float sums[8] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; |
|
const float *f = reinterpret_cast<const float *>(in); |
|
|
|
for (int i = 0; i < 2 * n; i += 8) |
|
{ |
|
for (int j = 0; j < 8; j++) { |
|
sums[j] += f[i + j] * f[i + j]; |
|
} |
|
} |
|
|
|
m_inputPowerSum += (sums[0] + sums[1]) + (sums[2] + sums[3]) + (sums[4] + sums[5]) + (sums[6] + sums[7]); |
|
m_inputPowerCount += n; |
|
} |
|
|
|
if (m_inputPowerCount >= (uint64_t) (m_inputRate / 100.0)) |
|
{ |
|
m_status.m_power = 10.0 * std::log10(std::max(1e-20, m_inputPowerSum / m_inputPowerCount)); |
|
m_inputPowerSum = 0.0; |
|
m_inputPowerCount = 0; |
|
} |
|
|
|
// The spectrum, of the raw input: for the carrier's estimate, all the blocks, then for display a block now and then |
|
if (m_status.m_estimating) |
|
{ |
|
int i = 0; |
|
|
|
while (i < nbIn) |
|
{ |
|
const int n = std::min(nbIn - i, m_fftSize - m_spectrumFill); |
|
std::copy(in + i, in + i + n, m_spectrumBlock.begin() + m_spectrumFill); |
|
m_spectrumFill += n; |
|
i += n; |
|
|
|
if (m_spectrumFill == m_fftSize) |
|
{ |
|
spectrumBlock(m_spectrumBlock.data(), m_estimatePower); |
|
m_spectrumFill = 0; |
|
|
|
if (++m_estimateBlocks == m_spectrumBlocks) |
|
{ |
|
estimateCarrier(); |
|
m_status.m_estimating = false; |
|
m_status.m_frequencyOffset = m_ncoFrequency; |
|
m_untilDisplayBlock = (uint64_t) (m_inputRate / 10.0); |
|
break; |
|
} |
|
} |
|
} |
|
|
|
if (m_status.m_estimating) { |
|
return; |
|
} |
|
|
|
// The rest of the samples are demodulated |
|
in += i; |
|
nbIn -= i; |
|
} |
|
else if ((m_spectrumFill > 0) || (m_untilDisplayBlock <= (uint64_t) nbIn)) |
|
{ |
|
if ((m_spectrumFill == 0) && (nbIn >= m_fftSize)) |
|
{ |
|
displayBlock(in); |
|
} |
|
else |
|
{ |
|
// Blocks shorter than the FFT gathered (the estimate's buffer's free once it's made), from the one due |
|
const int n = std::min(nbIn, m_fftSize - m_spectrumFill); |
|
std::copy(in, in + n, m_spectrumBlock.begin() + m_spectrumFill); |
|
m_spectrumFill += n; |
|
|
|
if (m_spectrumFill == m_fftSize) |
|
{ |
|
m_spectrumFill = 0; |
|
displayBlock(m_spectrumBlock.data()); |
|
} |
|
} |
|
} |
|
else |
|
{ |
|
m_untilDisplayBlock -= nbIn; |
|
} |
|
|
|
// NCO, in float, from the exact phase every block, into the buffer grown for it (its capacity doubled when short, |
|
// so it's seldom reallocated) |
|
const size_t base = m_buffer.size(); |
|
|
|
if (m_buffer.capacity() < base + nbIn) { |
|
m_buffer.reserve(2 * (base + nbIn)); |
|
} |
|
|
|
m_buffer.resize(base + nbIn); |
|
const double stepAngle = -2.0 * m_pi * m_ncoFrequency / m_inputRate; |
|
const Cf stepRot((float) std::cos(stepAngle), (float) std::sin(stepAngle)); |
|
|
|
if (m_ncoFrequency == 0.0) |
|
{ |
|
std::copy(in, in + nbIn, m_buffer.begin() + base); |
|
} |
|
else |
|
{ |
|
// A block of 64 phasors from 64 steps, then each block's start from the exact phase |
|
float stepRe[64], stepIm[64]; |
|
Cf p(1.0f, 0.0f); |
|
|
|
for (int k = 0; k < 64; k++) |
|
{ |
|
stepRe[k] = p.real(); |
|
stepIm[k] = p.imag(); |
|
p *= stepRot; |
|
} |
|
|
|
const float *src = reinterpret_cast<const float *>(in); |
|
float *dst = reinterpret_cast<float *>(m_buffer.data() + base); |
|
|
|
for (int j = 0; j < nbIn; j += 64) |
|
{ |
|
const int n = std::min(64, nbIn - j); |
|
const double phase = m_ncoPhase + j * stepAngle; |
|
const float rr = (float) std::cos(phase), ri = (float) std::sin(phase); |
|
|
|
for (int k = 0; k < n; k++) |
|
{ |
|
const float pr = rr * stepRe[k] - ri * stepIm[k], pi_ = rr * stepIm[k] + ri * stepRe[k]; |
|
const float xr = src[2 * (j + k)], xi = src[2 * (j + k) + 1]; |
|
dst[2 * (j + k)] = xr * pr - xi * pi_; |
|
dst[2 * (j + k) + 1] = xr * pi_ + xi * pr; |
|
} |
|
} |
|
|
|
m_ncoPhase = std::fmod(m_ncoPhase + nbIn * stepAngle, 2.0 * m_pi); |
|
} |
|
|
|
// Matched filter and interpolation, a block of symbols at a time, the timing loop updated after each |
|
const int maxOut = 2 * m_symbolsPerUpdate; |
|
m_outputs.resize(maxOut); |
|
|
|
while (true) |
|
{ |
|
const double step = m_step * (1.0 - m_integrator); |
|
const int made = dvbt::resampleFilter(m_buffer.data(), 0, (int) m_buffer.size(), m_pairs.data(), m_taps, m_phases, |
|
m_time, step, m_outputs.data(), maxOut); |
|
float error = 0.0f, power = 0.0f; |
|
int count = 0; |
|
int k = 0; |
|
bool endedOnSymbol = false; // The last output a symbol, so the next's a half way sample |
|
const size_t blockFirst = symbols.size(); // This block's symbols, for the AGC: the one held from the last too |
|
|
|
if (!m_onSymbol && (made > 0)) |
|
{ |
|
m_lastMid = m_outputs[0]; |
|
m_onSymbol = true; |
|
k = 1; |
|
} |
|
|
|
if (!m_haveLast && (k < made)) |
|
{ |
|
m_lastSymbol = m_outputs[k]; |
|
symbols.push_back(m_lastSymbol); |
|
m_haveLast = true; |
|
m_onSymbol = false; |
|
|
|
if (++k < made) |
|
{ |
|
m_lastMid = m_outputs[k++]; |
|
m_onSymbol = true; |
|
} |
|
else |
|
{ |
|
endedOnSymbol = true; |
|
} |
|
} |
|
|
|
// Pairs of a symbol and the half way sample after it, after any symbol just output. Gardner: positive when late, |
|
// the half way sample past the zero crossing of a transition |
|
const size_t first = symbols.size(); |
|
symbols.resize(first + (made - k + 1) / 2); |
|
Cf *out = symbols.data() + first; |
|
const Cf *outputs = m_outputs.data(); |
|
float lr = m_lastSymbol.real(), li = m_lastSymbol.imag(), mr = m_lastMid.real(), mi = m_lastMid.imag(); |
|
int s = 0; |
|
|
|
for (; k + 1 < made; k += 2) |
|
{ |
|
const float yr = outputs[k].real(), yi = outputs[k].imag(); |
|
error += mr * (yr - lr) + mi * (yi - li); |
|
power += yr * yr + yi * yi; |
|
out[s++] = outputs[k]; |
|
lr = yr; |
|
li = yi; |
|
mr = outputs[k + 1].real(); |
|
mi = outputs[k + 1].imag(); |
|
} |
|
|
|
count = s; |
|
|
|
if (k < made) |
|
{ |
|
// A symbol whose half way sample is in the next block |
|
const float yr = m_outputs[k].real(), yi = m_outputs[k].imag(); |
|
error += mr * (yr - lr) + mi * (yi - li); |
|
power += yr * yr + yi * yi; |
|
out[s++] = m_outputs[k]; |
|
lr = yr; |
|
li = yi; |
|
count++; |
|
m_onSymbol = false; |
|
} |
|
else if ((made > 0) && !endedOnSymbol) |
|
{ |
|
m_onSymbol = true; |
|
} |
|
|
|
symbols.resize(first + s); |
|
m_lastSymbol = Cf(lr, li); |
|
m_lastMid = Cf(mr, mi); |
|
|
|
if ((count > 0) && (power > 0.0)) |
|
{ |
|
const double e = error / power; |
|
m_integrator = std::max(-m_maxClockOffset, std::min(m_maxClockOffset, m_integrator + m_ki * e * count)); |
|
const double move = std::max(-m_maxPhaseStep, std::min(m_maxPhaseStep, m_kp * e * count)); |
|
m_time = std::max((double) (m_taps / 2 - 1), m_time - move * m_samplesPerSymbol); |
|
const double mean = power / count; |
|
m_power = m_powerValid ? 0.9 * m_power + 0.1 * mean : mean; |
|
m_powerValid = true; |
|
} |
|
|
|
if (m_powerValid) |
|
{ |
|
const float gain = (float) (1.0 / std::sqrt(std::max(1e-30, m_power))); |
|
|
|
for (size_t s = blockFirst; s < symbols.size(); s++) { |
|
symbols[s] *= gain; |
|
} |
|
} |
|
|
|
m_status.m_symbols += symbols.size() - blockFirst; |
|
|
|
if (made < maxOut) { |
|
break; |
|
} |
|
} |
|
|
|
m_status.m_clockOffset = (1.0 / (1.0 - m_integrator) - 1.0) * 1e6; |
|
|
|
// Keep the input the next outputs need |
|
const int consumed = std::min((int) m_buffer.size(), std::max(0, (int) m_time - (m_taps / 2 - 1))); |
|
|
|
if (consumed > 0) |
|
{ |
|
m_buffer.erase(m_buffer.begin(), m_buffer.begin() + consumed); |
|
m_time -= consumed; |
|
} |
|
} |
|
|
|
} // namespace dvbs
|
|
|