mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-09-12 08:06:34 -04:00
332 lines
12 KiB
C++
332 lines
12 KiB
C++
|
///////////////////////////////////////////////////////////////////////////////////
|
||
|
// Copyright (C) 2019 Edouard Griffiths, F4EXB //
|
||
|
// Copyright (C) 2023 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/>. //
|
||
|
///////////////////////////////////////////////////////////////////////////////////
|
||
|
|
||
|
#include <QDebug>
|
||
|
|
||
|
#include <complex.h>
|
||
|
|
||
|
#include "dsp/dspengine.h"
|
||
|
#include "dsp/scopevis.h"
|
||
|
#include "util/db.h"
|
||
|
#include "util/popcount.h"
|
||
|
#include "maincore.h"
|
||
|
|
||
|
#include "dscdemod.h"
|
||
|
#include "dscdemodsink.h"
|
||
|
|
||
|
DSCDemodSink::DSCDemodSink(DSCDemod *packetDemod) :
|
||
|
m_dscDemod(packetDemod),
|
||
|
m_channelSampleRate(DSCDemodSettings::DSCDEMOD_CHANNEL_SAMPLE_RATE),
|
||
|
m_channelFrequencyOffset(0),
|
||
|
m_magsqSum(0.0f),
|
||
|
m_magsqPeak(0.0f),
|
||
|
m_magsqCount(0),
|
||
|
m_messageQueueToChannel(nullptr),
|
||
|
m_exp(nullptr),
|
||
|
m_sampleBufferIndex(0)
|
||
|
{
|
||
|
m_magsq = 0.0;
|
||
|
|
||
|
for (int i = 0; i < DSCDemodSettings::m_scopeStreams; i++) {
|
||
|
m_sampleBuffer[i].resize(m_sampleBufferSize);
|
||
|
}
|
||
|
|
||
|
applySettings(m_settings, true);
|
||
|
applyChannelSettings(m_channelSampleRate, m_channelFrequencyOffset, true);
|
||
|
|
||
|
m_lowpassComplex1.create(301, DSCDemodSettings::DSCDEMOD_CHANNEL_SAMPLE_RATE, DSCDemodSettings::DSCDEMOD_BAUD_RATE * 1.1);
|
||
|
m_lowpassComplex2.create(301, DSCDemodSettings::DSCDEMOD_CHANNEL_SAMPLE_RATE, DSCDemodSettings::DSCDEMOD_BAUD_RATE * 1.1);
|
||
|
}
|
||
|
|
||
|
DSCDemodSink::~DSCDemodSink()
|
||
|
{
|
||
|
delete[] m_exp;
|
||
|
}
|
||
|
|
||
|
void DSCDemodSink::sampleToScope(Complex sample, Real abs1Filt, Real abs2Filt, Real unbiasedData, Real biasedData)
|
||
|
{
|
||
|
if (m_scopeSink)
|
||
|
{
|
||
|
m_sampleBuffer[0][m_sampleBufferIndex] = sample;
|
||
|
m_sampleBuffer[1][m_sampleBufferIndex] = Complex(m_magsq, 0.0f);
|
||
|
m_sampleBuffer[2][m_sampleBufferIndex] = Complex(abs1Filt, 0.0f);
|
||
|
m_sampleBuffer[3][m_sampleBufferIndex] = Complex(abs2Filt, 0.0f);
|
||
|
m_sampleBuffer[4][m_sampleBufferIndex] = Complex(unbiasedData, 0.0f);
|
||
|
m_sampleBuffer[5][m_sampleBufferIndex] = Complex(biasedData, 0.0f);
|
||
|
m_sampleBuffer[6][m_sampleBufferIndex] = Complex(m_data, 0.0f);
|
||
|
m_sampleBuffer[7][m_sampleBufferIndex] = Complex(m_clock, 0.0f);
|
||
|
m_sampleBuffer[8][m_sampleBufferIndex] = Complex(m_bit, 0.0f);
|
||
|
m_sampleBuffer[9][m_sampleBufferIndex] = Complex(m_gotSOP, 0.0f);
|
||
|
m_sampleBufferIndex++;
|
||
|
|
||
|
if (m_sampleBufferIndex == m_sampleBufferSize)
|
||
|
{
|
||
|
std::vector<ComplexVector::const_iterator> vbegin;
|
||
|
|
||
|
for (int i = 0; i < DSCDemodSettings::m_scopeStreams; i++) {
|
||
|
vbegin.push_back(m_sampleBuffer[i].begin());
|
||
|
}
|
||
|
|
||
|
m_scopeSink->feed(vbegin, m_sampleBufferSize);
|
||
|
m_sampleBufferIndex = 0;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void DSCDemodSink::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end)
|
||
|
{
|
||
|
Complex ci;
|
||
|
|
||
|
for (SampleVector::const_iterator it = begin; it != end; ++it)
|
||
|
{
|
||
|
Complex c(it->real(), it->imag());
|
||
|
c *= m_nco.nextIQ();
|
||
|
|
||
|
if (m_interpolatorDistance < 1.0f) // interpolate
|
||
|
{
|
||
|
while (!m_interpolator.interpolate(&m_interpolatorDistanceRemain, c, &ci))
|
||
|
{
|
||
|
processOneSample(ci);
|
||
|
m_interpolatorDistanceRemain += m_interpolatorDistance;
|
||
|
}
|
||
|
}
|
||
|
else // decimate
|
||
|
{
|
||
|
if (m_interpolator.decimate(&m_interpolatorDistanceRemain, c, &ci))
|
||
|
{
|
||
|
processOneSample(ci);
|
||
|
m_interpolatorDistanceRemain += m_interpolatorDistance;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void DSCDemodSink::processOneSample(Complex &ci)
|
||
|
{
|
||
|
// Calculate average and peak levels for level meter
|
||
|
double magsqRaw = ci.real()*ci.real() + ci.imag()*ci.imag();;
|
||
|
Real magsq = magsqRaw / (SDR_RX_SCALED*SDR_RX_SCALED);
|
||
|
m_movingAverage(magsq);
|
||
|
m_magsq = m_movingAverage.asDouble();
|
||
|
m_magsqSum += magsq;
|
||
|
if (magsq > m_magsqPeak)
|
||
|
{
|
||
|
m_magsqPeak = magsq;
|
||
|
}
|
||
|
m_magsqCount++;
|
||
|
|
||
|
// Sum power while data is being received
|
||
|
if (m_gotSOP)
|
||
|
{
|
||
|
m_rssiMagSqSum += magsq;
|
||
|
m_rssiMagSqCount++;
|
||
|
}
|
||
|
|
||
|
ci /= SDR_RX_SCALEF;
|
||
|
|
||
|
// Correlate with expected frequencies
|
||
|
Complex exp = m_exp[m_expIdx];
|
||
|
m_expIdx = (m_expIdx + 1) % m_expLength;
|
||
|
Complex corr1 = ci * exp;
|
||
|
Complex corr2 = ci * std::conj(exp);
|
||
|
|
||
|
// Low pass filter
|
||
|
Real abs1Filt = std::abs(m_lowpassComplex1.filter(corr1));
|
||
|
Real abs2Filt = std::abs(m_lowpassComplex2.filter(corr2));
|
||
|
|
||
|
// Envelope calculation
|
||
|
m_movMax1(abs1Filt);
|
||
|
m_movMax2(abs2Filt);
|
||
|
Real env1 = m_movMax1.getMaximum();
|
||
|
Real env2 = m_movMax2.getMaximum();
|
||
|
|
||
|
// Automatic threshold correction to compensate for frequency selective fading
|
||
|
// http://www.w7ay.net/site/Technical/ATC/index.html
|
||
|
Real bias1 = abs1Filt - 0.5 * env1;
|
||
|
Real bias2 = abs2Filt - 0.5 * env2;
|
||
|
Real unbiasedData = abs1Filt - abs2Filt;
|
||
|
Real biasedData = bias1 - bias2;
|
||
|
|
||
|
// Save current data for edge detection
|
||
|
m_dataPrev = m_data;
|
||
|
// Set data according to stongest correlation
|
||
|
m_data = biasedData > 0;
|
||
|
|
||
|
// Calculate timing error (we expect clockCount to be 0 when data changes), and add a proportion of it
|
||
|
if (m_data && !m_dataPrev) {
|
||
|
m_clockCount -= m_clockCount * 0.25;
|
||
|
}
|
||
|
|
||
|
m_clockCount += 1.0;
|
||
|
if (m_clockCount >= m_samplesPerBit/2.0-1.0)
|
||
|
{
|
||
|
// Sample in middle of symbol
|
||
|
receiveBit(m_data);
|
||
|
m_clock = 1;
|
||
|
// Wrap clock counter
|
||
|
m_clockCount -= m_samplesPerBit;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
m_clock = 0;
|
||
|
}
|
||
|
|
||
|
sampleToScope(ci, abs1Filt, abs2Filt, unbiasedData, biasedData);
|
||
|
}
|
||
|
|
||
|
const QList<DSCDemodSink::PhasingPattern> DSCDemodSink::m_phasingPatterns = {
|
||
|
{0b1011111001'1111011001'1011111001, 9}, // 125 111 125
|
||
|
{0b1111011001'1011111001'0111011010, 8}, // 111 125 110
|
||
|
{0b1011111001'0111011010'1011111001, 7}, // 125 110 125
|
||
|
{0b0111011010'1011111001'1011011010, 6}, // 110 125 109
|
||
|
{0b1011111001'1011011010'1011111001, 5}, // 125 109 125
|
||
|
{0b1011011010'1011111001'0011011011, 4}, // 109 125 108
|
||
|
{0b1011111001'0011011011'1011111001, 3}, // 125 108 125
|
||
|
{0b0011011011'1011111001'1101011010, 2}, // 108 125 107
|
||
|
{0b1011111001'1101011010'1011111001, 1}, // 125 107 125
|
||
|
{0b1101011010'1011111001'0101011011, 0}, // 107 125 106
|
||
|
};
|
||
|
|
||
|
void DSCDemodSink::receiveBit(bool bit)
|
||
|
{
|
||
|
m_bit = bit;
|
||
|
|
||
|
// Store in shift reg
|
||
|
m_bits = (m_bits << 1) | m_bit;
|
||
|
m_bitCount++;
|
||
|
|
||
|
if (!m_gotSOP)
|
||
|
{
|
||
|
// Dot pattern - 200 1/0s or 20 1/0s
|
||
|
// Phasing pattern - 6 DX=125 RX=111 110 109 108 107 106 105 104
|
||
|
// Phasing is considered to be achieved when two DXs and one RX, or two RXs and one DX, or three RXs in the appropriate DX or RX positions, respectively, are successfully received.
|
||
|
if (m_bitCount == 10*3)
|
||
|
{
|
||
|
m_bitCount--;
|
||
|
|
||
|
unsigned int pat = m_bits & 0x3fffffff;
|
||
|
for (int i = 0; i < m_phasingPatterns.size(); i++)
|
||
|
{
|
||
|
if (pat == m_phasingPatterns[i].m_pattern)
|
||
|
{
|
||
|
m_dscDecoder.init(m_phasingPatterns[i].m_offset);
|
||
|
m_gotSOP = true;
|
||
|
m_bitCount = 0;
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
if (m_bitCount == 10)
|
||
|
{
|
||
|
if (m_dscDecoder.decodeBits(m_bits & 0x3ff))
|
||
|
{
|
||
|
QByteArray bytes = m_dscDecoder.getMessage();
|
||
|
DSCMessage message(bytes, QDateTime::currentDateTime());
|
||
|
//qDebug() << "RX Bytes: " << bytes.toHex();
|
||
|
//qDebug() << "DSC Message: " << message.toString();
|
||
|
|
||
|
if (getMessageQueueToChannel())
|
||
|
{
|
||
|
float rssi = CalcDb::dbPower(m_rssiMagSqSum / m_rssiMagSqCount);
|
||
|
DSCDemod::MsgMessage *msg = DSCDemod::MsgMessage::create(message, m_dscDecoder.getErrors(), rssi);
|
||
|
getMessageQueueToChannel()->push(msg);
|
||
|
}
|
||
|
|
||
|
// Reset demod
|
||
|
init();
|
||
|
}
|
||
|
m_bitCount = 0;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void DSCDemodSink::applyChannelSettings(int channelSampleRate, int channelFrequencyOffset, bool force)
|
||
|
{
|
||
|
qDebug() << "DSCDemodSink::applyChannelSettings:"
|
||
|
<< " channelSampleRate: " << channelSampleRate
|
||
|
<< " channelFrequencyOffset: " << channelFrequencyOffset;
|
||
|
|
||
|
if ((m_channelFrequencyOffset != channelFrequencyOffset) ||
|
||
|
(m_channelSampleRate != channelSampleRate) || force)
|
||
|
{
|
||
|
m_nco.setFreq(-channelFrequencyOffset, channelSampleRate);
|
||
|
}
|
||
|
|
||
|
if ((m_channelSampleRate != channelSampleRate) || force)
|
||
|
{
|
||
|
m_interpolator.create(16, channelSampleRate, m_settings.m_rfBandwidth / 2.2);
|
||
|
m_interpolatorDistance = (Real) channelSampleRate / (Real) DSCDemodSettings::DSCDEMOD_CHANNEL_SAMPLE_RATE;
|
||
|
m_interpolatorDistanceRemain = m_interpolatorDistance;
|
||
|
}
|
||
|
|
||
|
m_channelSampleRate = channelSampleRate;
|
||
|
m_channelFrequencyOffset = channelFrequencyOffset;
|
||
|
}
|
||
|
|
||
|
void DSCDemodSink::init()
|
||
|
{
|
||
|
m_expIdx = 0;
|
||
|
m_bit = 0;
|
||
|
m_bits = 0;
|
||
|
m_bitCount = 0;
|
||
|
m_gotSOP = false;
|
||
|
m_errorCount = 0;
|
||
|
m_clockCount = -m_samplesPerBit/2.0;
|
||
|
m_clock = 0;
|
||
|
m_int = 0.0;
|
||
|
m_rssiMagSqSum = 0.0;
|
||
|
m_rssiMagSqCount = 0;
|
||
|
m_consecutiveErrors = 0;
|
||
|
m_messageBuffer = "";
|
||
|
}
|
||
|
|
||
|
void DSCDemodSink::applySettings(const DSCDemodSettings& settings, bool force)
|
||
|
{
|
||
|
qDebug() << "DSCDemodSink::applySettings:"
|
||
|
<< " m_rfBandwidth: " << settings.m_rfBandwidth
|
||
|
<< " force: " << force;
|
||
|
|
||
|
if ((settings.m_rfBandwidth != m_settings.m_rfBandwidth) || force)
|
||
|
{
|
||
|
m_interpolator.create(16, m_channelSampleRate, settings.m_rfBandwidth / 2.2);
|
||
|
m_interpolatorDistance = (Real) m_channelSampleRate / (Real) DSCDemodSettings::DSCDEMOD_CHANNEL_SAMPLE_RATE;
|
||
|
m_interpolatorDistanceRemain = m_interpolatorDistance;
|
||
|
}
|
||
|
|
||
|
if (force)
|
||
|
{
|
||
|
delete[] m_exp;
|
||
|
m_exp = new Complex[m_expLength];
|
||
|
Real f0 = 0.0f;
|
||
|
for (int i = 0; i < m_expLength; i++)
|
||
|
{
|
||
|
m_exp[i] = Complex(cos(f0), sin(f0));
|
||
|
f0 += 2.0f * (Real)M_PI * (DSCDemodSettings::DSCDEMOD_FREQUENCY_SHIFT/2.0f) / DSCDemodSettings::DSCDEMOD_CHANNEL_SAMPLE_RATE;
|
||
|
}
|
||
|
init();
|
||
|
|
||
|
m_movMax1.setSize(m_samplesPerBit * 8);
|
||
|
m_movMax2.setSize(m_samplesPerBit * 8);
|
||
|
}
|
||
|
|
||
|
m_settings = settings;
|
||
|
}
|