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.
231 lines
7.8 KiB
231 lines
7.8 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 "droneiddecoder.h" |
|
#include "droneiddetector.h" |
|
|
|
DroneIDDetector::DroneIDDetector() : |
|
m_threshold(0.20f), |
|
m_x(m_historySize), |
|
m_c(m_productSize), |
|
m_c2(m_productSize), |
|
m_e(m_productSize), |
|
m_p(m_corrSize), |
|
m_p2(m_corrSize), |
|
m_r(m_corrSize), |
|
m_rr(m_corrSize) |
|
{ |
|
std::vector<int> starts; |
|
DroneIDDecoder::symbolStarts(starts); |
|
const int last = DroneIDDecoder::m_nbSymbols - 1; |
|
for (int k = 0; k < DroneIDDecoder::m_nbSymbols; k++) |
|
{ |
|
m_offsets[k] = (starts[last] + DroneIDDecoder::m_cpLengths[last]) |
|
- (starts[k] + DroneIDDecoder::m_cpLengths[k]); |
|
} |
|
reset(); |
|
} |
|
|
|
void DroneIDDetector::reset() |
|
{ |
|
std::fill(m_x.begin(), m_x.end(), cf(0.0f, 0.0f)); |
|
std::fill(m_c.begin(), m_c.end(), cf(0.0f, 0.0f)); |
|
std::fill(m_c2.begin(), m_c2.end(), cf(0.0f, 0.0f)); |
|
std::fill(m_e.begin(), m_e.end(), 0.0f); |
|
std::fill(m_p.begin(), m_p.end(), cf(0.0f, 0.0f)); |
|
std::fill(m_p2.begin(), m_p2.end(), cf(0.0f, 0.0f)); |
|
std::fill(m_r.begin(), m_r.end(), 0.0f); |
|
std::fill(m_rr.begin(), m_rr.end(), 0.0f); |
|
m_pSum = 0.0; |
|
m_p2Sum = 0.0; |
|
m_rSum = 0.0; |
|
m_n = 0; |
|
m_state = Searching; |
|
m_peakMetric = 0.0f; |
|
m_peakScore = 0.0f; |
|
m_peakIndex = 0; |
|
m_searchStart = 0; |
|
m_captureEnd = 0; |
|
m_holdoffEnd = 0; |
|
m_burstMetric = 0.0f; |
|
m_burstIndex = 0; |
|
} |
|
|
|
// Recalculate running sums from stored products, to prevent accumulation of rounding errors |
|
void DroneIDDetector::recalculate() |
|
{ |
|
m_pSum = 0.0; |
|
m_p2Sum = 0.0; |
|
m_rSum = 0.0; |
|
for (int i = 0; i < m_window; i++) |
|
{ |
|
int64_t n = m_n - i; |
|
m_pSum += (std::complex<double>) m_c[n & m_productMask]; |
|
m_p2Sum += (std::complex<double>) m_c2[n & m_productMask]; |
|
m_rSum += m_e[n & m_productMask]; |
|
} |
|
} |
|
|
|
// Frame correlation for frame whose last symbol's CP correlation ends at sample n. |
|
// Returns normalised metric, and sets score to the unnormalised value. |
|
// If gate is true, returns 0 as soon as the metric is known to be below the threshold, |
|
// to save computation while searching. |
|
float DroneIDDetector::metric(int64_t n, float& score, bool gate) const |
|
{ |
|
cf f = 0.0f; |
|
float e = 0.0f; |
|
|
|
for (int k = 0; k < DroneIDDecoder::m_nbSymbols; k++) |
|
{ |
|
int i = (int) ((n - m_offsets[k]) & m_corrMask); |
|
f += m_p[i]; |
|
e += m_rr[i]; |
|
} |
|
|
|
// The metric can not exceed |f|/e, so skip the square roots when below the threshold |
|
float te = m_threshold * e; |
|
if (gate && (std::norm(f) <= te * te)) |
|
{ |
|
score = 0.0f; |
|
return 0.0f; |
|
} |
|
|
|
cf f2 = 0.0f; |
|
for (int k = 0; k < DroneIDDecoder::m_nbSymbols; k++) { |
|
f2 += m_p2[(n - m_offsets[k]) & m_corrMask]; |
|
} |
|
score = std::abs(f) - std::abs(f2); |
|
if (e <= 0.0f) { |
|
return 0.0f; |
|
} |
|
return score / e; |
|
} |
|
|
|
bool DroneIDDetector::process(cf x) |
|
{ |
|
m_n++; |
|
const int64_t n = m_n; |
|
|
|
m_x[n & m_historyMask] = x; |
|
|
|
// Products of this sample with earlier samples. These are stored, so they don't need to be |
|
// recalculated when they leave the window, and exactly what was added is subtracted |
|
const cf xl = m_x[(n - m_lag) & m_historyMask]; |
|
const cf xl2 = m_x[(n - m_lag2) & m_historyMask]; |
|
const cf c(x.real() * xl.real() + x.imag() * xl.imag(), x.imag() * xl.real() - x.real() * xl.imag()); |
|
const cf c2(x.real() * xl2.real() + x.imag() * xl2.imag(), x.imag() * xl2.real() - x.real() * xl2.imag()); |
|
const float e = x.real() * x.real() + x.imag() * x.imag(); |
|
const int pi = (int) (n & m_productMask); |
|
const int po = (int) ((n - m_window) & m_productMask); |
|
|
|
// Update windowed correlations and energy |
|
m_pSum += std::complex<double>((double) c.real() - (double) m_c[po].real(), (double) c.imag() - (double) m_c[po].imag()); |
|
m_p2Sum += std::complex<double>((double) c2.real() - (double) m_c2[po].real(), (double) c2.imag() - (double) m_c2[po].imag()); |
|
m_rSum += (double) e - (double) m_e[po]; |
|
m_c[pi] = c; |
|
m_c2[pi] = c2; |
|
m_e[pi] = e; |
|
|
|
if ((n & (m_recalcInterval - 1)) == 0) { |
|
recalculate(); |
|
} |
|
|
|
const int ci = (int) (n & m_corrMask); |
|
const float r = (float) std::max(m_rSum, 0.0); |
|
m_p[ci] = cf((float) m_pSum.real(), (float) m_pSum.imag()); |
|
m_p2[ci] = cf((float) m_p2Sum.real(), (float) m_p2Sum.imag()); |
|
m_r[ci] = r; |
|
m_rr[ci] = 0.5f * (r + m_r[(n - m_lag) & m_corrMask]); |
|
|
|
// Wait until there's enough history for a frame |
|
if (n < DroneIDDecoder::m_frameLength + m_lag + m_window) { |
|
return false; |
|
} |
|
|
|
// Frame start, if last symbol's correlation peak is at n |
|
const int lastPeakOffset = m_offsets[0] + DroneIDDecoder::m_cpLengths[0] + m_lag - 1; |
|
|
|
switch (m_state) |
|
{ |
|
case Searching: |
|
{ |
|
if ((n < m_holdoffEnd) || ((n % m_searchStride) != 0)) { |
|
break; |
|
} |
|
float score; |
|
float m = metric(n, score, true); |
|
if (m > m_threshold) |
|
{ |
|
m_peakMetric = m; |
|
m_peakScore = score; |
|
m_peakIndex = n; |
|
m_searchStart = n; |
|
m_state = FindingPeak; |
|
} |
|
break; |
|
} |
|
case FindingPeak: |
|
{ |
|
float score; |
|
float m = metric(n, score, false); |
|
if ((m > m_threshold) && (score > m_peakScore)) |
|
{ |
|
m_peakMetric = m; |
|
m_peakScore = score; |
|
m_peakIndex = n; |
|
} |
|
if (n - m_searchStart >= m_peakSearchLength) |
|
{ |
|
int64_t frameStart = m_peakIndex - lastPeakOffset; |
|
m_captureEnd = frameStart + DroneIDDecoder::m_frameLength + m_margin; |
|
m_state = Capturing; |
|
} |
|
else |
|
{ |
|
break; |
|
} |
|
} |
|
[[fallthrough]]; |
|
case Capturing: |
|
{ |
|
if (n >= m_captureEnd) |
|
{ |
|
int64_t frameStart = m_peakIndex - lastPeakOffset; |
|
int64_t start = frameStart - m_margin; |
|
int length = (int) (m_captureEnd - start + 1); |
|
|
|
m_burst.resize(length); |
|
for (int i = 0; i < length; i++) { |
|
m_burst[i] = m_x[(start + i) & m_historyMask]; |
|
} |
|
m_burstMetric = m_peakMetric; |
|
m_burstIndex = start; |
|
// Partial alignments with the end of this frame give high metrics for another frame's duration |
|
m_holdoffEnd = m_peakIndex + DroneIDDecoder::m_frameLength; |
|
m_state = Searching; |
|
return true; |
|
} |
|
break; |
|
} |
|
} |
|
|
|
return false; |
|
}
|
|
|