VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2024-11-17 22:01:45 -05:00
Code Issues Projects Releases Wiki Activity
c50c9753f6
sdrangel/plugins/channelrx/demodadsb/adsbdemodsinkworker.cpp

391 lines
18 KiB
C++
Raw Blame History

///////////////////////////////////////////////////////////////////////////////////
// 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);
}
Reference in New Issue View Git Blame Copy Permalink