diff --git a/modules/modules.pro b/modules/modules.pro index 49e9200aef57def33c6fc499d47cd88ef4b1dd3c..c8c68236ba654aab252708d8260bc626da6b2020 100644 --- a/modules/modules.pro +++ b/modules/modules.pro @@ -1,10 +1,12 @@ TEMPLATE = subdirs #main framework project -SUBDIRS += sources/source_plutosdr \ +SUBDIRS += \ + sources/source_plutosdr \ transforms/transform_fft \ transforms/mod_fm \ transforms/filter_fir \ + transforms/resample_pqfraction \ sources/source_files \ sinks/sink_file \ sinks/sink_plutosdr \ diff --git a/modules/transforms/CMakeLists.txt b/modules/transforms/CMakeLists.txt index 2309d6d48ec883ffc2273471ab91c77f17adb105..6e95ca32eb824c98462c3df65ff1467b2cbdd553 100644 --- a/modules/transforms/CMakeLists.txt +++ b/modules/transforms/CMakeLists.txt @@ -3,3 +3,4 @@ cmake_minimum_required(VERSION 3.5) add_subdirectory(mod_fm) add_subdirectory(transform_fft) add_subdirectory(filter_fir) +add_subdirectory(resample_pqfraction) diff --git a/modules/transforms/filter_fir/main.cpp b/modules/transforms/filter_fir/main.cpp index f8d2e7606cd9680c670c52a28d571d3b5337dc0a..11940b3c852af89f47b952d42b54b8fbb672a270 100644 --- a/modules/transforms/filter_fir/main.cpp +++ b/modules/transforms/filter_fir/main.cpp @@ -76,7 +76,7 @@ int do_fir(const cmdlineParser & args) const unsigned int iinput = args.toInt("in",0); const unsigned int i_tmin = args.toInt("in_time",0); const unsigned int i_tmout = args.toInt("out_time",0); - const double iout = args.toInt("out",0); + const int iout = args.toInt("out",0); std::vector hn = args.toDoubleArray("hn"); //工作模式 const int sptype = args.toInt("sptype",0); fprintf(stderr,"sptype is %d.",sptype); diff --git a/modules/transforms/resample_pqfraction/CMakeLists.txt b/modules/transforms/resample_pqfraction/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..04ad6d32bcc13394e0f4e5d5d0ecd88904993251 --- /dev/null +++ b/modules/transforms/resample_pqfraction/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.5) + +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOUIC ON) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(QT NAMES Qt6 COMPONENTS Core REQUIRED) +find_package(Qt${QT_VERSION_MAJOR} COMPONENTS Core REQUIRED) +include_directories(${TASKBUS_INTERFACEDIR}) +set(PRJ_SOURCES + resample.h + upfirdn.h + main.cpp + resample_pqfraction.qrc +) +#############Target====================== + +add_executable(resample_pqfraction + ${PRJ_SOURCES} +) +target_link_libraries(resample_pqfraction Qt${QT_VERSION_MAJOR}::Core) + diff --git a/modules/transforms/resample_pqfraction/main.cpp b/modules/transforms/resample_pqfraction/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..af8f261c4b31fccd2bdb1760e4a446ed49b48d24 --- /dev/null +++ b/modules/transforms/resample_pqfraction/main.cpp @@ -0,0 +1,398 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "cmdlineparser.h" +#include "tb_interface.h" +#include "resample.h" +using namespace TASKBUS; +const int OFFLINEDEBUG = 0; +//数据源方法 +int do_resample(const cmdlineParser & args); +//全局的终止标记 +static bool bfinished = false; +using namespace std; +int main(int argc , char * argv[]) +{ + QCoreApplication a(argc, argv); + //重要！设置输入输出为二进制！ + init_client(); + + //解释命令行 + cmdlineParser args; + if (OFFLINEDEBUG==0) + args.parser(argc,argv); + else + { + FILE * old_stdin, *old_stdout; + auto ars = debug("/home/user/codes/build-taskbus-linux-Release/bin/debug/pid7852/",&old_stdin,&old_stdout); + args.parser(ars); + } + + int ret = 0; + //每个模块要响应 --information参数,打印自己的功能定义字符串。或者提供一个json文件。 + if (args.contains("information")) + { + QFile fp(":/resample_pqfraction.json"); + if (fp.open(QIODevice::ReadOnly)==false) + { + fp.setFileName(":/json/filter_fir.json"); + fp.open(QIODevice::ReadOnly); + } + if (fp.isOpen()) + { + QByteArray arr = fp.readAll(); + arr.push_back('\0'); + puts(arr.constData()); + fflush(stdout); + } + ret = 0; + } + else if (args.contains("function"/*,"filter_fir"*/))//正常运行模式 + { + ret = do_resample(args); + } + else + { + fprintf(stderr,"Error:Function does not exits."); + ret = -1; + } + + return ret; +} + +std::vector input_f[2],output_f[2]; +std::vector out_cache; +int kUpfactor = 1, kDownfactor = 2; + +unsigned int instance = 0; +unsigned int iinput = 0; +unsigned int i_tmin = 0; +unsigned int i_tmout = 0; +int iout = 0; + + +unsigned long long input_clk = 0, output_clk = 0; +double doutin_clock = 0, doutin_ratio = 1; +std::map > timestamp_vec; +unsigned long long next_timestamps = 0; + +bool tmstamp_exists = false; +bool tmstamp_begin =false; +void calc_resample() +{ + if (input_f[0].size()) + resample ( kUpfactor, kDownfactor, input_f[0], output_f[0] ); + if (input_f[1].size()) + resample ( kUpfactor, kDownfactor, input_f[1], output_f[1] ); + + //缓存 + const int sizei = output_f[0].size(); + const int sizeq = output_f[1].size(); + for (int i =0;i next_timestamps) + { + //保持时续的一一对应 + long long commit_clk = output_clk; + double commit_dclk = doutin_clock; + for (int i =0;i= next_timestamps) + { + if (iout ) + { + push_subject(iout,instance, i * spsize, + (unsigned char *)out_cache.data() + ); + out_cache.erase(out_cache.begin(),out_cache.begin()+i*spsize/2); + } + //播发老时戳的数据 + if (i_tmout && timestamp_vec.size()) + push_subject(i_tmout,instance,timestamp_vec.begin()->second.size(), + (unsigned char *)timestamp_vec.begin()->second.data() + ); + //新时戳 + if (timestamp_vec.size()) + timestamp_vec.erase(timestamp_vec.begin()); + next_timestamps = -1;//max + if (timestamp_vec.size()) + next_timestamps = timestamp_vec.begin()->first; + output_clk = commit_clk; + doutin_clock = commit_dclk; + break; + } + ++commit_clk; + commit_dclk += doutin_ratio; + } + sizei = out_cache.size()*2/spsize; + sizeq = out_cache.size()*2/spsize; + } + + } + else + { + //播发 + if (iout) + { + push_subject(iout,instance,out_cache.size()*2, + (unsigned char *)out_cache.data() + ); + } + out_cache.clear(); + } +} + + +int do_resample(const cmdlineParser & args) +{ + using namespace TASKBUS; + int res = 0; + //获得平台告诉自己的实例名 + instance = args.toInt("instance",0); + iinput = args.toInt("in",0); + i_tmin = args.toInt("in_time",0); + i_tmout = args.toInt("out_time",0); + iout = args.toInt("out",0); + const int np = args.toInt("np",2); + const int nq = args.toInt("nq",1); + //工作模式 + const int sptype = args.toInt("sptype",0); fprintf(stderr,"sptype is %d.",sptype); + int type = args.toInt("type",1); + if (type<0 || type >1) + type = 0; + + fflush(stderr); + + //计算翻转倍数 + kUpfactor = np; + kDownfactor = nq; + + fprintf(stderr,"NP=%d,NQ=%d\n",np,nq); + + doutin_ratio = 1.0*kDownfactor/kUpfactor; + int nBatchSize = kDownfactor; + while (nBatchSize<1000) + nBatchSize *= 2; + + input_f[0].resize(nBatchSize); + if (type==1) + input_f[1].resize(nBatchSize); + + + try{ + //判断参数合法性 + if (instance==0) + throw "function=quit;{\"error\":\"instance is 0, quit.\"}"; + int failed_header = 0; + while (false==bfinished) + { + subject_package_header header; + vector packagedta = pull_subject(&header); + if (is_valid_header(header)==false) + { + if (++failed_header>16) + bfinished = true; + continue; + } + if ( is_control_subject(header)) + { + //收到命令进程退出的广播消息,退出 + if (strstr(control_subject(header,packagedta).c_str(),"function=quit;")!=nullptr) + bfinished = true; + } + else if (header.subject_id==i_tmin && i_tmout) + { + tmstamp_exists = true; + if (!tmstamp_begin ) + { + if (i_tmout) + push_subject(i_tmout,instance,packagedta.size(), + (unsigned char *)packagedta.data() + ); + } + else + { + if (timestamp_vec.size()==0) + next_timestamps = input_clk; + timestamp_vec[input_clk] = std::move(packagedta); + broadresult(); + } + tmstamp_begin = true; + } + else if (header.subject_id == iinput) + { + //Input Buffer + switch(sptype) + { + //Intel 16bit + case 0:{ + if (type==0) + { + const short * pdata = (const short *) packagedta.data(); + const int samples = packagedta.size()/sizeof(short); + for (int spid = 0;spid < samples; ++ spid) + { + input_f[0][input_clk % nBatchSize] = pdata[spid]; + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + else + { + const short (* pdata)[2] = (const short (*)[2]) packagedta.data(); + const int samples = packagedta.size()/sizeof(short)/2; + for (int spid = 0;spid < samples; ++ spid) + { + input_f[0][input_clk % nBatchSize] = pdata[spid][0]; + input_f[1][input_clk % nBatchSize] = pdata[spid][1]; + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + + } + break; + //16bit U + case 1:{ + if (type==0) + { + const unsigned char * pdata = (const unsigned char *) packagedta.data(); + const int samples = packagedta.size()/sizeof(short); + for (int spid = 0;spid < samples; ++ spid) + { + unsigned char pt[2] = {pdata[spid*2+1],pdata[spid*2]}; + short * ptv = (short *)pt; + input_f[0][input_clk % nBatchSize] = *ptv; + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + else + { + const unsigned char * pdata = (const unsigned char *) packagedta.data(); + const int samples = packagedta.size()/sizeof(short)/2; + for (int spid = 0;spid < samples; ++ spid) + { + unsigned char pt[2][2] = { + {pdata[spid*4+1],pdata[spid*4]}, + {pdata[2+spid*4+1],pdata[2+spid*4]} + }; + short * ptv[2] = {(short *)pt[0],(short *)pt[1]}; + input_f[0][input_clk % nBatchSize] = *(ptv[0]); + input_f[1][input_clk % nBatchSize] = *(ptv[1]); + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + + } + break; + //int8 + case 2:{ + if (type==0) + { + const char * pdata = (const char *) packagedta.data(); + const int samples = packagedta.size()/sizeof(char); + for (int spid = 0;spid < samples; ++ spid) + { + input_f[0][input_clk % nBatchSize] = pdata[spid]; + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + else + { + const char (* pdata)[2] = (const char ( *)[2]) packagedta.data(); + const int samples = packagedta.size()/sizeof(char)/2; + for (int spid = 0;spid < samples; ++ spid) + { + input_f[0][input_clk % nBatchSize] = pdata[spid][0]; + input_f[1][input_clk % nBatchSize] = pdata[spid][1]; + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + + } + break; + case 3:{ + if (type==0) + { + const unsigned char * pdata = (const unsigned char *) packagedta.data(); + const int samples = packagedta.size()/sizeof(char); + for (int spid = 0;spid < samples; ++spid ) + { + input_f[0][input_clk % nBatchSize] = pdata[spid]; + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + else + { + const unsigned (* pdata)[2] = (const unsigned ( *)[2]) packagedta.data(); + const int samples = packagedta.size()/sizeof(char)/2; + for (int spid = 0;spid < samples; ++ spid) + { + input_f[0][input_clk % nBatchSize] = pdata[spid][0]; + input_f[1][input_clk % nBatchSize] = pdata[spid][1]; + ++input_clk; + if (input_clk % nBatchSize==0) + calc_resample(); + } + } + } + break; + default: + break; + } + if (!tmstamp_exists) + broadresult(); + + } + } + + + } + catch (const char * errMessage) + { + //向所有部位广播，偶要退出。 + push_subject(control_subect_id(),/*instance,broadcast_destin_id(),*/0,errMessage); + fprintf(stderr,"Error:%s.",errMessage); + fflush (stderr); + res = -1; + } + + return res; +} + diff --git a/modules/transforms/resample_pqfraction/resample.h b/modules/transforms/resample_pqfraction/resample.h new file mode 100644 index 0000000000000000000000000000000000000000..baca35fa4c54be3540d3a3811987a1e488eefcfb --- /dev/null +++ b/modules/transforms/resample_pqfraction/resample.h @@ -0,0 +1,189 @@ +//RESAMPLE Change the sampling rate of a signal. +// Y = RESAMPLE(UpFactor, DownFactor, InputSignal, OutputSignal) resamples the sequence in +// vector InputSignal at UpFactor/DownFactor times and stores the resampled data to OutputSignal. +// OutputSignal is UpFactor/DownFactor times the length of InputSignal. UpFactor and DownFactor must be +// positive integers. + +//This function is translated from Matlab's Resample funtion. + +//Author: Haoqi Bai + +#pragma once + +#include +#include +#include +#include +#include "upfirdn.h" + +using std::vector; + +template +T sinc ( T x ) +{ + if ( std::abs ( x - 0.0 ) < 0.000001 ) + return 1; + return std::sin ( M_PI * x ) / ( M_PI * x ); +} + +inline int quotientCeil ( int num1, int num2 ) +{ + if ( num1 % num2 != 0 ) + return num1 / num2 + 1; + return num1 / num2; +} + +template +std::vector firls ( int length, vector freq, const vector& amplitude) +{ + int freqSize = freq.size (); + int weightSize = freqSize / 2; + + vector weight(weightSize, 1.0); + + int filterLength = length + 1; + + for (auto &it: freq) + it /= 2.0; + + length = ( filterLength - 1 ) / 2; + bool Nodd = filterLength & 1; + vector k( length + 1 ); + std::iota(k.begin(), k.end(), 0.0); + if (!Nodd) { + for (auto &it : k) + it += 0.5; + } + + T b0 = 0.0; + if (Nodd) { + k.erase(k.begin()); + } + + vector b(k.size(), 0.0); + for ( int i = 0; i < freqSize; i += 2 ) + { + auto Fi = freq[i]; + auto Fip1 = freq[i+1]; + auto ampi = amplitude[i]; + auto ampip1 = amplitude[i+1]; + auto wt2 = std::pow(weight[i/2], 2); + auto m_s = (ampip1-ampi)/(Fip1-Fi); + auto b1 = ampi-(m_s*Fi); + if (Nodd) + { + b0 += (b1*(Fip1-Fi)) + m_s/2*(std::pow(Fip1, 2)-std::pow(Fi, 2))*wt2; + } + std::transform(b.begin(), b.end(), k.begin(),b.begin(), + [m_s, Fi, Fip1, wt2](T b, T k) { + return b + (m_s/(4*std::pow(M_PI, 2))* + (std::cos(2*M_PI*Fip1)-std::cos(2*M_PI*Fi))/(std::pow(k, 2)))*wt2;}); + std::transform(b.begin(), b.end(), k.begin(), b.begin(), + [m_s, Fi, Fip1, wt2, b1](T b, T k) { + return b + (Fip1*(m_s*Fip1+b1)*sinc(2*k*Fip1) - + Fi*(m_s*Fi+b1)*sinc(2*k*Fi))*wt2;}); + } + + if (Nodd) + { + b.insert(b.begin(), b0); + } + + auto w0 = weight[0]; + vector a(b.size()); + std::transform(b.begin(), b.end(), + a.begin(), + [w0](T b) {return std::pow(w0, 2)*4*b;}); + + vector result = {a.rbegin(), a.rend()}; + decltype(a.begin()) it; + if (Nodd) + { + it = a.begin()+1; + } + else + { + it = a.begin(); + } + result.insert(result.end(), it, a.end()); + + for (auto &it : result) { + it *= 0.5; + } + + return result; +} + +template +std::vector kaiser ( const int order, const T bta ) +{ + T Numerator, Denominator; + Denominator = std::cyl_bessel_i(0, bta); + auto od2 = (static_cast(order)-1)/2; + std::vector window; + window.reserve(order); + for (int n = 0; n < order; n++) { + auto x = bta*std::sqrt(1-std::pow((n-od2)/od2, 2)); + Numerator = std::cyl_bessel_i(0, x); + window.push_back(Numerator / Denominator); + } + return window; +} + +template +void resample ( int upFactor, int downFactor, + vector& inputSignal, vector& outputSignal ) +{ + const int n = 10; + const T bta = 5.0; + if ( upFactor <= 0 || downFactor <= 0 ) + throw std::runtime_error ( "factors must be positive integer" ); + int gcd_o = std::gcd ( upFactor, downFactor ); + upFactor /= gcd_o; + downFactor /= gcd_o; + + if ( upFactor == downFactor ) + { + outputSignal = inputSignal; + return; + } + + int inputSize = inputSignal.size(); + outputSignal.clear (); + int outputSize = quotientCeil ( inputSize * upFactor, downFactor ); + outputSignal.reserve ( outputSize ); + + int maxFactor = std::max ( upFactor, downFactor ); + T firlsFreq = 1.0 / 2.0 / static_cast ( maxFactor ); + int length = 2 * n * maxFactor + 1; + vector firlsFreqsV = { 0.0, 2 * firlsFreq, 2 * firlsFreq, 1.0 }; + vector firlsAmplitudeV = { 1.0, 1.0, 0.0, 0.0 }; + vector coefficients = firls ( length - 1, firlsFreqsV, firlsAmplitudeV); + vector window = kaiser ( length, bta ); + int coefficientsSize = coefficients.size(); + for( int i = 0; i < coefficientsSize; i++ ) + coefficients[i] *= upFactor * window[i]; + + int lengthHalf = ( length - 1 ) / 2; + int nz = downFactor - lengthHalf % downFactor; + vector h; + h.reserve ( coefficientsSize + nz ); + for ( int i = 0; i < nz; i++ ) + h.push_back ( 0.0 ); + for ( int i = 0; i < coefficientsSize; i++ ) + h.push_back ( coefficients[i] ); + int hSize = h.size(); + lengthHalf += nz; + int delay = lengthHalf / downFactor; + nz = 0; + while ( quotientCeil( ( inputSize - 1 ) * upFactor + hSize + nz, downFactor ) - delay < outputSize ) + nz++; + for ( int i = 0; i < nz; i++ ) + h.push_back ( 0.0 ); + vector y; + upfirdn ( upFactor, downFactor, inputSignal, h, y ); + for ( int i = delay; i < outputSize + delay; i++ ) + { + outputSignal.push_back ( y[i] ); + } +} diff --git a/modules/transforms/resample_pqfraction/resample_pqfraction.json b/modules/transforms/resample_pqfraction/resample_pqfraction.json new file mode 100644 index 0000000000000000000000000000000000000000..b9e2899678043d73d94bdf1e7e9568becc650973 --- /dev/null +++ b/modules/transforms/resample_pqfraction/resample_pqfraction.json @@ -0,0 +1,59 @@ +{ + "resample_pqfraction":{ + "name":"resamplepq", + "parameters":{ + "sptype":{ + "type":"enum", + "tooltip":"sample point format", + "default":0, + "range":{ + "0":"16 bit Intel", + "1":"16 bit Moto", + "2":"int8", + "3":"uint8" + } + }, + "type":{ + "type":"int", + "tooltip":"type", + "default":"1", + "range":"0:real,1:complex" + }, + "np":{ + "type":"int", + "tooltip":"new rate", + "default":"1", + "range":"N" + }, + "nq":{ + "type":"int", + "tooltip":"old rate", + "default":"1", + "range":"N" + } + }, + "input_subjects": + { + "in":{ + "type":"byte", + "tooltip":"Input" + }, + "in_time":{ + "type":"timestamp", + "tooltip":"Input timestamp" + } + }, + "output_subjects":{ + "out":{ + "type":"vector", + "tooltip":"Output" + }, + "out_time":{ + "type":"timestamp", + "tooltip":"Out timestamp" + } + + } + } +} + diff --git a/modules/transforms/resample_pqfraction/resample_pqfraction.pro b/modules/transforms/resample_pqfraction/resample_pqfraction.pro new file mode 100644 index 0000000000000000000000000000000000000000..4fcd30eeed28d987642e51ea485512efc4034ff1 --- /dev/null +++ b/modules/transforms/resample_pqfraction/resample_pqfraction.pro @@ -0,0 +1,12 @@ +TEMPLATE = app +QT -= gui +CONFIG += c++11 console +CONFIG -= app_bundle +INCLUDEPATH += ../../../tb_interface +DESTDIR = $$OUT_PWD/../../../bin/modules +SOURCES += \ + main.cpp +HEADERS += \ + upfirdn.h + resample.h +RESOURCES += resample_pqfraction.qrc diff --git a/modules/transforms/resample_pqfraction/resample_pqfraction.qrc b/modules/transforms/resample_pqfraction/resample_pqfraction.qrc new file mode 100644 index 0000000000000000000000000000000000000000..efb03624b9ea7584fa01eb606b44269453120891 --- /dev/null +++ b/modules/transforms/resample_pqfraction/resample_pqfraction.qrc @@ -0,0 +1,5 @@ + + + resample_pqfraction.json + + diff --git a/modules/transforms/resample_pqfraction/upfirdn.h b/modules/transforms/resample_pqfraction/upfirdn.h new file mode 100644 index 0000000000000000000000000000000000000000..85617e732fbb5ce3392dfb0fb59c7001aa36317c --- /dev/null +++ b/modules/transforms/resample_pqfraction/upfirdn.h @@ -0,0 +1,252 @@ +/* +Copyright (c) 2009, Motorola, Inc + +All Rights Reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are +met: + +* Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. + +* Neither the name of Motorola nor the names of its contributors may be +used to endorse or promote products derived from this software without +specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#pragma once + +using namespace std; + +#include +#include +#include + +template +class Resampler{ +public: + typedef S1 inputType; + typedef S2 outputType; + typedef C coefType; + + Resampler(int upRate, int downRate, C *coefs, int coefCount); + virtual ~Resampler(); + + int apply(S1* in, int inCount, S2* out, int outCount); + int neededOutCount(int inCount); + int coefsPerPhase() { return _coefsPerPhase; } + +private: + int _upRate; + int _downRate; + + coefType *_transposedCoefs; + inputType *_state; + inputType *_stateEnd; + + int _paddedCoefCount; // ceil(len(coefs)/upRate)*upRate + int _coefsPerPhase; // _paddedCoefCount / upRate + + int _t; // "time" (modulo upRate) + int _xOffset; + +}; + + +#include +#include + +/* +using std::cout; +using std::endl; + +using std::fill; +using std::copy; +*/ + +using std::invalid_argument; + +template +Resampler::Resampler(int upRate, int downRate, C *coefs, + int coefCount): + _upRate(upRate), _downRate(downRate), _t(0), _xOffset(0) +/* + The coefficients are copied into local storage in a transposed, flipped + arrangement. For example, suppose upRate is 3, and the input number + of coefficients coefCount = 10, represented as h[0], ..., h[9]. + Then the internal buffer will look like this: + h[9], h[6], h[3], h[0], // flipped phase 0 coefs + 0, h[7], h[4], h[1], // flipped phase 1 coefs (zero-padded) + 0, h[8], h[5], h[2], // flipped phase 2 coefs (zero-padded) +*/ +{ + _paddedCoefCount = coefCount; + while (_paddedCoefCount % _upRate) { + _paddedCoefCount++; + } + _coefsPerPhase = _paddedCoefCount / _upRate; + + _transposedCoefs = new coefType[_paddedCoefCount]; + fill(_transposedCoefs, _transposedCoefs + _paddedCoefCount, 0.); + + _state = new inputType[_coefsPerPhase - 1]; + _stateEnd = _state + _coefsPerPhase - 1; + fill(_state, _stateEnd, 0.); + + + /* This both transposes, and "flips" each phase, while + * copying the defined coefficients into local storage. + * There is probably a faster way to do this + */ + for (int i=0; i<_upRate; ++i) { + for (int j=0; j<_coefsPerPhase; ++j) { + if (j*_upRate + i < coefCount) + _transposedCoefs[(_coefsPerPhase-1-j) + i*_coefsPerPhase] = + coefs[j*_upRate + i]; + } + } +} + +template +Resampler::~Resampler() { + delete [] _transposedCoefs; + delete [] _state; +} + +template +int Resampler::neededOutCount(int inCount) +/* compute how many outputs will be generated for inCount inputs */ +{ + int np = inCount * _upRate; + int need = np / _downRate; + if ((_t + _upRate * _xOffset) < (np % _downRate)) + need++; + return need; +} + +template +int Resampler::apply(S1* in, int inCount, + S2* out, int outCount) { + if (outCount < neededOutCount(inCount)) + throw invalid_argument("Not enough output samples"); + + // x points to the latest processed input sample + inputType *x = in + _xOffset; + outputType *y = out; + inputType *end = in + inCount; + while (x < end) { + outputType acc = 0.; + coefType *h = _transposedCoefs + _t*_coefsPerPhase; + inputType *xPtr = x - _coefsPerPhase + 1; + int offset = in - xPtr; + if (offset > 0) { + // need to draw from the _state buffer + inputType *statePtr = _stateEnd - offset; + while (statePtr < _stateEnd) { + acc += *statePtr++ * *h++; + } + xPtr += offset; + } + while (xPtr <= x) { + acc += *xPtr++ * *h++; + } + *y++ = acc; + _t += _downRate; + + int advanceAmount = _t / _upRate; + + x += advanceAmount; + // which phase of the filter to use + _t %= _upRate; + } + _xOffset = x - end; + + // manage _state buffer + // find number of samples retained in buffer: + int retain = (_coefsPerPhase - 1) - inCount; + if (retain > 0) { + // for inCount smaller than state buffer, copy end of buffer + // to beginning: + copy(_stateEnd - retain, _stateEnd, _state); + // Then, copy the entire (short) input to end of buffer + copy(in, end, _stateEnd - inCount); + } else { + // just copy last input samples into state buffer + copy(end - (_coefsPerPhase - 1), end, _state); + } + // number of samples computed + return y - out; +} + +template +void upfirdn(int upRate, int downRate, + S1 *input, int inLength, C *filter, int filterLength, + vector &results) +/* +This template function provides a one-shot resampling. Extra samples +are padded to the end of the input in order to capture all of the non-zero +output samples. +The output is in the "results" vector which is modified by the function. + +Note, I considered returning a vector instead of taking one on input, but +then the C++ compiler has trouble with implicit template instantiation +(e.g. have to say upfirdn every time - this +way we can let the compiler infer the template types). + +Thanks to Lewis Anderson (lkanders@ucsd.edu) at UCSD for +the original version of this function. +*/ +{ + // Create the Resampler + Resampler theResampler(upRate, downRate, filter, filterLength); + + // pad input by length of one polyphase of filter to flush all values out + int padding = theResampler.coefsPerPhase() - 1; + S1 *inputPadded = new S1[inLength + padding]; + for (int i = 0; i < inLength + padding; i++) { + if (i < inLength) + inputPadded[i] = input[i]; + else + inputPadded[i] = 0; + } + + // calc size of output + int resultsCount = theResampler.neededOutCount(inLength + padding); + + results.resize(resultsCount); + + // run filtering + int numSamplesComputed = theResampler.apply(inputPadded, + inLength + padding, &results[0], resultsCount); + delete[] inputPadded; +} + +template +void upfirdn(int upRate, int downRate, + vector &input, vector &filter, vector &results) +/* +This template function provides a one-shot resampling. +The output is in the "results" vector which is modified by the function. +In this version, the input and filter are vectors as opposed to +pointer/count pairs. +*/ +{ + upfirdn(upRate, downRate, &input[0], input.size(), &filter[0], + filter.size(), results); +} diff --git a/modules/uhd/uhd_usrp_continous/uhd_io_continous.cpp b/modules/uhd/uhd_usrp_continous/uhd_io_continous.cpp index aa6cfa9ee36c6e4ce252f3308433281d6adc34b0..62689b6039261204d4e8193f69b24610dad3a649 100644 --- a/modules/uhd/uhd_usrp_continous/uhd_io_continous.cpp +++ b/modules/uhd/uhd_usrp_continous/uhd_io_continous.cpp @@ -56,7 +56,7 @@ int do_iio(const cmdlineParser & args) std::vector rx_bw = args.toDoubleArray("rx_bw"); adjuestUnit (rx_channel_count,rx_bw ,0 ,1e6); //RX Antenna - std::vector rx_atn = args.toStringArray("rx_antenna"); + std::vector rx_atn = args.toStringArray("rx_atn"); adjustString (rx_channel_count,rx_atn,"RX2"); //RX switch bool rx_on = args.toInt("rx_on",0)?true:false; @@ -81,7 +81,7 @@ int do_iio(const cmdlineParser & args) std::vector tx_bw = args.toDoubleArray("tx_bw"); adjuestUnit (tx_channel_count,tx_bw,0 ,1e6); //TX Antenna - std::vector tx_atn = args.toStringArray("tx_antenna"); + std::vector tx_atn = args.toStringArray("tx_atn"); adjustString (rx_channel_count,tx_atn,"TX/RX"); //TX switch bool tx_on = args.toInt("tx_on",0)?true:false; diff --git a/modules/uhd/uhd_usrp_io/uhd_io.cpp b/modules/uhd/uhd_usrp_io/uhd_io.cpp index 203dae19cb712486131fa55bb29791fbb5edbc81..8b435536117687c577c55e2696a0f43a6d5ad404 100644 --- a/modules/uhd/uhd_usrp_io/uhd_io.cpp +++ b/modules/uhd/uhd_usrp_io/uhd_io.cpp @@ -98,7 +98,7 @@ int do_iio(const cmdlineParser & args) std::vector rx_bw = args.toDoubleArray("rx_bw"); adjuestUnit (rx_channel_count,rx_bw ,0 ,1e6); //RX Antenna - std::vector rx_atn = args.toStringArray("rx_antenna"); + std::vector rx_atn = args.toStringArray("rx_atn"); adjustString (rx_channel_count,rx_atn,"RX2"); //RX switch @@ -121,7 +121,7 @@ int do_iio(const cmdlineParser & args) std::vector tx_bw = args.toDoubleArray("tx_bw"); adjuestUnit (tx_channel_count,tx_bw,0 ,1e6); //TX Antenna - std::vector tx_atn = args.toStringArray("tx_antenna"); + std::vector tx_atn = args.toStringArray("tx_atn"); adjustString (rx_channel_count,tx_atn,"TX/RX"); //TX switch bool tx_on = args.toInt("tx_on",0)?true:false;