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.
 
 
 
 
 
 

473 lines
16 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 <cmath>
#include <algorithm>
#include "dsp/fftengine.h"
#include "dsp/fftfactory.h"
#include "util/ltegold.h"
#include "droneiddecoder.h"
const int DroneIDDecoder::m_cpLengths[DroneIDDecoder::m_nbSymbols] = {80, 72, 72, 72, 72, 72, 72, 72, 80};
const int DroneIDDecoder::m_zcSymbols[2] = {3, 5};
const int DroneIDDecoder::m_zcRoots[2] = {600, 147};
const int DroneIDDecoder::m_dataSymbols[6] = {1, 2, 4, 6, 7, 8};
DroneIDDecoder::DroneIDDecoder(FFTFactory *fftFactory) :
m_maxIterations(8),
m_maxIntegerCFO(8),
m_fftFactory(fftFactory),
m_fftEngine(nullptr),
m_fftSequence(0),
m_kissFFT(m_fftSize, false)
{
if (m_fftFactory) {
m_fftSequence = m_fftFactory->getEngine(m_fftSize, false, &m_fftEngine);
}
carrierBins(m_bins);
zadoffChu(m_zcRoots[0], m_zc[0]);
zadoffChu(m_zcRoots[1], m_zc[1]);
m_zcRef.resize(m_nbCarriers);
for (int c = 0; c < m_nbCarriers; c++) {
m_zcRef[c] = std::conj(m_zc[0][c]) * m_zc[1][c];
}
m_zcDiff.resize(m_fftSize);
symbolStarts(m_symbolStarts);
LTEGold::generate(m_scramblerInit, m_codedBits, m_scrambler);
m_turbo.setBlockSize(m_blockBits);
m_fftIn.resize(m_fftSize);
m_symbols.resize(m_nbSymbols);
for (auto& s : m_symbols) {
s.resize(m_fftSize);
}
}
void DroneIDDecoder::carrierBins(std::vector<int>& bins)
{
bins.resize(m_nbCarriers);
for (int i = 0; i < m_nbCarriers; i++)
{
int k = i < m_nbCarriers / 2 ? i - m_nbCarriers / 2 : i - m_nbCarriers / 2 + 1; // -300..-1, 1..300
bins[i] = (k + m_fftSize) % m_fftSize;
}
}
void DroneIDDecoder::zadoffChu(int root, std::vector<cf>& seq)
{
const int Nzc = m_nbCarriers + 1;
// Element on DC (n=300) isn't transmitted
seq.clear();
for (int n = 0; n < Nzc; n++)
{
if (n != Nzc / 2)
{
double phase = -M_PI * root * (double) n * (double) (n + 1) / Nzc;
seq.push_back(cf((float) std::cos(phase), (float) std::sin(phase)));
}
}
}
void DroneIDDecoder::symbolStarts(std::vector<int>& starts)
{
starts.resize(m_nbSymbols);
int p = 0;
for (int i = 0; i < m_nbSymbols; i++)
{
starts[i] = p;
p += m_cpLengths[i] + m_fftSize;
}
}
// Find candidate frame starts by correlating each symbol's cyclic prefix with the end of the symbol.
// As symbols are similar lengths, the metric is also high when offset by a whole number of symbols,
// particularly at low SNR, so we return the best start along with the best starts nearby offset by
// whole symbols, to be checked with the ZC sequences.
void DroneIDDecoder::findTiming(const cf *samples, int length, std::vector<Candidate>& candidates)
{
const int N = m_fftSize;
const int symbolLength = m_cpLengths[1] + N;
const int search = 24;
std::vector<std::complex<double>> q(length + 1);
std::vector<double> e(length + 1);
const int nbStarts = length - m_frameLength + 1;
candidates.clear();
if (nbStarts <= 0) {
return;
}
q[0] = 0.0;
e[0] = 0.0;
for (int i = 0; i < length; i++)
{
std::complex<double> x = samples[i];
q[i + 1] = q[i] + ((i + N < length) ? x * std::conj((std::complex<double>) samples[i + N]) : 0.0);
e[i + 1] = e[i] + std::norm(x);
}
std::vector<float> metrics(nbStarts);
std::vector<cf> corrs(nbStarts);
int best = 0;
for (int s = 0; s < nbStarts; s++)
{
std::complex<double> acc = 0.0;
double energy = 0.0;
for (int k = 0; k < m_nbSymbols; k++)
{
int a = s + m_symbolStarts[k];
int cp = m_cpLengths[k];
acc += q[a + cp] - q[a];
energy += 0.5 * ((e[a + cp] - e[a]) + (e[a + N + cp] - e[a + N]));
}
metrics[s] = energy > 0.0 ? (float) (std::abs(acc) / energy) : 0.0f;
corrs[s] = cf((float) acc.real(), (float) acc.imag());
if (metrics[s] > metrics[best]) {
best = s;
}
}
for (int j = -m_maxSymbolOffset; j <= m_maxSymbolOffset; j++)
{
int centre = best + j * symbolLength;
int lo = std::max(0, centre - (j == 0 ? 0 : search));
int hi = std::min(nbStarts - 1, centre + (j == 0 ? 0 : search));
if (lo > hi) {
continue;
}
int s = lo;
for (int i = lo + 1; i <= hi; i++)
{
if (metrics[i] > metrics[s]) {
s = i;
}
}
Candidate c;
c.m_start = s;
c.m_correlation = corrs[s];
c.m_metric = metrics[s];
candidates.push_back(c);
}
}
DroneIDDecoder::~DroneIDDecoder()
{
if (m_fftFactory && m_fftEngine) {
m_fftFactory->releaseEngine(m_fftSize, false, m_fftSequence);
}
}
// FFT a symbol after correcting frequency offset
void DroneIDDecoder::fftSymbol(const cf *samples, int length, int frameStart, double cyclesPerSample, int symbol)
{
const int start = frameStart + m_symbolStarts[symbol] + m_cpLengths[symbol] - m_fftBackoff;
cf *in = m_fftEngine ? m_fftEngine->in() : m_fftIn.data();
// Use absolute sample index for phase, so symbols are phase coherent
std::complex<double> rotation = std::polar(1.0, -2.0 * M_PI * std::fmod(cyclesPerSample * start, 1.0));
const std::complex<double> step = std::polar(1.0, -2.0 * M_PI * cyclesPerSample);
for (int i = 0; i < m_fftSize; i++)
{
int idx = start + i;
if ((idx >= 0) && (idx < length)) {
in[i] = samples[idx] * cf((float) rotation.real(), (float) rotation.imag());
} else {
in[i] = 0.0f;
}
rotation *= step;
}
if (m_fftEngine)
{
m_fftEngine->transform();
std::copy(m_fftEngine->out(), m_fftEngine->out() + m_fftSize, m_symbols[symbol].begin());
}
else
{
m_kissFFT.transform(m_fftIn.data(), m_symbols[symbol].data());
}
}
// FFT each symbol after correcting frequency offset
void DroneIDDecoder::fftSymbols(const cf *samples, int length, int frameStart, double cyclesPerSample)
{
for (int k = 0; k < m_nbSymbols; k++) {
fftSymbol(samples, length, frameStart, cyclesPerSample, k);
}
}
// Phase difference between the ZC symbols in each FFT bin, for zcConsistency
void DroneIDDecoder::zcPhaseDifference()
{
const std::vector<cf>& y4 = m_symbols[m_zcSymbols[0]];
const std::vector<cf>& y6 = m_symbols[m_zcSymbols[1]];
// Only phase is used, so a strong narrowband interferer on one carrier doesn't dominate
for (int i = 0; i < m_fftSize; i++)
{
cf p = y4[i] * std::conj(y6[i]);
float mag = std::abs(p);
m_zcDiff[i] = mag > 0.0f ? p / mag : 0.0f;
}
}
// Estimate channel from both ZC symbols, assuming a frequency offset of shift bins.
// With the correct shift, both estimates are the same apart from a common phase rotation.
// As the ZC sequences have unit magnitude, h4*conj(h6) = y4*conj(y6) * conj(zc4)*zc6.
float DroneIDDecoder::zcConsistency(int shift)
{
cf corr = 0.0f;
for (int c = 0; c < m_nbCarriers; c++)
{
int bin = (m_bins[c] + shift + m_fftSize) % m_fftSize;
corr += m_zcDiff[bin] * m_zcRef[c];
}
return std::abs(corr) / m_nbCarriers;
}
bool DroneIDDecoder::checkCRC(const std::vector<uint8_t>& bits)
{
uint8_t bytes[m_blockBytes];
for (int i = 0; i < m_blockBytes; i++)
{
uint8_t b = 0;
for (int j = 0; j < 8; j++) {
b = (b << 1) | bits[i * 8 + j];
}
bytes[i] = b;
}
m_crc.init();
m_crc.calculate(bytes, m_blockBytes);
return m_crc.get() == 0;
}
bool DroneIDDecoder::decode(const cf *samples, int length, Result& result)
{
// Pad so a frame without the first symbol can start at the beginning of the buffer
const int pad = m_cpLengths[0] + m_fftSize;
const int paddedLength = length + 2 * pad;
result = Result();
m_derotated.assign(paddedLength, 0.0f);
std::copy(samples, samples + length, m_derotated.begin() + pad);
// Candidate timings and fractional frequency offsets from cyclic prefix
std::vector<Candidate> candidates;
findTiming(m_derotated.data(), paddedLength, candidates);
for (const auto& candidate : candidates) {
result.m_cpMetric = std::max(result.m_cpMetric, candidate.m_metric);
}
// For each candidate, find integer frequency offset where both ZC symbols give consistent
// channel estimates. Choose candidate with best consistency.
int frameStart = -1;
double cyclesPerSample = 0.0;
int bestShift = 0;
float bestMetric = -1.0f;
for (const auto& candidate : candidates)
{
if (candidate.m_metric < m_minCPMetric) {
continue;
}
double cycles = -std::arg(candidate.m_correlation) / (2.0 * M_PI * m_fftSize);
// Only the ZC symbols are needed to evaluate the candidate
fftSymbol(m_derotated.data(), paddedLength, candidate.m_start, cycles, m_zcSymbols[0]);
fftSymbol(m_derotated.data(), paddedLength, candidate.m_start, cycles, m_zcSymbols[1]);
zcPhaseDifference();
for (int shift = -m_maxIntegerCFO; shift <= m_maxIntegerCFO; shift++)
{
float m = zcConsistency(shift);
if (m > bestMetric)
{
bestMetric = m;
bestShift = shift;
frameStart = candidate.m_start;
cyclesPerSample = cycles;
result.m_cpMetric = candidate.m_metric;
}
}
}
result.m_zcMetric = std::max(bestMetric, 0.0f);
if ((frameStart < 0) || (bestMetric < m_minZCMetric)) {
return false;
}
result.m_frameStart = frameStart - pad;
cyclesPerSample += bestShift / (double) m_fftSize;
fftSymbols(m_derotated.data(), paddedLength, frameStart, cyclesPerSample);
result.m_cfo = (float) (cyclesPerSample * m_sampleRate);
// Channel estimate from ZC symbols
std::vector<cf> h4(m_nbCarriers), h6(m_nbCarriers), h(m_nbCarriers);
cf drift = 0.0f;
for (int c = 0; c < m_nbCarriers; c++)
{
h4[c] = m_symbols[m_zcSymbols[0]][m_bins[c]] * std::conj(m_zc[0][c]);
h6[c] = m_symbols[m_zcSymbols[1]][m_bins[c]] * std::conj(m_zc[1][c]);
cf p = h6[c] * std::conj(h4[c]);
float mag = std::abs(p);
if (mag > 0.0f) {
drift += p / mag;
}
}
// Residual phase rotation between the ZC symbols
float phi = std::arg(drift);
cf derot = std::polar(1.0f, -phi);
std::vector<float> noise(m_nbCarriers);
std::vector<cf> havg(m_nbCarriers);
for (int c = 0; c < m_nbCarriers; c++)
{
cf h6r = h6[c] * derot;
havg[c] = 0.5f * (h4[c] + h6r);
// Each estimate has the noise variance of the received signal, the difference has twice that
noise[c] = 0.5f * std::norm(h4[c] - h6r);
}
// Smooth channel estimate across adjacent carriers, and noise estimate over a wider range.
// Noise is estimated per carrier, so carriers with narrowband interference have low confidence.
std::vector<float> sigma2(m_nbCarriers);
float signal = 0.0f;
float noiseTotal = 0.0f;
for (int c = 0; c < m_nbCarriers; c++)
{
cf sum = 0.0f;
int n = 0;
for (int d = -m_channelSmoothing; d <= m_channelSmoothing; d++)
{
int i = c + d;
if ((i >= 0) && (i < m_nbCarriers))
{
sum += havg[i];
n++;
}
}
h[c] = sum / (float) n;
signal += std::norm(h[c]);
float nsum = 0.0f;
n = 0;
for (int d = -m_noiseSmoothing; d <= m_noiseSmoothing; d++)
{
int i = c + d;
if ((i >= 0) && (i < m_nbCarriers))
{
nsum += noise[i];
n++;
}
}
sigma2[c] = nsum / n;
noiseTotal += noise[c];
}
signal /= m_nbCarriers;
noiseTotal = std::max(noiseTotal / m_nbCarriers, signal * 1e-6f);
for (int c = 0; c < m_nbCarriers; c++) {
sigma2[c] = std::max(sigma2[c], signal * 1e-6f);
}
result.m_snr = 10.0f * std::log10(signal / noiseTotal);
// Channel for each symbol, interpolating phase drift linearly in time
const float t4 = (float) (m_symbolStarts[m_zcSymbols[0]] + m_cpLengths[m_zcSymbols[0]]);
const float t6 = (float) (m_symbolStarts[m_zcSymbols[1]] + m_cpLengths[m_zcSymbols[1]]);
auto symbolRotation = [&](int k) {
float t = (float) (m_symbolStarts[k] + m_cpLengths[k]);
return std::polar(1.0f, phi * (t - t4) / (t6 - t4));
};
// Check first symbol, which is scrambled zeros, to see if it was transmitted
{
cf rot = symbolRotation(0);
int zeros = 0;
for (int c = 0; c < m_nbCarriers; c++)
{
cf r = m_symbols[0][m_bins[c]] * std::conj(h[c] * rot);
zeros += ((r.real() < 0.0f ? 1 : 0) == m_scrambler[2 * c]);
zeros += ((r.imag() < 0.0f ? 1 : 0) == m_scrambler[2 * c + 1]);
}
result.m_legacy = zeros < (int) (0.75f * 2 * m_nbCarriers);
}
// Soft demap QPSK, descramble
std::vector<float> llrs(m_codedBits);
const float scale = 2.0f * (float) M_SQRT2;
float evm = 0.0f;
for (int d = 0; d < 6; d++)
{
int k = m_dataSymbols[d];
cf rot = symbolRotation(k);
for (int c = 0; c < m_nbCarriers; c++)
{
cf hk = h[c] * rot;
cf y = m_symbols[k][m_bins[c]];
cf r = y * std::conj(hk);
int idx = 2 * (d * m_nbCarriers + c);
llrs[idx] = scale * r.real() / sigma2[c] * (m_scrambler[idx] ? -1.0f : 1.0f);
llrs[idx + 1] = scale * r.imag() / sigma2[c] * (m_scrambler[idx + 1] ? -1.0f : 1.0f);
float hn = std::norm(hk);
if (hn > 0.0f)
{
cf eq = r / hn;
cf ref((eq.real() < 0.0f ? -1.0f : 1.0f) / (float) M_SQRT2, (eq.imag() < 0.0f ? -1.0f : 1.0f) / (float) M_SQRT2);
evm += std::norm(eq - ref);
}
}
}
result.m_evm = std::sqrt(evm / (6 * m_nbCarriers));
// Rate dematching and turbo decoding
std::vector<float> d0, d1, d2;
std::vector<uint8_t> bits;
LTERateMatcher::dematch(llrs, m_blockBits + 4, 0, d0, d1, d2);
int iterations = m_turbo.decode(d0, d1, d2, bits, m_maxIterations,
[this](const std::vector<uint8_t>& b) { return checkCRC(b); });
result.m_iterations = std::abs(iterations);
result.m_crcValid = iterations > 0;
if (result.m_crcValid)
{
result.m_payload.resize(m_dataBytes);
for (int i = 0; i < m_dataBytes; i++)
{
uint8_t b = 0;
for (int j = 0; j < 8; j++) {
b = (b << 1) | bits[i * 8 + j];
}
result.m_payload[i] = b;
}
}
return true;
}