252 lines
5.5 KiB
C++
252 lines
5.5 KiB
C++
/*
|
|
* Polyphase channelizer
|
|
*
|
|
* Copyright (C) 2012-2014 Tom Tsou <tom@tsou.cc>
|
|
* Copyright (C) 2015 Ettus Research LLC
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU Affero General Public License as published by
|
|
* the Free Software Foundation, either 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 Affero General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Affero General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
* See the COPYING file in the main directory for details.
|
|
*/
|
|
|
|
#include <malloc.h>
|
|
#include <math.h>
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <cstdio>
|
|
|
|
#include "Logger.h"
|
|
#include "ChannelizerBase.h"
|
|
|
|
extern "C" {
|
|
#include "fft.h"
|
|
}
|
|
|
|
static float sinc(float x)
|
|
{
|
|
if (x == 0.0f)
|
|
return 0.999999999999f;
|
|
|
|
return sin(M_PI * x) / (M_PI * x);
|
|
}
|
|
|
|
/*
|
|
* There are more efficient reversal algorithms, but we only reverse at
|
|
* initialization so we don't care.
|
|
*/
|
|
static void reverse(float *buf, size_t len)
|
|
{
|
|
float tmp[2 * len];
|
|
memcpy(tmp, buf, 2 * len * sizeof(float));
|
|
|
|
for (size_t i = 0; i < len; i++) {
|
|
buf[2 * i + 0] = tmp[2 * (len - 1 - i) + 0];
|
|
buf[2 * i + 1] = tmp[2 * (len - 1 - i) + 1];
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Create polyphase filterbank
|
|
*
|
|
* Implementation based material found in,
|
|
*
|
|
* "harris, fred, Multirate Signal Processing, Upper Saddle River, NJ,
|
|
* Prentice Hall, 2006."
|
|
*/
|
|
bool ChannelizerBase::initFilters()
|
|
{
|
|
size_t protoLen = m * hLen;
|
|
float *proto;
|
|
float sum = 0.0f, scale = 0.0f;
|
|
float midpt = (float) (protoLen - 1.0) / 2.0;
|
|
|
|
/*
|
|
* Allocate 'M' partition filters and the temporary prototype
|
|
* filter. Coefficients are real only and must be 16-byte memory
|
|
* aligned for SSE usage.
|
|
*/
|
|
proto = new float[protoLen];
|
|
if (!proto)
|
|
return false;
|
|
|
|
subFilters = (float **) malloc(sizeof(float *) * m);
|
|
if (!subFilters) {
|
|
delete[] proto;
|
|
return false;
|
|
}
|
|
|
|
for (size_t i = 0; i < m; i++) {
|
|
subFilters[i] = (float *)
|
|
memalign(16, hLen * 2 * sizeof(float));
|
|
}
|
|
|
|
/*
|
|
* Generate the prototype filter with a Blackman-harris window.
|
|
* Scale coefficients with DC filter gain set to unity divided
|
|
* by the number of channels.
|
|
*/
|
|
float a0 = 0.35875;
|
|
float a1 = 0.48829;
|
|
float a2 = 0.14128;
|
|
float a3 = 0.01168;
|
|
|
|
for (size_t i = 0; i < protoLen; i++) {
|
|
proto[i] = sinc(((float) i - midpt) / (float) m);
|
|
proto[i] *= a0 -
|
|
a1 * cos(2 * M_PI * i / (protoLen - 1)) +
|
|
a2 * cos(4 * M_PI * i / (protoLen - 1)) -
|
|
a3 * cos(6 * M_PI * i / (protoLen - 1));
|
|
sum += proto[i];
|
|
}
|
|
scale = (float) m / sum;
|
|
|
|
/*
|
|
* Populate partition filters and reverse the coefficients per
|
|
* convolution requirements.
|
|
*/
|
|
for (size_t i = 0; i < hLen; i++) {
|
|
for (size_t n = 0; n < m; n++) {
|
|
subFilters[n][2 * i + 0] = proto[i * m + n] * scale;
|
|
subFilters[n][2 * i + 1] = 0.0f;
|
|
}
|
|
}
|
|
|
|
for (size_t i = 0; i < m; i++)
|
|
reverse(subFilters[i], hLen);
|
|
|
|
delete[] proto;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool ChannelizerBase::initFFT()
|
|
{
|
|
size_t size;
|
|
|
|
if (fftInput || fftOutput || fftHandle)
|
|
return false;
|
|
|
|
size = blockLen * m * 2 * sizeof(float);
|
|
fftInput = (float *) fft_malloc(size);
|
|
memset(fftInput, 0, size);
|
|
|
|
size = (blockLen + hLen) * m * 2 * sizeof(float);
|
|
fftOutput = (float *) fft_malloc(size);
|
|
memset(fftOutput, 0, size);
|
|
|
|
if (!fftInput | !fftOutput) {
|
|
LOG(ALERT) << "Memory allocation error";
|
|
return false;
|
|
}
|
|
|
|
fftHandle = init_fft(0, m, blockLen, blockLen + hLen,
|
|
fftInput, fftOutput, hLen);
|
|
return true;
|
|
}
|
|
|
|
bool ChannelizerBase::mapBuffers()
|
|
{
|
|
if (!fftHandle) {
|
|
LOG(ALERT) << "FFT buffers not initialized";
|
|
return false;
|
|
}
|
|
|
|
hInputs = (float **) malloc(sizeof(float *) * m);
|
|
hOutputs = (float **) malloc(sizeof(float *) * m);
|
|
if (!hInputs | !hOutputs)
|
|
return false;
|
|
|
|
for (size_t i = 0; i < m; i++) {
|
|
hInputs[i] = &fftOutput[2 * (i * (blockLen + hLen) + hLen)];
|
|
hOutputs[i] = &fftInput[2 * (i * blockLen)];
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
* Setup filterbank internals
|
|
*/
|
|
bool ChannelizerBase::init()
|
|
{
|
|
/*
|
|
* Filterbank coefficients, fft plan, history, and output sample
|
|
* rate conversion blocks
|
|
*/
|
|
if (!initFilters()) {
|
|
LOG(ALERT) << "Failed to initialize channelizing filter";
|
|
return false;
|
|
}
|
|
|
|
hist = (float **) malloc(sizeof(float *) * m);
|
|
for (size_t i = 0; i < m; i++) {
|
|
hist[i] = new float[2 * hLen];
|
|
memset(hist[i], 0, 2 * hLen * sizeof(float));
|
|
}
|
|
|
|
if (!initFFT()) {
|
|
LOG(ALERT) << "Failed to initialize FFT";
|
|
return false;
|
|
}
|
|
|
|
mapBuffers();
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Check vector length validity */
|
|
bool ChannelizerBase::checkLen(size_t innerLen, size_t outerLen)
|
|
{
|
|
if (outerLen != innerLen * m) {
|
|
LOG(ALERT) << "Invalid outer length " << innerLen
|
|
<< " is not multiple of " << blockLen;
|
|
return false;
|
|
}
|
|
|
|
if (innerLen != blockLen) {
|
|
LOG(ALERT) << "Invalid inner length " << outerLen
|
|
<< " does not equal " << blockLen;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
* Setup channelizer paramaters
|
|
*/
|
|
ChannelizerBase::ChannelizerBase(size_t m, size_t blockLen, size_t hLen)
|
|
: fftInput(NULL), fftOutput(NULL), fftHandle(NULL)
|
|
{
|
|
this->m = m;
|
|
this->hLen = hLen;
|
|
this->blockLen = blockLen;
|
|
}
|
|
|
|
ChannelizerBase::~ChannelizerBase()
|
|
{
|
|
free_fft(fftHandle);
|
|
|
|
for (size_t i = 0; i < m; i++) {
|
|
free(subFilters[i]);
|
|
delete hist[i];
|
|
}
|
|
|
|
fft_free(fftInput);
|
|
fft_free(fftOutput);
|
|
|
|
free(hInputs);
|
|
free(hOutputs);
|
|
free(hist);
|
|
}
|