///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2019 F4EXB //
// written by Edouard Griffiths //
// //
// 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 <QString>
#include <QDebug>
#include <algorithm>
#include "inthalfbandfilter.h"
#include "dspcommands.h"
#include "hbfilterchainconverter.h"
#include "upchannelizer.h"
UpChannelizer::UpChannelizer(ChannelSampleSource* sampleSource) :
m_filterChainSetMode(false),
m_sampleSource(sampleSource),
m_basebandSampleRate(0),
m_requestedInputSampleRate(0),
m_requestedCenterFrequency(0),
m_channelSampleRate(0),
m_channelFrequencyOffset(0),
m_log2Interp(0),
m_filterChainHash(0)
{
}
UpChannelizer::~UpChannelizer()
{
freeFilterChain();
}
void UpChannelizer::pullOne(Sample& sample)
{
if (m_sampleSource == nullptr)
{
m_sampleBuffer.clear();
return;
}
unsigned int log2Interp = m_filterStages.size();
if (log2Interp == 0) // optimization when no downsampling is done anyway
{
m_sampleSource->pullOne(sample);
}
else
{
FilterStages::iterator stage = m_filterStages.begin();
std::vector<Sample>::iterator stageSample = m_stageSamples.begin();
for (; stage != m_filterStages.end(); ++stage, ++stageSample)
{
if(stage == m_filterStages.end() - 1)
{
if ((*stage)->work(&m_sampleIn, &(*stageSample)))
{
m_sampleSource->pullOne(m_sampleIn); // get new input sample
}
}
else
{
if (!(*stage)->work(&(*(stageSample+1)), &(*stageSample)))
{
break;
}
}
}
sample = *m_stageSamples.begin();
}
}
void UpChannelizer::pull(SampleVector::iterator begin, unsigned int nbSamples)
{
if (m_sampleSource == nullptr)
{
m_sampleBuffer.clear();
return;
}
unsigned int log2Interp = m_filterStages.size();
if (log2Interp == 0) // optimization when no downsampling is done anyway
{
m_sampleSource->pull(begin, nbSamples);
}
else
{
std::for_each(
begin,
begin + nbSamples,
[this](Sample& s) {
pullOne(s);
}
);
}
}
void UpChannelizer::prefetch(unsigned int nbSamples)
{
unsigned int log2Interp = m_filterStages.size();
m_sampleSource->prefetch(nbSamples/(1<<log2Interp)); // 2^n less samples will be produced by the source
}
void UpChannelizer::setChannelization(int requestedSampleRate, qint64 requestedCenterFrequency)
{
m_requestedInputSampleRate = requestedSampleRate;
m_requestedCenterFrequency = requestedCenterFrequency;
applyChannelization();
}
void UpChannelizer::setBasebandSampleRate(int basebandSampleRate, bool interp)
{
m_basebandSampleRate = basebandSampleRate;
if (interp) {
applyInterpolation();
} else {
applyChannelization();
}
}
void UpChannelizer::applyChannelization()
{
m_filterChainSetMode = false;
if (m_basebandSampleRate == 0)
{
qDebug() << "UpChannelizer::applyConfiguration: aborting (out=0):"
<< " out:" << m_basebandSampleRate
<< " req:" << m_requestedInputSampleRate
<< " in:" << m_channelSampleRate
<< " fc:" << m_channelFrequencyOffset;
return;
}
freeFilterChain();
m_channelFrequencyOffset = createFilterChain(
m_basebandSampleRate / -2, m_basebandSampleRate / 2,
m_requestedCenterFrequency - m_requestedInputSampleRate / 2, m_requestedCenterFrequency + m_requestedInputSampleRate / 2);
m_channelSampleRate = m_basebandSampleRate / (1 << m_filterStages.size());
qDebug() << "UpChannelizer::applyConfiguration: done: "
<< " out:" << m_basebandSampleRate
<< " req:" << m_requestedInputSampleRate
<< " in:" << m_channelSampleRate
<< " fc:" << m_channelFrequencyOffset;
}
void UpChannelizer::setInterpolation(unsigned int log2Interp, unsigned int filterChainHash)
{
m_log2Interp = log2Interp;
m_filterChainHash = filterChainHash;
applyInterpolation();
}
void UpChannelizer::applyInterpolation()
{
m_filterChainSetMode = true;
std::vector<unsigned int> stageIndexes;
m_channelFrequencyOffset = m_basebandSampleRate * HBFilterChainConverter::convertToIndexes(m_log2Interp, m_filterChainHash, stageIndexes);
m_requestedCenterFrequency = m_channelFrequencyOffset;
freeFilterChain();
m_channelFrequencyOffset = m_basebandSampleRate * setFilterChain(stageIndexes);
m_channelSampleRate = m_basebandSampleRate / (1 << m_filterStages.size());
m_requestedInputSampleRate = m_channelSampleRate;
qDebug() << "UpChannelizer::applyInterpolation:"
<< " m_log2Interp:" << m_log2Interp
<< " m_filterChainHash:" << m_filterChainHash
<< " out:" << m_basebandSampleRate
<< " in:" << m_channelSampleRate
<< " fc:" << m_channelFrequencyOffset;
}
#ifdef USE_SSE4_1
UpChannelizer::FilterStage::FilterStage(Mode mode) :
m_filter(new IntHalfbandFilterEO1<UPCHANNELIZER_HB_FILTER_ORDER>),
m_workFunction(0)
{
switch(mode) {
case ModeCenter:
m_workFunction = &IntHalfbandFilterEO1<UPCHANNELIZER_HB_FILTER_ORDER>::workInterpolateCenter;
break;
case ModeLowerHalf:
m_workFunction = &IntHalfbandFilterEO1<UPCHANNELIZER_HB_FILTER_ORDER>::workInterpolateLowerHalf;
break;
case ModeUpperHalf:
m_workFunction = &IntHalfbandFilterEO1<UPCHANNELIZER_HB_FILTER_ORDER>::workInterpolateUpperHalf;
break;
}
}
#else
UpChannelizer::FilterStage::FilterStage(Mode mode) :
m_filter(new IntHalfbandFilterDB<qint32, UPCHANNELIZER_HB_FILTER_ORDER>),
m_workFunction(0)
{
switch(mode) {
case ModeCenter:
m_workFunction = &IntHalfbandFilterDB<qint32, UPCHANNELIZER_HB_FILTER_ORDER>::workInterpolateCenter;
break;
case ModeLowerHalf:
m_workFunction = &IntHalfbandFilterDB<qint32, UPCHANNELIZER_HB_FILTER_ORDER>::workInterpolateLowerHalf;
break;
case ModeUpperHalf:
m_workFunction = &IntHalfbandFilterDB<qint32, UPCHANNELIZER_HB_FILTER_ORDER>::workInterpolateUpperHalf;
break;
}
}
#endif
UpChannelizer::FilterStage::~FilterStage()
{
delete m_filter;
}
bool UpChannelizer::signalContainsChannel(Real sigStart, Real sigEnd, Real chanStart, Real chanEnd) const
{
//qDebug(" testing signal [%f, %f], channel [%f, %f]", sigStart, sigEnd, chanStart, chanEnd);
if(sigEnd <= sigStart)
return false;
if(chanEnd <= chanStart)
return false;
return (sigStart <= chanStart) && (sigEnd >= chanEnd);
}
Real UpChannelizer::createFilterChain(Real sigStart, Real sigEnd, Real chanStart, Real chanEnd)
{
Real sigBw = sigEnd - sigStart;
Real rot = sigBw / 4;
Sample s;
qDebug() << "UpChannelizer::createFilterChain: start:"
<< " sig: [" << sigStart << ":" << sigEnd << "]"
<< " BW: " << sigBw
<< " chan: [" << chanStart << ":" << chanEnd << "]"
<< " rot: " << rot;
// check if it fits into the left half
if(signalContainsChannel(sigStart, sigStart + sigBw / 2.0, chanStart, chanEnd))
{
qDebug() << "UpChannelizer::createFilterChain: take left half (rotate by +1/4 and decimate by 2):"
<< " [" << m_filterStages.size() << "]"
<< " sig: [" << sigStart << ":" << sigStart + sigBw / 2.0 << "]";
m_filterStages.push_back(new FilterStage(FilterStage::ModeLowerHalf));
m_stageSamples.push_back(s);
return createFilterChain(sigStart, sigStart + sigBw / 2.0, chanStart, chanEnd);
}
// check if it fits into the right half
if(signalContainsChannel(sigEnd - sigBw / 2.0f, sigEnd, chanStart, chanEnd))
{
qDebug() << "UpChannelizer::createFilterChain: take right half (rotate by -1/4 and decimate by 2):"
<< " [" << m_filterStages.size() << "]"
<< " sig: [" << sigEnd - sigBw / 2.0f << ":" << sigEnd << "]";
m_filterStages.push_back(new FilterStage(FilterStage::ModeUpperHalf));
m_stageSamples.push_back(s);
return createFilterChain(sigEnd - sigBw / 2.0f, sigEnd, chanStart, chanEnd);
}
// check if it fits into the center
// Was: if(signalContainsChannel(sigStart + rot + safetyMargin, sigStart + rot + sigBw / 2.0f - safetyMargin, chanStart, chanEnd)) {
if(signalContainsChannel(sigStart + rot, sigEnd - rot, chanStart, chanEnd))
{
qDebug() << "UpChannelizer::createFilterChain: take center half (decimate by 2):"
<< " [" << m_filterStages.size() << "]"
<< " sig: [" << sigStart + rot << ":" << sigEnd - rot << "]";
m_filterStages.push_back(new FilterStage(FilterStage::ModeCenter));
m_stageSamples.push_back(s);
// Was: return createFilterChain(sigStart + rot, sigStart + sigBw / 2.0f + rot, chanStart, chanEnd);
return createFilterChain(sigStart + rot, sigEnd - rot, chanStart, chanEnd);
}
Real ofs = ((chanEnd - chanStart) / 2.0 + chanStart) - ((sigEnd - sigStart) / 2.0 + sigStart);
qDebug() << "UpChannelizer::createFilterChain: complete:"
<< " #stages: " << m_filterStages.size()
<< " BW: " << sigBw
<< " ofs: " << ofs;
return ofs;
}
double UpChannelizer::setFilterChain(const std::vector<unsigned int>& stageIndexes)
{
// filters are described from lower to upper level but the chain is constructed the other way round
std::vector<unsigned int>::const_reverse_iterator rit = stageIndexes.rbegin();
double ofs = 0.0, ofs_stage = 0.25;
Sample s;
// Each index is a base 3 number with 0 = low, 1 = center, 2 = high
// Functions at upper level will convert a number to base 3 to describe the filter chain. Common converting
// algorithms will go from LSD to MSD. This explains the reverse order.
for (; rit != stageIndexes.rend(); ++rit)
{
if (*rit == 0)
{
m_filterStages.push_back(new FilterStage(FilterStage::ModeLowerHalf));
m_stageSamples.push_back(s);
ofs -= ofs_stage;
qDebug("UpChannelizer::setFilterChain: lower half: ofs: %f", ofs);
}
else if (*rit == 1)
{
m_filterStages.push_back(new FilterStage(FilterStage::ModeCenter));
m_stageSamples.push_back(s);
qDebug("UpChannelizer::setFilterChain: center: ofs: %f", ofs);
}
else if (*rit == 2)
{
m_filterStages.push_back(new FilterStage(FilterStage::ModeUpperHalf));
m_stageSamples.push_back(s);
ofs += ofs_stage;
qDebug("UpChannelizer::setFilterChain: upper half: ofs: %f", ofs);
}
ofs_stage /= 2;
}
qDebug() << "UpChannelizer::setFilterChain: complete:"
<< " #stages: " << m_filterStages.size()
<< " ofs: " << ofs;
return ofs;
}
void UpChannelizer::freeFilterChain()
{
for(FilterStages::iterator it = m_filterStages.begin(); it != m_filterStages.end(); ++it)
delete *it;
m_filterStages.clear();
m_stageSamples.clear();
}