mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-11-26 09:48:45 -05:00
391 lines
18 KiB
C++
391 lines
18 KiB
C++
///////////////////////////////////////////////////////////////////////////////////
|
|
// Copyright (C) 2020 Jon Beniston, M7RCE //
|
|
// //
|
|
// 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/>. //
|
|
///////////////////////////////////////////////////////////////////////////////////
|
|
|
|
#define BOOST_CHRONO_HEADER_ONLY
|
|
#include <boost/chrono/chrono.hpp>
|
|
|
|
#include <QDebug>
|
|
|
|
#include "util/stepfunctions.h"
|
|
#include "util/db.h"
|
|
#include "dsp/dspengine.h"
|
|
#include "dsp/dspcommands.h"
|
|
#include "device/deviceapi.h"
|
|
|
|
#include "adsbdemodreport.h"
|
|
#include "adsbdemodsink.h"
|
|
#include "adsbdemodsinkworker.h"
|
|
#include "adsbdemodsettings.h"
|
|
#include "adsb.h"
|
|
|
|
MESSAGE_CLASS_DEFINITION(ADSBDemodSinkWorker::MsgConfigureADSBDemodSinkWorker, Message)
|
|
|
|
void ADSBDemodSinkWorker::run()
|
|
{
|
|
int readBuffer = 0;
|
|
|
|
// Acquire first buffer
|
|
m_sink->m_bufferRead[readBuffer].acquire();
|
|
|
|
// Start recording how much time is spent processing in this method
|
|
boost::chrono::steady_clock::time_point startPoint = boost::chrono::steady_clock::now();
|
|
|
|
// Check for updated settings
|
|
handleInputMessages();
|
|
|
|
// samplesPerBit is only changed when the thread is stopped
|
|
int samplesPerBit = m_settings.m_samplesPerBit;
|
|
int samplesPerFrame = samplesPerBit*(ADS_B_PREAMBLE_BITS+ADS_B_ES_BITS);
|
|
int samplesPerChip = samplesPerBit/ADS_B_CHIPS_PER_BIT;
|
|
|
|
qDebug() << "ADSBDemodSinkWorker:: running with"
|
|
<< " samplesPerFrame: " << samplesPerFrame
|
|
<< " samplesPerChip: " << samplesPerChip
|
|
<< " samplesPerBit: " << samplesPerBit
|
|
<< " correlateFullPreamble: " << m_settings.m_correlateFullPreamble
|
|
<< " correlationScale: " << m_correlationScale
|
|
<< " correlationThreshold: " << m_settings.m_correlationThreshold;
|
|
|
|
int readIdx = m_sink->m_samplesPerFrame - 1;
|
|
|
|
while (true)
|
|
{
|
|
int startIdx = readIdx;
|
|
|
|
// Correlate received signal with expected preamble
|
|
// chip+ indexes are 0, 2, 7, 9
|
|
// correlating over first 6 bits gives a reduction in per-sample
|
|
// processing, but more than doubles the number of false matches
|
|
Real preambleCorrelationOnes = 0.0;
|
|
Real preambleCorrelationZeros = 0.0;
|
|
if (m_settings.m_correlateFullPreamble)
|
|
{
|
|
for (int i = 0; i < samplesPerChip; i++)
|
|
{
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 0*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 1*samplesPerChip + i];
|
|
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 2*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 3*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 4*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 5*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 6*samplesPerChip + i];
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 7*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 8*samplesPerChip + i];
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 9*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 10*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 11*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 12*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 13*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 14*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 15*samplesPerChip + i];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (int i = 0; i < samplesPerChip; i++)
|
|
{
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 0*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 1*samplesPerChip + i];
|
|
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 2*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 3*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 4*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 5*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 6*samplesPerChip + i];
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 7*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 8*samplesPerChip + i];
|
|
preambleCorrelationOnes += m_sink->m_sampleBuffer[readBuffer][startIdx + 9*samplesPerChip + i];
|
|
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 10*samplesPerChip + i];
|
|
preambleCorrelationZeros += m_sink->m_sampleBuffer[readBuffer][startIdx + 11*samplesPerChip + i];
|
|
}
|
|
}
|
|
|
|
// Use the ratio of ones power over zeros power, as we don't care how powerful the signal
|
|
// is, just whether there is a good correlation with the preamble. The absolute value varies
|
|
// too much with different radios, AGC settings and and the noise floor is not constant
|
|
// (E.g: it's quite possible to receive multiple frames simultaneously, so we don't
|
|
// want a maximum threshold for the zeros, as a weaker signal may transmit 1s in
|
|
// a stronger signals 0 chip position. Similarly a strong signal in an adjacent
|
|
// channel may casue AGC to reduce gain, reducing the ampltiude of an otherwise
|
|
// strong signal, as well as the noise floor)
|
|
// The threshold accounts for the different number of zeros and ones in the preamble
|
|
// If the sum of ones is exactly 0, it's probably no signal
|
|
|
|
Real preambleCorrelation = preambleCorrelationOnes/preambleCorrelationZeros; // without one/zero ratio correction
|
|
|
|
if ((preambleCorrelation > m_correlationThresholdLinear) && (preambleCorrelationOnes != 0.0f))
|
|
{
|
|
int firstIdx = startIdx;
|
|
|
|
m_demodStats.m_correlatorMatches++;
|
|
// Skip over preamble
|
|
startIdx += samplesPerBit*ADS_B_PREAMBLE_BITS;
|
|
|
|
// Demodulate waveform to bytes
|
|
unsigned char data[ADS_B_ES_BYTES];
|
|
int byteIdx = 0;
|
|
int currentBit;
|
|
unsigned char currentByte = 0;
|
|
int df;
|
|
|
|
for (int bit = 0; bit < ADS_B_ES_BITS; bit++)
|
|
{
|
|
// PPM (Pulse position modulation) - Each bit spreads to two chips, 1->10, 0->01
|
|
// Determine if bit is 1 or 0, by seeing which chip has largest combined energy over the sampling period
|
|
Real oneSum = 0.0f;
|
|
Real zeroSum = 0.0f;
|
|
for (int i = 0; i < samplesPerChip; i++)
|
|
{
|
|
oneSum += m_sink->m_sampleBuffer[readBuffer][startIdx+i];
|
|
zeroSum += m_sink->m_sampleBuffer[readBuffer][startIdx+samplesPerChip+i];
|
|
}
|
|
currentBit = oneSum > zeroSum;
|
|
startIdx += samplesPerBit;
|
|
// Convert bit to bytes - MSB first
|
|
currentByte |= currentBit << (7-(bit & 0x7));
|
|
if ((bit & 0x7) == 0x7)
|
|
{
|
|
data[byteIdx++] = currentByte;
|
|
currentByte = 0;
|
|
// Don't try to demodulate any further, if this isn't an ADS-B frame
|
|
// to help reduce processing overhead
|
|
if (!m_settings.m_demodModeS && (bit == 7))
|
|
{
|
|
df = ((data[0] >> 3) & ADS_B_DF_MASK);
|
|
if ((df != 17) && (df != 18))
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Is ADS-B?
|
|
df = ((data[0] >> 3) & ADS_B_DF_MASK);
|
|
if ((df == 17) || (df == 18))
|
|
{
|
|
m_crc.init();
|
|
unsigned int parity = (data[11] << 16) | (data[12] << 8) | data[13]; // Parity / CRC
|
|
|
|
m_crc.calculate(data, ADS_B_ES_BYTES-3);
|
|
if (parity == m_crc.get())
|
|
{
|
|
// Got a valid frame
|
|
m_demodStats.m_adsbFrames++;
|
|
// Get 24-bit ICAO and save in hash of ICAOs that have been seen
|
|
unsigned icao = ((data[1] & 0xff) << 16) | ((data[2] & 0xff) << 8) | (data[3] & 0xff);
|
|
m_icaos.insert(icao, true);
|
|
// Don't try to re-demodulate the same frame
|
|
// We could possibly allow a partial overlap here
|
|
readIdx += (ADS_B_ES_BITS+ADS_B_PREAMBLE_BITS)*ADS_B_CHIPS_PER_BIT*samplesPerChip - 1;
|
|
// Pass to GUI
|
|
if (m_sink->getMessageQueueToGUI())
|
|
{
|
|
ADSBDemodReport::MsgReportADSB *msg = ADSBDemodReport::MsgReportADSB::create(
|
|
QByteArray((char*)data, sizeof(data)),
|
|
preambleCorrelation * m_correlationScale,
|
|
preambleCorrelationOnes / samplesPerChip,
|
|
rxDateTime(firstIdx, readBuffer),
|
|
m_crc.get());
|
|
m_sink->getMessageQueueToGUI()->push(msg);
|
|
}
|
|
// Pass to worker to feed to other servers
|
|
if (m_sink->getMessageQueueToWorker())
|
|
{
|
|
ADSBDemodReport::MsgReportADSB *msg = ADSBDemodReport::MsgReportADSB::create(
|
|
QByteArray((char*)data, sizeof(data)),
|
|
preambleCorrelation * m_correlationScale,
|
|
preambleCorrelationOnes / samplesPerChip,
|
|
rxDateTime(firstIdx, readBuffer),
|
|
m_crc.get());
|
|
m_sink->getMessageQueueToWorker()->push(msg);
|
|
}
|
|
}
|
|
else
|
|
m_demodStats.m_crcFails++;
|
|
}
|
|
else if (m_settings.m_demodModeS)
|
|
{
|
|
int bytes;
|
|
|
|
// Determine number of bytes in frame depending on downlink format
|
|
if ((df == 0) || (df == 4) || (df == 5) || (df == 11)) {
|
|
bytes = 56/8;
|
|
} else if ((df == 16) || (df == 20) || (df == 21) || (df >= 24)) {
|
|
bytes = 112/8;
|
|
} else {
|
|
bytes = 0;
|
|
}
|
|
if (bytes > 0)
|
|
{
|
|
// Extract received parity
|
|
int parity = (data[bytes-3] << 16) | (data[bytes-2] << 8) | data[bytes-1];
|
|
// Calculate CRC on received frame
|
|
m_crc.init();
|
|
m_crc.calculate(data, bytes-3);
|
|
int crc = m_crc.get();
|
|
// DF4 / DF5 / DF20 / DF21 have ICAO address XORed in to parity.
|
|
// Extract ICAO from parity and see if it matches an aircraft we've already
|
|
// received an ADS-B frame from
|
|
if ((df == 4) || (df == 5) || (df == 20) || (df == 21))
|
|
{
|
|
unsigned icao = (parity ^ crc) & 0xffffff;
|
|
if (m_icaos.contains(icao)) {
|
|
crc ^= icao;
|
|
}
|
|
}
|
|
// For DF11, the last 7 bits may have an address/interogration indentifier (II)
|
|
// XORed in, so we ignore those bits
|
|
if ((parity == crc) || ((df == 11) && ((parity & 0xffff80) == (crc & 0xffff80))))
|
|
{
|
|
m_demodStats.m_modesFrames++;
|
|
// Pass to GUI (only pass formats it can decode)
|
|
if (m_sink->getMessageQueueToGUI() && ((df == 4) || (df == 5) || (df == 20) || (df == 21)))
|
|
{
|
|
ADSBDemodReport::MsgReportADSB *msg = ADSBDemodReport::MsgReportADSB::create(
|
|
QByteArray((char*)data, sizeof(data)),
|
|
preambleCorrelation * m_correlationScale,
|
|
preambleCorrelationOnes / samplesPerChip,
|
|
rxDateTime(firstIdx, readBuffer),
|
|
m_crc.get());
|
|
m_sink->getMessageQueueToGUI()->push(msg);
|
|
}
|
|
// Pass to worker to feed to other servers
|
|
if (m_sink->getMessageQueueToWorker())
|
|
{
|
|
ADSBDemodReport::MsgReportADSB *msg = ADSBDemodReport::MsgReportADSB::create(
|
|
QByteArray((char*)data, sizeof(data)),
|
|
preambleCorrelation * m_correlationScale,
|
|
preambleCorrelationOnes / samplesPerChip,
|
|
rxDateTime(firstIdx, readBuffer),
|
|
m_crc.get());
|
|
m_sink->getMessageQueueToWorker()->push(msg);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
m_demodStats.m_crcFails++;
|
|
}
|
|
}
|
|
else
|
|
m_demodStats.m_typeFails++;
|
|
}
|
|
else
|
|
m_demodStats.m_typeFails++;
|
|
}
|
|
readIdx++;
|
|
if (readIdx > m_sink->m_bufferSize - samplesPerFrame)
|
|
{
|
|
int nextBuffer = readBuffer+1;
|
|
if (nextBuffer >= m_sink->m_buffers)
|
|
nextBuffer = 0;
|
|
|
|
// Update amount of time spent processing (don't include time spend in acquire)
|
|
boost::chrono::duration<double> sec = boost::chrono::steady_clock::now() - startPoint;
|
|
m_demodStats.m_demodTime += sec.count();
|
|
m_demodStats.m_feedTime = m_sink->m_feedTime;
|
|
|
|
// Send stats to GUI
|
|
if (m_sink->getMessageQueueToGUI())
|
|
{
|
|
ADSBDemodReport::MsgReportDemodStats *msg = ADSBDemodReport::MsgReportDemodStats::create(m_demodStats);
|
|
m_sink->getMessageQueueToGUI()->push(msg);
|
|
}
|
|
|
|
if (!isInterruptionRequested())
|
|
{
|
|
// Get next buffer
|
|
m_sink->m_bufferRead[nextBuffer].acquire();
|
|
|
|
// Check for updated settings
|
|
handleInputMessages();
|
|
|
|
// Resume timing how long we are processing
|
|
startPoint = boost::chrono::steady_clock::now();
|
|
|
|
int samplesRemaining = m_sink->m_bufferSize - readIdx;
|
|
if (samplesRemaining > 0)
|
|
{
|
|
// Copy remaining samples, to start of next buffer
|
|
memcpy(&m_sink->m_sampleBuffer[nextBuffer][samplesPerFrame - 1 - samplesRemaining], &m_sink->m_sampleBuffer[readBuffer][readIdx], samplesRemaining*sizeof(Real));
|
|
readIdx = samplesPerFrame - 1 - samplesRemaining;
|
|
}
|
|
else
|
|
{
|
|
readIdx = samplesPerFrame - 1;
|
|
}
|
|
|
|
m_sink->m_bufferWrite[readBuffer].release();
|
|
|
|
readBuffer = nextBuffer;
|
|
}
|
|
else
|
|
{
|
|
// Use a break to avoid testing a condition in the main loop
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
void ADSBDemodSinkWorker::handleInputMessages()
|
|
{
|
|
Message* message;
|
|
|
|
while ((message = m_inputMessageQueue.pop()) != nullptr)
|
|
{
|
|
if (MsgConfigureADSBDemodSinkWorker::match(*message))
|
|
{
|
|
MsgConfigureADSBDemodSinkWorker* cfg = (MsgConfigureADSBDemodSinkWorker*)message;
|
|
|
|
ADSBDemodSettings settings = cfg->getSettings();
|
|
bool force = cfg->getForce();
|
|
|
|
if (settings.m_correlateFullPreamble) {
|
|
m_correlationScale = 3.0;
|
|
} else {
|
|
m_correlationScale = 2.0;
|
|
}
|
|
|
|
if ((m_settings.m_correlationThreshold != settings.m_correlationThreshold) || force)
|
|
{
|
|
m_correlationThresholdLinear = CalcDb::powerFromdB(settings.m_correlationThreshold);
|
|
m_correlationThresholdLinear /= m_correlationScale;
|
|
qDebug() << "m_correlationThresholdLinear: " << m_correlationThresholdLinear;
|
|
}
|
|
|
|
m_settings = settings;
|
|
delete message;
|
|
}
|
|
}
|
|
}
|
|
|
|
QDateTime ADSBDemodSinkWorker::rxDateTime(int firstIdx, int readBuffer) const
|
|
{
|
|
const qint64 samplesPerSecondMSec = ADS_B_BITS_PER_SECOND * m_settings.m_samplesPerBit / 1000;
|
|
const qint64 offsetMSec = (firstIdx - m_sink->m_samplesPerFrame - 1) / samplesPerSecondMSec;
|
|
return m_sink->m_bufferFirstSampleDateTime[readBuffer].addMSecs(offsetMSec);
|
|
}
|