mirror of
https://github.com/f4exb/sdrangel.git
synced 2024-12-23 18:15:45 -05:00
557 lines
16 KiB
C++
557 lines
16 KiB
C++
///////////////////////////////////////////////////////////////////////////////////
|
|
// Copyright (C) 2019-2020, 2022-2023 Edouard Griffiths, F4EXB <f4exb06@gmail.com> //
|
|
// Copyright (C) 2022 Jiří Pinkava <jiri.pinkava@rossum.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 <QDebug>
|
|
|
|
#include "codec2/freedv_api.h"
|
|
#include "codec2/modem_stats.h"
|
|
|
|
#include "dsp/basebandsamplesink.h"
|
|
#include "util/db.h"
|
|
|
|
#include "freedvdemodsink.h"
|
|
|
|
const unsigned int FreeDVDemodSink::m_ssbFftLen = 1024;
|
|
const float FreeDVDemodSink::m_agcTarget = 3276.8f; // -10 dB amplitude => -20 dB power: center of normal signal
|
|
|
|
FreeDVDemodSink::FreeDVStats::FreeDVStats()
|
|
{
|
|
init();
|
|
}
|
|
|
|
void FreeDVDemodSink::FreeDVStats::init()
|
|
{
|
|
m_sync = false;
|
|
m_snrEst = -20;
|
|
m_clockOffset = 0;
|
|
m_freqOffset = 0;
|
|
m_syncMetric = 0;
|
|
m_totalBitErrors = 0;
|
|
m_lastTotalBitErrors = 0;
|
|
m_ber = 0;
|
|
m_frameCount = 0;
|
|
m_berFrameCount = 0;
|
|
m_fps = 1;
|
|
}
|
|
|
|
void FreeDVDemodSink::FreeDVStats::collect(struct freedv *freeDV)
|
|
{
|
|
struct MODEM_STATS stats;
|
|
|
|
freedv_get_modem_extended_stats(freeDV, &stats);
|
|
m_totalBitErrors = freedv_get_total_bit_errors(freeDV);
|
|
m_clockOffset = stats.clock_offset;
|
|
m_freqOffset = stats.foff;
|
|
m_syncMetric = stats.sync_metric;
|
|
m_sync = stats.sync != 0;
|
|
m_snrEst = stats.snr_est;
|
|
|
|
if (m_berFrameCount >= m_fps)
|
|
{
|
|
m_ber = m_totalBitErrors - m_lastTotalBitErrors;
|
|
m_ber = m_ber < 0 ? 0 : m_ber;
|
|
m_berFrameCount = 0;
|
|
m_lastTotalBitErrors = m_totalBitErrors;
|
|
// qDebug("FreeDVStats::collect: demod sync: %s sync metric: %f demod snr: %3.2f dB BER: %d clock offset: %f freq offset: %f",
|
|
// m_sync ? "ok" : "ko", m_syncMetric, m_snrEst, m_ber, m_clockOffset, m_freqOffset);
|
|
}
|
|
|
|
m_berFrameCount++;
|
|
m_frameCount++;
|
|
}
|
|
|
|
FreeDVDemodSink::FreeDVSNR::FreeDVSNR()
|
|
{
|
|
m_sum = 0.0f;
|
|
m_peak = 0.0f;
|
|
m_n = 0;
|
|
m_reset = true;
|
|
}
|
|
|
|
void FreeDVDemodSink::FreeDVSNR::accumulate(float snrdB)
|
|
{
|
|
if (m_reset)
|
|
{
|
|
m_sum = CalcDb::powerFromdB(snrdB);
|
|
m_peak = snrdB;
|
|
m_n = 1;
|
|
m_reset = false;
|
|
}
|
|
else
|
|
{
|
|
m_sum += CalcDb::powerFromdB(snrdB);
|
|
m_peak = std::max(m_peak, snrdB);
|
|
m_n++;
|
|
}
|
|
}
|
|
|
|
FreeDVDemodSink::LevelRMS::LevelRMS()
|
|
{
|
|
m_sum = 0.0f;
|
|
m_peak = 0.0f;
|
|
m_n = 0;
|
|
m_reset = true;
|
|
}
|
|
|
|
void FreeDVDemodSink::LevelRMS::accumulate(float level)
|
|
{
|
|
if (m_reset)
|
|
{
|
|
m_sum = level * level;
|
|
m_peak = std::fabs(level);
|
|
m_n = 1;
|
|
m_reset = false;
|
|
}
|
|
else
|
|
{
|
|
m_sum += level * level;
|
|
m_peak = std::max(m_peak, std::fabs(level));
|
|
m_n++;
|
|
}
|
|
}
|
|
|
|
FreeDVDemodSink::FreeDVDemodSink() :
|
|
m_hiCutoff(6000),
|
|
m_lowCutoff(0),
|
|
m_volume(2),
|
|
m_spanLog2(3),
|
|
m_sum(fftfilt::cmplx{0,0}),
|
|
m_channelSampleRate(48000),
|
|
m_modemSampleRate(48000),
|
|
m_speechSampleRate(8000), // fixed 8 kS/s
|
|
m_audioSampleRate(48000),
|
|
m_channelFrequencyOffset(0),
|
|
m_audioMute(false),
|
|
m_simpleAGC(0.003f, 0.0f, 1e-6f),
|
|
m_agcActive(false),
|
|
m_squelchDelayLine(2*48000),
|
|
m_audioActive(false),
|
|
m_spectrumSink(nullptr),
|
|
m_audioFifo(24000),
|
|
m_freeDV(nullptr),
|
|
m_nSpeechSamples(0),
|
|
m_nMaxModemSamples(0),
|
|
m_iSpeech(0),
|
|
m_iModem(0),
|
|
m_speechOut(nullptr),
|
|
m_modIn(nullptr),
|
|
m_levelInNbSamples(480) // 10ms @ 48 kS/s
|
|
{
|
|
m_audioBuffer.resize(1<<14);
|
|
m_audioBufferFill = 0;
|
|
m_undersampleCount = 0;
|
|
|
|
m_magsq = 0.0f;
|
|
m_magsqSum = 0.0f;
|
|
m_magsqPeak = 0.0f;
|
|
m_magsqCount = 0;
|
|
|
|
m_simpleAGC.resizeNew(m_modemSampleRate/10, 0.003);
|
|
|
|
SSBFilter = new fftfilt(m_lowCutoff / m_modemSampleRate, m_hiCutoff / m_modemSampleRate, m_ssbFftLen);
|
|
m_SSBFilterBuffer = new fftfilt::cmplx[m_ssbFftLen];
|
|
std::fill(m_SSBFilterBuffer, m_SSBFilterBuffer + m_ssbFftLen, fftfilt::cmplx{0.0f, 0.0f});
|
|
m_SSBFilterBufferIndex = 0;
|
|
|
|
applyChannelSettings(m_channelSampleRate, m_channelFrequencyOffset, true);
|
|
applySettings(m_settings, true);
|
|
}
|
|
|
|
FreeDVDemodSink::~FreeDVDemodSink()
|
|
{
|
|
delete SSBFilter;
|
|
delete[] m_SSBFilterBuffer;
|
|
}
|
|
|
|
void FreeDVDemodSink::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end)
|
|
{
|
|
if (!m_freeDV) {
|
|
return;
|
|
}
|
|
|
|
QMutexLocker mlock(&m_mutex);
|
|
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
|
|
{
|
|
if (m_interpolator.decimate(&m_interpolatorDistanceRemain, c, &ci))
|
|
{
|
|
processOneSample(ci);
|
|
m_interpolatorDistanceRemain += m_interpolatorDistance;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (m_spectrumSink && (m_sampleBuffer.size() != 0))
|
|
{
|
|
m_spectrumSink->feed(m_sampleBuffer.begin(), m_sampleBuffer.end(), true);
|
|
m_sampleBuffer.clear();
|
|
}
|
|
}
|
|
|
|
void FreeDVDemodSink::processOneSample(Complex &ci)
|
|
{
|
|
fftfilt::cmplx *sideband;
|
|
int n_out = 0;
|
|
int decim = 1<<(m_spanLog2 - 1);
|
|
unsigned char decim_mask = decim - 1; // counter LSB bit mask for decimation by 2^(m_scaleLog2 - 1)
|
|
|
|
m_sum += m_SSBFilterBuffer[m_SSBFilterBufferIndex];
|
|
|
|
if (!(m_undersampleCount++ & decim_mask))
|
|
{
|
|
Real avgr = m_sum.real() / decim;
|
|
Real avgi = m_sum.imag() / decim;
|
|
m_magsq = (avgr*avgr + avgi*avgi) / (SDR_RX_SCALED*SDR_RX_SCALED);
|
|
|
|
m_magsqSum += m_magsq;
|
|
|
|
if (m_magsq > m_magsqPeak)
|
|
{
|
|
m_magsqPeak = m_magsq;
|
|
}
|
|
|
|
m_magsqCount++;
|
|
m_sampleBuffer.push_back(Sample(avgr, avgi));
|
|
m_sum.real(0.0);
|
|
m_sum.imag(0.0);
|
|
}
|
|
|
|
fftfilt::cmplx z = m_SSBFilterBuffer[m_SSBFilterBufferIndex];
|
|
Real demod = (z.real() + z.imag()) * 0.7;
|
|
// Real demod = m_SSBFilterBuffer[m_SSBFilterBufferIndex].real(); // works as good
|
|
|
|
if (m_agcActive)
|
|
{
|
|
m_simpleAGC.feed(demod);
|
|
demod *= (m_settings.m_volumeIn * 3276.8f) / m_simpleAGC.getValue(); // provision for peak to average ratio (here 10) compensated by m_volumeIn
|
|
}
|
|
else
|
|
{
|
|
demod *= m_settings.m_volumeIn;
|
|
}
|
|
|
|
pushSampleToDV((qint16) demod);
|
|
n_out = SSBFilter->runSSB(ci, &sideband, true); // always USB side
|
|
|
|
if (n_out > 0)
|
|
{
|
|
std::copy(sideband, sideband + n_out, m_SSBFilterBuffer);
|
|
m_SSBFilterBufferIndex = 0;
|
|
}
|
|
else if (m_SSBFilterBufferIndex < m_ssbFftLen - 1)
|
|
{
|
|
m_SSBFilterBufferIndex++;
|
|
}
|
|
}
|
|
|
|
void FreeDVDemodSink::pushSampleToDV(int16_t sample)
|
|
{
|
|
qint16 audioSample;
|
|
|
|
calculateLevel(sample);
|
|
|
|
if (m_iModem == m_nin)
|
|
{
|
|
int nout = freedv_rx(m_freeDV, m_speechOut, m_modIn);
|
|
m_freeDVStats.collect(m_freeDV);
|
|
m_freeDVSNR.accumulate(m_freeDVStats.m_snrEst);
|
|
|
|
if (m_settings.m_audioMute)
|
|
{
|
|
for (uint32_t i = 0; i < nout * m_audioResampler.getDecimation(); i++) {
|
|
pushSampleToAudio(0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (int i = 0; i < nout; i++)
|
|
{
|
|
while (!m_audioResampler.upSample(m_speechOut[i], audioSample)) {
|
|
pushSampleToAudio(audioSample);
|
|
}
|
|
|
|
pushSampleToAudio(audioSample);
|
|
}
|
|
}
|
|
|
|
m_iModem = 0;
|
|
m_iSpeech = 0;
|
|
}
|
|
|
|
m_modIn[m_iModem++] = sample;
|
|
}
|
|
|
|
void FreeDVDemodSink::calculateLevel(int16_t& sample)
|
|
{
|
|
if (m_levelIn.m_n >= m_levelInNbSamples)
|
|
{
|
|
m_rmsLevel = sqrt(m_levelIn.m_sum / m_levelInNbSamples);
|
|
m_peakLevel = m_levelIn.m_peak;
|
|
m_levelIn.m_reset = true;
|
|
}
|
|
|
|
m_levelIn.accumulate(sample/29491.2f); // scale on 90% (0.9 * 32768.0)
|
|
}
|
|
|
|
void FreeDVDemodSink::pushSampleToAudio(int16_t sample)
|
|
{
|
|
m_audioBuffer[m_audioBufferFill].l = sample * m_volume;
|
|
m_audioBuffer[m_audioBufferFill].r = sample * m_volume;
|
|
++m_audioBufferFill;
|
|
|
|
if (m_audioBufferFill >= m_audioBuffer.size())
|
|
{
|
|
std::size_t res = m_audioFifo.write((const quint8*)&m_audioBuffer[0], std::min(m_audioBufferFill, m_audioBuffer.size()));
|
|
|
|
if (res != m_audioBufferFill) {
|
|
qDebug("FreeDVDemodSink::pushSampleToAudio: %lu/%lu samples written", res, m_audioBufferFill);
|
|
}
|
|
|
|
m_audioBufferFill = 0;
|
|
}
|
|
}
|
|
|
|
void FreeDVDemodSink::applyChannelSettings(int channelSampleRate, int channnelFrequencyOffset, bool force)
|
|
{
|
|
qDebug() << "FreeDVDemodSink::applyChannelSettings:"
|
|
<< " channelSampleRate: " << channelSampleRate
|
|
<< " inputFrequencyOffset: " << channnelFrequencyOffset;
|
|
|
|
if ((m_channelFrequencyOffset != channnelFrequencyOffset) ||
|
|
(m_channelSampleRate != channelSampleRate) || force)
|
|
{
|
|
m_nco.setFreq(-channnelFrequencyOffset, channelSampleRate);
|
|
}
|
|
|
|
if ((m_channelSampleRate != channelSampleRate) || force)
|
|
{
|
|
m_interpolator.create(16, channelSampleRate, m_hiCutoff * 1.5f, 2.0f);
|
|
m_interpolatorDistanceRemain = 0;
|
|
m_interpolatorDistance = (Real) channelSampleRate / (Real) m_modemSampleRate;
|
|
}
|
|
|
|
m_channelSampleRate = channelSampleRate;
|
|
m_channelFrequencyOffset = channnelFrequencyOffset;
|
|
}
|
|
|
|
void FreeDVDemodSink::applyAudioSampleRate(int sampleRate)
|
|
{
|
|
if (sampleRate < 0)
|
|
{
|
|
qWarning("FreeDVDemodSink::applyAudioSampleRate: invalid sample rate: %d", sampleRate);
|
|
return;
|
|
}
|
|
|
|
qDebug("FreeDVDemodSink::applyAudioSampleRate: %d", sampleRate);
|
|
|
|
m_audioFifo.setSize(sampleRate);
|
|
m_audioResampler.setDecimation(sampleRate / m_speechSampleRate);
|
|
m_audioResampler.setAudioFilters(sampleRate, sampleRate, 250, 3300, 4.0f);
|
|
m_audioSampleRate = sampleRate;
|
|
}
|
|
|
|
void FreeDVDemodSink::applyFreeDVMode(FreeDVDemodSettings::FreeDVMode mode)
|
|
{
|
|
m_hiCutoff = FreeDVDemodSettings::getHiCutoff(mode);
|
|
m_lowCutoff = FreeDVDemodSettings::getLowCutoff(mode);
|
|
uint32_t modemSampleRate = FreeDVDemodSettings::getModSampleRate(mode);
|
|
QMutexLocker mlock(&m_mutex);
|
|
|
|
SSBFilter->create_filter(m_lowCutoff / (float) modemSampleRate, m_hiCutoff / (float) modemSampleRate);
|
|
|
|
// baseband interpolator
|
|
if (modemSampleRate != m_modemSampleRate)
|
|
{
|
|
m_interpolatorDistanceRemain = 0;
|
|
//m_interpolatorConsumed = false;
|
|
m_interpolatorDistance = (Real) m_channelSampleRate / (Real) modemSampleRate;
|
|
m_interpolator.create(16, m_channelSampleRate, m_hiCutoff * 1.5f, 2.0f);
|
|
m_modemSampleRate = modemSampleRate;
|
|
|
|
m_simpleAGC.resizeNew(modemSampleRate/10, 0.003);
|
|
m_levelInNbSamples = m_modemSampleRate / 100; // 10ms
|
|
}
|
|
|
|
// FreeDV object
|
|
|
|
if (m_freeDV) {
|
|
freedv_close(m_freeDV);
|
|
}
|
|
|
|
int fdv_mode = -1;
|
|
|
|
switch(mode)
|
|
{
|
|
case FreeDVDemodSettings::FreeDVMode700C:
|
|
fdv_mode = FREEDV_MODE_700C;
|
|
break;
|
|
case FreeDVDemodSettings::FreeDVMode700D:
|
|
fdv_mode = FREEDV_MODE_700D;
|
|
break;
|
|
case FreeDVDemodSettings::FreeDVMode800XA:
|
|
fdv_mode = FREEDV_MODE_800XA;
|
|
break;
|
|
case FreeDVDemodSettings::FreeDVMode1600:
|
|
fdv_mode = FREEDV_MODE_1600;
|
|
break;
|
|
case FreeDVDemodSettings::FreeDVMode2400A:
|
|
default:
|
|
fdv_mode = FREEDV_MODE_2400A;
|
|
break;
|
|
}
|
|
|
|
if (fdv_mode == FREEDV_MODE_700D)
|
|
{
|
|
struct freedv_advanced adv;
|
|
adv.interleave_frames = 1;
|
|
m_freeDV = freedv_open_advanced(fdv_mode, &adv);
|
|
}
|
|
else
|
|
{
|
|
m_freeDV = freedv_open(fdv_mode);
|
|
}
|
|
|
|
if (m_freeDV)
|
|
{
|
|
freedv_set_test_frames(m_freeDV, 0);
|
|
freedv_set_snr_squelch_thresh(m_freeDV, -100.0);
|
|
freedv_set_squelch_en(m_freeDV, 0);
|
|
freedv_set_clip(m_freeDV, 0);
|
|
freedv_set_ext_vco(m_freeDV, 0);
|
|
freedv_set_sync(m_freeDV, FREEDV_SYNC_MANUAL);
|
|
|
|
int nSpeechSamples = freedv_get_n_speech_samples(m_freeDV);
|
|
int nMaxModemSamples = freedv_get_n_max_modem_samples(m_freeDV);
|
|
int Fs = freedv_get_modem_sample_rate(m_freeDV);
|
|
int Rs = freedv_get_modem_symbol_rate(m_freeDV);
|
|
m_freeDVStats.init();
|
|
|
|
if (nSpeechSamples > m_nSpeechSamples)
|
|
{
|
|
if (m_speechOut) {
|
|
delete[] m_speechOut;
|
|
}
|
|
|
|
m_speechOut = new int16_t[nSpeechSamples];
|
|
m_nSpeechSamples = nSpeechSamples;
|
|
}
|
|
|
|
if (nMaxModemSamples > m_nMaxModemSamples)
|
|
{
|
|
if (m_modIn) {
|
|
delete[] m_modIn;
|
|
}
|
|
|
|
m_modIn = new int16_t[nMaxModemSamples];
|
|
m_nMaxModemSamples = nMaxModemSamples;
|
|
}
|
|
|
|
m_iSpeech = 0;
|
|
m_iModem = 0;
|
|
m_nin = freedv_nin(m_freeDV);
|
|
|
|
if (m_nin > 0) {
|
|
m_freeDVStats.m_fps = m_modemSampleRate / m_nin;
|
|
}
|
|
|
|
qDebug() << "FreeDVDemodSink::applyFreeDVMode:"
|
|
<< " fdv_mode: " << fdv_mode
|
|
<< " m_modemSampleRate: " << m_modemSampleRate
|
|
<< " m_lowCutoff: " << m_lowCutoff
|
|
<< " m_hiCutoff: " << m_hiCutoff
|
|
<< " Fs: " << Fs
|
|
<< " Rs: " << Rs
|
|
<< " m_nSpeechSamples: " << m_nSpeechSamples
|
|
<< " m_nMaxModemSamples: " << m_nMaxModemSamples
|
|
<< " m_nin: " << m_nin
|
|
<< " FPS: " << m_freeDVStats.m_fps;
|
|
}
|
|
else
|
|
{
|
|
qCritical("FreeDVDemodSink::applyFreeDVMode: m_freeDV was not allocated");
|
|
}
|
|
}
|
|
|
|
void FreeDVDemodSink::applySettings(const FreeDVDemodSettings& settings, bool force)
|
|
{
|
|
qDebug() << "FreeDVDemodSink::applySettings:"
|
|
<< " m_inputFrequencyOffset: " << settings.m_inputFrequencyOffset
|
|
<< " m_freeDVMode: " << (int) settings.m_freeDVMode
|
|
<< " m_volume: " << settings.m_volume
|
|
<< " m_volumeIn: " << settings.m_volumeIn
|
|
<< " m_spanLog2: " << settings.m_spanLog2
|
|
<< " m_audioMute: " << settings.m_audioMute
|
|
<< " m_agcActive: " << settings.m_agc
|
|
<< " m_audioDeviceName: " << settings.m_audioDeviceName
|
|
<< " m_streamIndex: " << settings.m_streamIndex
|
|
<< " m_useReverseAPI: " << settings.m_useReverseAPI
|
|
<< " m_reverseAPIAddress: " << settings.m_reverseAPIAddress
|
|
<< " m_reverseAPIPort: " << settings.m_reverseAPIPort
|
|
<< " m_reverseAPIDeviceIndex: " << settings.m_reverseAPIDeviceIndex
|
|
<< " m_reverseAPIChannelIndex: " << settings.m_reverseAPIChannelIndex
|
|
<< " force: " << force;
|
|
|
|
if ((m_settings.m_volume != settings.m_volume) || force)
|
|
{
|
|
m_volume = settings.m_volume;
|
|
m_volume /= 4.0; // for 3276.8
|
|
}
|
|
|
|
m_spanLog2 = settings.m_spanLog2;
|
|
m_audioMute = settings.m_audioMute;
|
|
m_agcActive = settings.m_agc;
|
|
m_settings = settings;
|
|
}
|
|
|
|
void FreeDVDemodSink::getSNRLevels(double& avg, double& peak, int& nbSamples)
|
|
{
|
|
if (m_freeDVSNR.m_n > 0)
|
|
{
|
|
avg = CalcDb::dbPower(m_freeDVSNR.m_sum / m_freeDVSNR.m_n);
|
|
peak = m_freeDVSNR.m_peak;
|
|
nbSamples = m_freeDVSNR.m_n;
|
|
m_freeDVSNR.m_reset = true;
|
|
}
|
|
else
|
|
{
|
|
avg = 0.0;
|
|
peak = 0.0;
|
|
nbSamples = 1;
|
|
}
|
|
}
|
|
|
|
void FreeDVDemodSink::resyncFreeDV()
|
|
{
|
|
QMutexLocker mlock(&m_mutex);
|
|
freedv_set_sync(m_freeDV, FREEDV_SYNC_UNSYNC);
|
|
}
|